优化了点击pid计算流程，修复了bsp未初始化的异常，修复了数个未定义和隐式声明的警告

.gitignore

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

.vscode/settings.json

@@ -39,6 +39,7 @@
         "general_def.h": "c",
         "super_cap.h": "c",
         "motor_def.h": "c",
+        "quaternionekf.h": "c"
     },
     "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
 }

HAL_N_Middlewares/Src/freertos.c

@@ -186,7 +186,7 @@ void StartDAEMONTASK(void const * argument)
 {
   while (1)
   {
-    //500Hz
+    //100Hz
     DaemonTask();
     osDelay(10);
   }

HAL_N_Middlewares/Src/main.c

@@ -33,7 +33,6 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "robot.h"
-#include "message_center.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -114,7 +113,6 @@ int main(void)
 
 	RobotInit();
 
-	
 	/* USER CODE END 2 */
 
 	/* Call init function for freertos objects (in freertos.c) */

application/chassis/chassis.c

@@ -59,17 +59,19 @@ void ChassisInit()
 {
     // 四个轮子的参数一样,改tx_id和反转标志位即可
     Motor_Init_Config_s chassis_motor_config = {
-        .can_init_config.can_handle=&hcan1,
+        .can_init_config.can_handle = &hcan1,
         .controller_param_init_config = {
             .speed_PID = {
-                .Kp=10,
+                .Kp = 10,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
             .current_PID = {
-                .Kp=10,
+                .Kp = 10,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
         },
         .controller_setting_init_config = {
@@ -78,26 +80,27 @@ void ChassisInit()
             .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
         },
-        .motor_type = M3508};
+        .motor_type = M3508,
-    
+    };
-    chassis_motor_config.can_init_config.tx_id=1;
+
-    chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.can_init_config.tx_id = 1;
+    chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_lf = DJIMotorInit(&chassis_motor_config);
 
-    chassis_motor_config.can_init_config.tx_id=2,
+    chassis_motor_config.can_init_config.tx_id = 2,
-    chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_rf = DJIMotorInit(&chassis_motor_config);
-    
+
-    chassis_motor_config.can_init_config.tx_id=3,
+    chassis_motor_config.can_init_config.tx_id = 3,
-    chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_lb = DJIMotorInit(&chassis_motor_config);
 
-    chassis_motor_config.can_init_config.tx_id=4,
+    chassis_motor_config.can_init_config.tx_id = 4,
-    chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_rb = DJIMotorInit(&chassis_motor_config);
 
     referee_data = RefereeInit(&huart6);
-    
+
     SuperCap_Init_Config_s cap_conf = {
         .can_config = {
             .can_handle = &hcan2,

application/cmd/robot_cmd.c

@@ -99,8 +99,8 @@ static void RemoteControlSet()
     if (switch_is_down(rc_data[TEMP].rc.s[1]) || vision_recv_data->target_state == NO_TARGET)
     { // 按照摇杆的输出大小进行角度增量,增益系数需调整
         gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.joystick[2];
-        gimbal_cmd_send.pitch += 0.0025f * (float)rc_data[TEMP].rc.joystick[3];
+        gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.joystick[3];
-        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
     }
 
     // 底盘参数,目前没有加入小陀螺(调试似乎没有必要),系数需要调整
@@ -145,7 +145,7 @@ static void EmergencyHandler()
         robot_state = ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式
         gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
         chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
-        shoot_cmd_send.load_mode = SHOOT_STOP;
+        shoot_cmd_send.shoot_mode = SHOOT_OFF;
         return;
     }
     // if(rc_data[TEMP].rc.joystick[4]<-300 && 各个模块正常)

application/gimbal/gimbal.c

@@ -16,24 +16,27 @@ static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
 
 void GimbalInit()
 {
-    Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
+    // Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
 
     // YAW
     Motor_Init_Config_s yaw_config = {
         .can_init_config = {
             .can_handle = &hcan1,
-            .tx_id = 2,
+            .tx_id = 1,
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kd = 1,
+                .Kp = 10,
                 .Ki = 0,
                 .Kd = 0,
+                .MaxOut = 2000,
+                .DeadBand=0.3,
             },
             .speed_PID = {
-                .Kd = 1,
+                .Kp = 10,
                 .Ki = 0,
                 .Kd = 0,
+                .MaxOut = 2000,
             },
             .other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle,
             // 还需要增加角速度额外反馈指针
@@ -44,25 +47,28 @@ void GimbalInit()
             .speed_feedback_source = MOTOR_FEED,
             .outer_loop_type = ANGLE_LOOP,
             .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
-            .reverse_flag = MOTOR_DIRECTION_REVERSE,
+            .reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
         .motor_type = GM6020};
     // PITCH
     Motor_Init_Config_s pitch_config = {
         .can_init_config = {
             .can_handle = &hcan1,
-            .tx_id = 1,
+            .tx_id = 3,
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kd = 10,
+                .Kp = 10,
-                .Ki = 1,
-                .Kd = 2,
-            },
-            .speed_PID = {
-                .Kd = 1,
                 .Ki = 0,
                 .Kd = 0,
+                .MaxOut = 2000,
+                .DeadBand=0.3,
+            },
+            .speed_PID = {
+                .Kp = 10,
+                .Ki = 0,
+                .Kd = 0,
+                .MaxOut = 2000,
             },
             .other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch,
             // 还需要增加角速度额外反馈指针
@@ -73,7 +79,7 @@ void GimbalInit()
             .speed_feedback_source = MOTOR_FEED,
             .outer_loop_type = ANGLE_LOOP,
             .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
-            .reverse_flag = MOTOR_DIRECTION_REVERSE,
+            .reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
         .motor_type = GM6020,
     };

application/robot.c

@@ -1,3 +1,4 @@
+#include "bsp_init.h"
 #include "robot.h"
 #include "robot_def.h"
 
@@ -12,6 +13,7 @@
 
 void RobotInit()
 {
+    BSPInit();
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
     ChassisInit();
 #endif

application/robot_def.h

@@ -88,8 +88,14 @@ typedef enum
     GIMBAL_GYRO_MODE,  // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
 } gimbal_mode_e;
 
+
 // 发射模式设置
 typedef enum
+{
+    SHOOT_ON,
+    SHOOT_OFF,
+}shoot_mode_e;
+typedef enum
 {
     FRICTION_OFF, // 摩擦轮关闭
     FRICTION_ON,  // 摩擦轮开启
@@ -103,7 +109,6 @@ typedef enum
 
 typedef enum
 {
-    SHOOT_STOP,     // 停止整个发射模块,后续可能隔离出来
     LOAD_STOP,      // 停止发射
     LOAD_REVERSE,   // 反转
     LOAD_1_BULLET,  // 单发
@@ -150,6 +155,7 @@ typedef struct
 // cmd发布的发射控制数据,由shoot订阅
 typedef struct
 {
+    shoot_mode_e shoot_mode;
     loader_mode_e load_mode;
     lid_mode_e lid_mode;
     friction_mode_e friction_mode;

application/shoot/shoot.c

@@ -29,14 +29,16 @@ void ShootInit()
         },
         .controller_param_init_config = {
             .speed_PID = {
-                .Kp=10,
+                .Kp = 10,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
             .current_PID = {
-                .Kp=10,
+                .Kp = 10,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
         },
         .controller_setting_init_config = {
@@ -56,14 +58,16 @@ void ShootInit()
         },
         .controller_param_init_config = {
             .speed_PID = {
-                .Kp=1,
+                .Kp = 1,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
             .current_PID = {
-                .Kp=1,
+                .Kp = 1,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
         },
         .controller_setting_init_config = {
@@ -83,19 +87,22 @@ void ShootInit()
         .controller_param_init_config = {
             .angle_PID = {
                 // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
-                .Kd = 10,
+                .Kp = 10,
                 .Ki = 1,
                 .Kd = 2,
+                .MaxOut = 200,
             },
             .speed_PID = {
-                .Kp=1,
+                .Kp = 1,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
             .current_PID = {
-                .Kp=1,
+                .Kp = 1,
-                .Ki=0,
+                .Ki = 0,
-                .Kd=0,
+                .Kd = 0,
+                .MaxOut = 200,
             },
         },
         .controller_setting_init_config = {
@@ -121,7 +128,7 @@ void ShootTask()
     SubGetMessage(shoot_sub, &shoot_cmd_recv);
 
     // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机
-    if (shoot_cmd_recv.load_mode == SHOOT_STOP)
+    if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
     {
         DJIMotorStop(friction_l);
         DJIMotorStop(friction_r);
@@ -175,15 +182,15 @@ void ShootTask()
     {
     case SMALL_AMU_15:
         DJIMotorSetRef(friction_l, 0);
-        DJIMotorSetRef(friction_l, 0);
+        DJIMotorSetRef(friction_r, 0);
         break;
     case SMALL_AMU_18:
         DJIMotorSetRef(friction_l, 0);
-        DJIMotorSetRef(friction_l, 0);
+        DJIMotorSetRef(friction_r, 0);
         break;
     case SMALL_AMU_30:
         DJIMotorSetRef(friction_l, 0);
-        DJIMotorSetRef(friction_l, 0);
+        DJIMotorSetRef(friction_r, 0);
         break;
     default:
         break;

modules/algorithm/controller.c

@@ -29,10 +29,11 @@ static void f_PID_ErrorHandle(PIDInstance *pid);           // 堵转保护
  */
 void PID_Init(PIDInstance *pid, PID_Init_config_s *config)
 {
+    memset(pid, 0, sizeof(PIDInstance));
     // utilize the quality of struct that its memeory is continuous
     memcpy(pid, config, sizeof(PID_Init_config_s));
     // set rest of memory to 0
-    memset(&pid->Measure, 0, sizeof(PIDInstance) - sizeof(PID_Init_config_s));
+    
 }
 
 /**

modules/daemon/daemon.c

@@ -1,5 +1,6 @@
 #include "daemon.h"
 #include "bsp_dwt.h" // 后续通过定时器来计时?
+#include "stdlib.h"
 
 static DaemonInstance *daemon_instances[DAEMON_MX_CNT] = {NULL};
 static uint8_t idx;

modules/general_def.h

@@ -1,7 +1,9 @@
 #ifndef GENERAL_DEF_H
 #define GENERAL_DEF_H
 
+#ifndef PI
 #define PI 3.1415926535f
+#endif // !PI
 #define PI2 (PI * 2.0f) // 2 pi
 
 #define RAD_2_ANGLE (180.0f / PI)

modules/imu/ins_task.c

@@ -68,6 +68,7 @@ attitude_t *INS_Init(void)
 
     // noise of accel is relatively big and of high freq,thus lpf is used
     INS.AccelLPF = 0.0085;
+    return (attitude_t*)&INS.Roll;
 }
 
 /* 注意以1kHz的频率运行此任务 */

modules/motor/dji_motor.c

@@ -223,7 +223,7 @@ void DJIMotorControl()
     static Motor_Control_Setting_s *motor_setting;
     static Motor_Controller_s *motor_controller;
     static DJI_Motor_Measure_s *motor_measure;
-    static float pid_measure;
+    static float pid_measure,pid_ref;
     // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
     for (size_t i = 0; i < idx; i++)
     {
@@ -233,6 +233,7 @@ void DJIMotorControl()
             motor_setting = &motor->motor_settings;
             motor_controller = &motor->motor_controller;
             motor_measure = &motor->motor_measure;
+            pid_ref=motor_controller->pid_ref; //保存设定值,防止motor_controller->pid_ref在计算过程中被修改
 
             // pid_ref会顺次通过被启用的闭环充当数据的载体
             // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
@@ -243,7 +244,7 @@ void DJIMotorControl()
                 else                                          // MOTOR_FEED
                     pid_measure = motor_measure->total_angle; // 对total angle闭环,防止在边界处出现突跃
                 // 更新pid_ref进入下一个环
-                motor_controller->pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, motor_controller->pid_ref);
+                pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, pid_ref);
             }
 
             // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
@@ -254,17 +255,17 @@ void DJIMotorControl()
                 else // MOTOR_FEED
                     pid_measure = motor_measure->speed_angle_per_sec;
                 // 更新pid_ref进入下一个环
-                motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref);
+                pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, pid_ref);
             }
 
             // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
             if (motor_setting->close_loop_type & CURRENT_LOOP)
             {
-                motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref);
+                pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, pid_ref);
             }
 
             // 获取最终输出
-            set = (int16_t)motor_controller->pid_ref;
+            set = (int16_t)pid_ref;
             if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE)
                 set *= -1; // 设置反转
 

modules/remote/remote_control.c

@@ -19,11 +19,11 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
 {
     memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据
     // 摇杆,直接解算时减去偏置
-    rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET;                              //!< Channel 0
+    rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff)- RC_CH_VALUE_OFFSET ;                              //!< Channel 0
-    rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET;                       //!< Channel 1
+    rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff)- RC_CH_VALUE_OFFSET ;                       //!< Channel 1
-    rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
+    rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 2
-    rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET;                       //!< Channel 3
+    rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff)- RC_CH_VALUE_OFFSET ;                       //!< Channel 3
-    rc_ctrl[TEMP].rc.joystick[4] = (sbus_buf[16] | (sbus_buf[17] << 8)) - RC_CH_VALUE_OFFSET;                                       // 左侧拨轮
+    rc_ctrl[TEMP].rc.joystick[4] = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)- RC_CH_VALUE_OFFSET;                                       // 左侧拨轮
     // 开关,0左1右
     rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003);      //!< Switch left
     rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
@@ -81,5 +81,5 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
     conf.usart_handle = rc_usart_handle;
     conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
     rc_usart_instance = USARTRegister(&conf);
-    return &rc_ctrl;
+    return (RC_ctrl_t*)&rc_ctrl;
 }