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

.gitignore

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

HAL_N_Middlewares/Src/freertos.c

@@ -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 */

application/chassis/chassis.c

@@ -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 = {

application/gimbal/gimbal.c

@@ -26,12 +26,14 @@ void GimbalInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kd = 10,
+                .Kd = 1,
-                .Ki = 1,
+                .Ki = 0,
-                .Kd = 2,
+                .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);

application/shoot/shoot.c

@@ -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 = {

bsp/bsp_can.c

@@ -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说明已经遍历完所有实例
         {                        // 两者相等说明这是要找的实例

modules/imu/ins_task.c

@@ -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);
+        BMI088_Read(&BMI088);
-        t += dt;
 
-        // ins update
+        INS.Accel[X] = BMI088.Accel[X];
-        if ((count % 1) == 0)
+        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.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
-
-            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;
         }
+        BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
 
-        // temperature control
+        // 获取最终数据
-        if ((count % 2) == 0)
+        INS.Yaw = QEKF_INS.Yaw;
-        {
+        INS.Pitch = QEKF_INS.Pitch;
-            // 500hz
+        INS.Roll = QEKF_INS.Roll;
-            IMU_Temperature_Ctrl();
+        INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
-        }
+    }
-
+
-        if ((count++ % 1000) == 0)
+    // temperature control
-        {
+    if ((count % 2) == 0)
-            // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
+    {
-        }
+        // 500hz
+        IMU_Temperature_Ctrl();
+    }
+
+    if ((count++ % 1000) == 0)
+    {
+        // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
     }
-    osDelay(1);
 }
 
 /**

modules/motor/dji_motor.c

@@ -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)
+            // if (motor->stop_flag == MOTOR_STOP)
-            { // 若该电机处于停止状态,直接将buff置零
+            // { // 若该电机处于停止状态,直接将buff置零
-                memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
+            //     memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
-            }
+            // }
         }
-        else // 遇到空指针说明所有遍历结束,退出循环
-            break;
     }
 
     // 遍历flag,检查是否要发送这一帧报文