SkyLiCoder
7260fe9fba
根因：imu_init(200) 声明200Hz，但imu_update实际在50Hz任务中调用， Mahony积分dt差4倍，导致转90度只测出22度。 修复：imu_update移至10ms任务(100Hz)，传感器采集与控制频率对齐。 同时将任务周期全部参数化： - Task.h 统一定义 TASK_10MS/20MS/50MS_FREQ_HZ - 各子系统init改为传入频率(Hz)，内部算周期(dt) - PIT定时器周期由存储的dt自动计算，消除硬编码
306 lines
9.5 KiB
C
306 lines
9.5 KiB
C
#include "Command.h"
|
|
#include "zf_driver_uart.h"
|
|
#include "zf_common_interrupt.h"
|
|
#include "ti/driverlib/dl_uart_main.h"
|
|
#include "ti/driverlib/dl_dma.h"
|
|
#include "zf_common_fifo.h"
|
|
#include "Beep.h"
|
|
#include "Task.h"
|
|
#include "Motor.h"
|
|
#include "PID_Controller.h"
|
|
#include "IMU.h"
|
|
|
|
volatile fifo_struct command_fifo = {0};
|
|
volatile uint8_t command_fifo_buffer[COMMAND_FIFO_BUFFER_SIZE] = {0};
|
|
volatile Vision_Mode_e current_mode = COMMAND_MODE_DEBUG;
|
|
uint8_t command_sum(uint8_t* data,int len);
|
|
|
|
static void command_process_manual(uint8_t *data);
|
|
static void command_process_debug(uint8_t *data);
|
|
|
|
void command_callback(uint32 state, void *ptr)
|
|
{
|
|
while(false == DL_UART_isRXFIFOEmpty(UART2))//读取到一个数据
|
|
{
|
|
fifo_write_element((fifo_struct*)&command_fifo,DL_UART_Main_receiveData(UART2));//写入软件FIFO
|
|
}
|
|
}
|
|
void command_init()
|
|
{
|
|
uart_init(UART_2,115200,UART2_TX_B15,UART2_RX_B16);
|
|
uart_set_interrupt_config(UART_2,UART_INTERRUPT_CONFIG_RX_ENABLE);
|
|
interrupt_set_priority(UART2_INT_IRQn,2);//设置为次于主要任务的优先级
|
|
|
|
//开启发送DMA
|
|
static const DL_DMA_Config dma_tx_config =
|
|
{
|
|
.trigger = DMA_UART2_TX_TRIG,
|
|
.triggerType = DL_DMA_TRIGGER_TYPE_EXTERNAL,
|
|
.transferMode = DL_DMA_SINGLE_TRANSFER_MODE,
|
|
.extendedMode = DL_DMA_NORMAL_MODE,
|
|
.srcWidth = DL_DMA_WIDTH_BYTE,
|
|
.destWidth = DL_DMA_WIDTH_BYTE,
|
|
.srcIncrement = DL_DMA_ADDR_INCREMENT,
|
|
.destIncrement = DL_DMA_ADDR_UNCHANGED,
|
|
};
|
|
DL_DMA_initChannel(DMA, 0, &dma_tx_config);
|
|
DL_UART_Main_enableDMATransmitEvent(UART2);
|
|
|
|
//开启硬件FIFO
|
|
DL_UART_Main_enableFIFOs(UART2);
|
|
DL_UART_Main_setRXFIFOThreshold(UART2,DL_UART_MAIN_RX_FIFO_LEVEL_ONE_ENTRY);
|
|
DL_UART_Main_setTXFIFOThreshold(UART2,DL_UART_MAIN_TX_FIFO_LEVEL_ONE_ENTRY);
|
|
|
|
//设置软件FIFO
|
|
fifo_init((fifo_struct *)&command_fifo,FIFO_DATA_8BIT,(void*)command_fifo_buffer,COMMAND_FIFO_BUFFER_SIZE);
|
|
//设置中断回调
|
|
uart_set_callback(UART_2,command_callback,NULL);
|
|
}
|
|
//从软件FIFO中解析数据包并执行
|
|
void command_task()
|
|
{
|
|
static uint8_t state = 0;
|
|
static uint8_t sof = 0;
|
|
static uint8_t payload_len = 0;
|
|
static uint8_t buf[37]; // 最大payload: 0xA8的37字节(9*4 float + 1 checksum 先读到buf)
|
|
static uint8_t idx = 0;
|
|
uint8_t byte;
|
|
|
|
while(fifo_used((fifo_struct*)&command_fifo) > 0)
|
|
{
|
|
switch(state)
|
|
{
|
|
case 0: // 等待SOF
|
|
fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN);
|
|
if(byte == COMMAND_SOF_MODE)
|
|
{
|
|
sof = COMMAND_SOF_MODE;
|
|
payload_len = 2; // cmd(1) + checksum(1)
|
|
idx = 0;
|
|
state = 1;
|
|
}
|
|
else if(byte == COMMAND_SOF_MANUAL)
|
|
{
|
|
sof = COMMAND_SOF_MANUAL;
|
|
payload_len = 7; // motor_speed_base(2) + angle_velocity(4) + checksum(1)
|
|
idx = 0;
|
|
state = 1;
|
|
}
|
|
else if(byte == COMMAND_SOF_DEBUG)
|
|
{
|
|
sof = COMMAND_SOF_DEBUG;
|
|
payload_len = 37; // 9*4 float + checksum(1)
|
|
idx = 0;
|
|
state = 1;
|
|
}
|
|
break;
|
|
|
|
case 1: // 读取payload
|
|
fifo_read_element((fifo_struct*)&command_fifo, &buf[idx], FIFO_READ_AND_CLEAN);
|
|
idx++;
|
|
if(idx >= payload_len)
|
|
{
|
|
// 校验checksum (payload最后一个字节)
|
|
uint8_t sum = sof;
|
|
for(uint8_t i = 0; i < payload_len - 1; i++)
|
|
sum += buf[i];
|
|
if(buf[payload_len - 1] == sum)
|
|
{
|
|
if(sof == COMMAND_SOF_MODE)
|
|
{
|
|
// 命令帧: buf[0]=cmd
|
|
switch(buf[0])
|
|
{
|
|
case 0x01: SwitchToAutoMode(); break;
|
|
case 0x02: SwitchToManualMode(); break;
|
|
case 0x03: SwitchToDebugMode(); break;
|
|
}
|
|
}
|
|
else if(sof == COMMAND_SOF_MANUAL)
|
|
{
|
|
command_process_manual(buf);
|
|
}
|
|
else if(sof == COMMAND_SOF_DEBUG)
|
|
{
|
|
command_process_debug(buf);
|
|
}
|
|
}
|
|
state = 0;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
//切换到自动模式
|
|
void SwitchToAutoMode()
|
|
{
|
|
current_mode = COMMAND_MODE_AUTO;
|
|
|
|
/* 清理PID状态 */
|
|
PIDClear((PIDInstance *)&pid_speed_right);
|
|
PIDClear((PIDInstance *)&pid_speed_left);
|
|
PIDClear((PIDInstance *)&pid_yaw);
|
|
PIDClear((PIDInstance *)&pid_position);
|
|
|
|
/* 初始化输入输出值 */
|
|
//角度参考值设为当前值
|
|
yaw_ref = imu_angle.yaw;
|
|
//角度速率值归零
|
|
angle_velocity = 0;
|
|
//速度参考值归零
|
|
motor_speed_right_ref = 0;
|
|
motor_speed_left_ref = 0;
|
|
//电机PWM归零
|
|
motor_set_pwm_right(0);
|
|
motor_set_pwm_left(0);
|
|
|
|
//设定基础直行速度
|
|
motor_speed_base=AUTO_MODE_BASE_SPEED;
|
|
|
|
//清理子任务标志
|
|
position_turn_right_90_runing=0;
|
|
|
|
/* 开启控制任务 */
|
|
motor_control_task_enable=1;
|
|
angle_control_task_enable=1;
|
|
position_control_task_enable=1;
|
|
beep_on_time(100);
|
|
}
|
|
|
|
//切换到手动模式
|
|
void SwitchToManualMode()
|
|
{
|
|
current_mode = COMMAND_MODE_MANUAL;
|
|
|
|
/* 立即关闭位置环 */
|
|
position_control_task_enable=0;
|
|
|
|
/* 清理PID状态 */
|
|
PIDClear((PIDInstance *)&pid_speed_right);
|
|
PIDClear((PIDInstance *)&pid_speed_left);
|
|
PIDClear((PIDInstance *)&pid_yaw);
|
|
|
|
/* 初始化输入输出值 */
|
|
//角度参考值设为当前值
|
|
//yaw_ref = imu_angle.yaw;//暂时关掉，方便在调试模式下更改姿态，然后切回手动模式观察响应
|
|
//角度速率归零
|
|
angle_velocity = 0;
|
|
//速度参考值归零
|
|
motor_speed_right_ref = 0;
|
|
motor_speed_left_ref = 0;
|
|
//电机PWM归零
|
|
motor_set_pwm_right(0);
|
|
motor_set_pwm_left(0);
|
|
|
|
//基础直行速度归零
|
|
motor_speed_base=0;
|
|
|
|
/* 开启控制任务 */
|
|
motor_control_task_enable=1;
|
|
angle_control_task_enable=1;
|
|
|
|
beep_on_time(200);
|
|
}
|
|
|
|
//切换到调试模式
|
|
void SwitchToDebugMode()
|
|
{
|
|
current_mode = COMMAND_MODE_DEBUG;
|
|
|
|
/* 立即关闭所有控制任务 */
|
|
motor_control_task_enable=0;
|
|
angle_control_task_enable=0;
|
|
position_control_task_enable=0;
|
|
|
|
/* 电机PWM设为0 */
|
|
motor_set_pwm_right(0);
|
|
motor_set_pwm_left(0);
|
|
|
|
beep_on_time(300);
|
|
}
|
|
|
|
//解析手动控制帧 0xA6: [motor_speed_base:2 int16_le][angle_velocity:4 float_le]
|
|
static void command_process_manual(uint8_t *data)
|
|
{
|
|
if(current_mode != COMMAND_MODE_MANUAL)
|
|
return;
|
|
|
|
motor_speed_base = (int16_t)(data[0] | (data[1] << 8));
|
|
union { float f; uint8_t b[4]; } u;
|
|
u.b[0] = data[2];
|
|
u.b[1] = data[3];
|
|
u.b[2] = data[4];
|
|
u.b[3] = data[5];
|
|
angle_velocity = u.f;
|
|
command_send_angle_velocity((float *)&angle_velocity);
|
|
}
|
|
|
|
//解析调试调参帧 0xA8: 9个float (KpKiKd_speed + KpKiKd_yaw + KpKiKd_position)
|
|
static void command_process_debug(uint8_t *data)
|
|
{
|
|
if(current_mode != COMMAND_MODE_DEBUG)
|
|
return;
|
|
|
|
union { float f; uint8_t b[4]; } u[9];
|
|
for(int i = 0; i < 9; i++)
|
|
{
|
|
u[i].b[0] = data[i*4 + 0];
|
|
u[i].b[1] = data[i*4 + 1];
|
|
u[i].b[2] = data[i*4 + 2];
|
|
u[i].b[3] = data[i*4 + 3];
|
|
}
|
|
|
|
// 电机速度环 KpKiKd (左右共用)
|
|
pid_speed_right.Kp = u[0].f;
|
|
pid_speed_right.Ki = u[1].f;
|
|
pid_speed_right.Kd = u[2].f;
|
|
pid_speed_left.Kp = u[0].f;
|
|
pid_speed_left.Ki = u[1].f;
|
|
pid_speed_left.Kd = u[2].f;
|
|
|
|
// 角度环 KpKiKd
|
|
pid_yaw.Kp = u[3].f;
|
|
pid_yaw.Ki = u[4].f;
|
|
pid_yaw.Kd = u[5].f;
|
|
|
|
// 位置环 KpKiKd
|
|
pid_position.Kp = u[6].f;
|
|
pid_position.Ki = u[7].f;
|
|
pid_position.Kd = u[8].f;
|
|
}
|
|
//DMA发送使用的地址
|
|
uint8_t angle_velocity_data[6] = {0};
|
|
//发送速度前馈
|
|
void command_send_angle_velocity(float* pointer_angle_velocity)
|
|
{
|
|
//等待上一次DMA发送完成
|
|
while(DL_DMA_isChannelEnabled(DMA, 0));
|
|
//等待UART物理发送完成
|
|
while(!DL_UART_isTXFIFOEmpty(UART2));
|
|
|
|
//填充数据
|
|
uint8_t* p = (uint8_t*)pointer_angle_velocity;
|
|
angle_velocity_data[0]=0xA9;
|
|
angle_velocity_data[1]=*(p++);
|
|
angle_velocity_data[2]=*(p++);
|
|
angle_velocity_data[3]=*(p++);
|
|
angle_velocity_data[4]=*(p++);
|
|
angle_velocity_data[5]=command_sum(angle_velocity_data,5);
|
|
//DMA发送（非阻塞）
|
|
DL_DMA_setSrcAddr(DMA, 0, (uint32_t)angle_velocity_data);
|
|
DL_DMA_setDestAddr(DMA, 0, (uint32_t)&UART2->TXDATA);
|
|
DL_DMA_setTransferSize(DMA, 0, 6);
|
|
DL_DMA_enableChannel(DMA, 0);
|
|
// UART TX FIFO为空，DMA触发条件已满足，使能通道即自动开始传输
|
|
|
|
}
|
|
uint8_t command_sum(uint8_t* data,int len)
|
|
{
|
|
uint8_t sum = 0;
|
|
for(int i=0;i<len;i++)
|
|
{
|
|
sum+=data[i];
|
|
}
|
|
return sum;
|
|
} |