修复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,8 +75,7 @@ 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;
 
@@ -137,8 +136,6 @@ void INS_Task(void)
         // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
     }
 }
-    osDelay(1);
-}
 
 /**
  * @brief          Transform 3dvector from BodyFrame to EarthFrame

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,14 +275,12 @@ 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,检查是否要发送这一帧报文
     for (size_t i = 0; i < 6; i++)