HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复INS_Task双重循环NAN的问题，暂时关闭了急停模式

This commit is contained in:
NeoZng 2022-12-08 23:08:28 +08:00
parent fe85ae5a6e
commit 9f09002235
8 changed files with 101 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -51,4 +51,4 @@ build
./idea
.vscode/.cortex-debug.peripherals.state.json
.vscode/.cortex-debug.registers.state.json
.jdebug
*.jdebug

2
HAL_N_Middlewares/Src/freertos.c
View File

@ -128,7 +128,7 @@ void MX_FREERTOS_Init(void) {
osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 512);
defaultTaskHandle = osThreadCreate(osThread(daemontask), NULL);
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 2048);
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024);
defaultTaskHandle = osThreadCreate(osThread(robottask), NULL);
/* USER CODE BEGIN RTOS_THREADS */

8
application/chassis/chassis.c
View File

@ -62,10 +62,14 @@ void ChassisInit()
.can_init_config.can_handle=&hcan2,
.controller_param_init_config = {
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {

18
application/gimbal/gimbal.c
View File

@ -26,12 +26,14 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 10,
.Ki = 1,
.Kd = 2,
.Kd = 1,
.Ki = 0,
.Kd = 0,
},
.speed_PID = {
.Kd = 1,
.Ki = 0,
.Kd = 0,
},
.other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针
@ -58,7 +60,9 @@ void GimbalInit()
.Kd = 2,
},
.speed_PID = {
.Kd = 1,
.Ki = 0,
.Kd = 0,
},
.other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针
@ -71,8 +75,8 @@ void GimbalInit()
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.reverse_flag = MOTOR_DIRECTION_REVERSE,
},
.motor_type = GM6020};
.motor_type = GM6020,
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
pitch_motor = DJIMotorInit(&pitch_config);

38
application/shoot/shoot.c
View File

@ -28,16 +28,15 @@ void ShootInit()
.tx_id = 1,
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 10,
.Ki = 1,
.Kd = 2,
},
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {
@ -56,16 +55,15 @@ void ShootInit()
.tx_id = 2,
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 10,
.Ki = 1,
.Kd = 2,
},
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {
@ -90,13 +88,19 @@ void ShootInit()
.Kd = 2,
},
.angle_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {

2
bsp/bsp_can.c
View File

@ -115,7 +115,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
uint8_t can_rx_buff[8];
CAN_RxHeaderTypeDef rxconf;
HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff);
for (size_t i = 0; i < DEVICE_CAN_CNT; i++)
for (size_t i = 0; i < MX_REGISTER_DEVICE_CNT; i++)
{
if (instance[i] != NULL) // 碰到NULL说明已经遍历完所有实例
{ // 两者相等说明这是要找的实例

111
modules/imu/ins_task.c
View File

@ -75,69 +75,66 @@ void INS_Task(void)
{
static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f};
while (1)
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
// ins update
if ((count % 1) == 0)
{
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
BMI088_Read(&BMI088);
// ins update
if ((count % 1) == 0)
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
// 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
// 核心函数,EKF更新四元数
IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
// 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
BodyFrameToEarthFrame(xb, INS.xn, INS.q);
BodyFrameToEarthFrame(yb, INS.yn, INS.q);
BodyFrameToEarthFrame(zb, INS.zn, INS.q);
// 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
float gravity_b[3];
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
{
BMI088_Read(&BMI088);
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
// 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
// 核心函数,EKF更新四元数
IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
// 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
BodyFrameToEarthFrame(xb, INS.xn, INS.q);
BodyFrameToEarthFrame(yb, INS.yn, INS.q);
BodyFrameToEarthFrame(zb, INS.zn, INS.q);
// 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
float gravity_b[3];
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
{
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
// 获取最终数据
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
// temperature control
if ((count % 2) == 0)
{
// 500hz
IMU_Temperature_Ctrl();
}
if ((count++ % 1000) == 0)
{
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
// 获取最终数据
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
}
// temperature control
if ((count % 2) == 0)
{
// 500hz
IMU_Temperature_Ctrl();
}
if ((count++ % 1000) == 0)
{
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
osDelay(1);
}
/**

14
modules/motor/dji_motor.c
View File

@ -124,7 +124,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
static uint8_t *rxbuff;
static DJI_Motor_Measure_s *measure;
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance == _instance)
{
@ -225,7 +225,7 @@ void DJIMotorControl()
static DJI_Motor_Measure_s *motor_measure;
static float pid_measure;
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i])
{
@ -275,13 +275,11 @@ void DJIMotorControl()
sender_assignment[group].tx_buff[2 * num + 1] = 0xff & set;
// 电机是否停止运行
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将buff置零
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
}
// if (motor->stop_flag == MOTOR_STOP)
// { // 若该电机处于停止状态,直接将buff置零
// memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
// }
}
else // 遇到空指针说明所有遍历结束,退出循环
break;
}
// 遍历flag,检查是否要发送这一帧报文