重新组织了文件的防止

modules/BMI088/bmi088.h

@@ -1,17 +1,48 @@
+#ifndef __BMI088_H__
+#define __BMI088_H__
+
 #include "bsp_spi.h"
+#include "controller.h"
+#include "bsp_pwm.h"
+#include "stdint.h"
+
+typedef enum
+{
+    BMI088_CALIBRATE_MODE = 0, // 初始化时进行标定
+    BMI088_LOAD_PRE_CALI_MODE, // 使用预设标定参数
+} BMI088_Work_Mode_e;
 
 /* BMI088实例结构体定义 */
 typedef struct
 {
-    SPIInstance *spi_gyro; 
+    // 传输模式和工作模式控制
+    SPIInstance *spi_gyro;
     SPIInstance *spi_acc;
-
-
+    // 温度控制
+    PIDInstance *heat_pid; // 恒温PID
+    PWMInstance *heat_pwm; // 加热PWM
+    // IMU数据
+    float gyro[3];     // 陀螺仪数据,xyz
+    float acc[3];      // 加速度计数据,xyz
+    float temperature; // 温度
+    // 标定数据
+    float gyro_offset[3]; // 陀螺仪零偏
+    float gNorm;          // 重力加速度模长,从标定获取
+    float acc_coef;       // 加速度计原始数据转换系数
+    // 用于计算两次采样的时间间隔
+    uint32_t bias_dwt_cnt;
 } BMI088Instance;
 
 /* BMI088初始化配置 */
 typedef struct
 {
+    SPI_Init_Config_s spi_gyro_config;
+    SPI_Init_Config_s spi_acc_config;
+    BMI088_Work_Mode_e mode;
+    PID_Init_Config_s heat_pid_config;
+    PWM_Init_Config_s heat_pwm_config;
+} BMI088_Init_Config_s;
 
-}BMI088_Init_Config_s;
 
+
+#endif // !__BMI088_H__

modules/algorithm/controller.c

@@ -126,14 +126,14 @@ static void f_PID_ErrorHandle(PIDInstance *pid)
  * @param pid    PID实例
  * @param config PID初始化设置
  */
-void PID_Init(PIDInstance *pid, PID_Init_config_s *config)
+void PID_Init(PIDInstance *pid, PID_Init_Config_s *config)
 {   
     // config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
     // @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
     // 后续修改为逐个赋值
     memset(pid, 0, sizeof(PIDInstance));
     // utilize the quality of struct that its memeory is continuous
-    memcpy(pid, config, sizeof(PID_Init_config_s));
+    memcpy(pid, config, sizeof(PID_Init_Config_s));
     // set rest of memory to 0
     
 }

modules/algorithm/controller.h

@@ -28,7 +28,7 @@
 // PID 优化环节使能标志位
 typedef enum
 {
-    PID_IMPROVE_NONE = 0b00000000,                        // 0000 0000
+    PID_IMPROVE_NONE = 0b00000000,            // 0000 0000
     Integral_Limit = 0b00000001,              // 0000 0001
     Derivative_On_Measurement = 0b00000010,   // 0000 0010
     Trapezoid_Intergral = 0b00000100,         // 0000 0100
@@ -112,7 +112,7 @@ typedef struct
     float Derivative_LPF_RC;
 
     PID_Improvement_e Improve;
-} PID_Init_config_s;
+} PID_Init_Config_s;
 
 /**
  * @brief 初始化PID实例
@@ -120,7 +120,7 @@ typedef struct
  * @param pid    PID实例指针
  * @param config PID初始化配置
  */
-void PID_Init(PIDInstance *pid, PID_Init_config_s *config);
+void PID_Init(PIDInstance *pid, PID_Init_Config_s *config);
 
 /**
  * @brief 计算PID输出

modules/general_def.h

@@ -3,13 +3,12 @@
 
 #ifndef PI
 #define PI 3.1415926535f
-#endif // !PI
+#endif                  // !PI
 #define PI2 (PI * 2.0f) // 2 pi
 
-#define RAD_2_ANGLE (180.0f / PI)
-#define ANGLE_2_RAD (PI / 180.0f)
-
-#define RPM_2_ANGLE_PER_SEC (360.0f/60.0f) // ×360°/60sec
+#define RAD_2_ANGLE 57.2957795f    // 180/pi
+#define ANGLE_2_RAD 0.01745329252f // pi/180
 
+#define RPM_2_ANGLE_PER_SEC 6.0f // ×360°/60sec
 
 #endif // !GENERAL_DEF_H

modules/imu/ins_task.c

@@ -57,7 +57,7 @@ attitude_t *INS_Init(void)
 
     IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
     // imu heat init
-    PID_Init_config_s config = {.MaxOut = 2000,
+    PID_Init_Config_s config = {.MaxOut = 2000,
                                 .IntegralLimit = 300,
                                 .DeadBand = 0,
                                 .Kp = 1000,
@@ -68,7 +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;
+    return (attitude_t*)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
 }
 
 /* 注意以1kHz的频率运行此任务 */
@@ -96,8 +96,8 @@ void INS_Task(void)
         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;
+        // 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);

modules/imu/ins_task.h

@@ -9,7 +9,7 @@
  ******************************************************************************
  * @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在
  *            应用层调用初始化函数.
- * 
+ *
  ******************************************************************************
  */
 #ifndef __INS_TASK_H
@@ -27,32 +27,36 @@
 
 typedef struct
 {
+    float Gyro[3];  // 角速度
+    float Accel[3]; // 加速度
     // 还需要增加角速度数据
     float Roll;
     float Pitch;
     float Yaw;
     float YawTotalAngle;
-} attitude_t; //最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
+} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
 
 typedef struct
 {
     float q[4]; // 四元数估计值
 
-    float Gyro[3];          // 角速度
-    float Accel[3];         // 加速度
     float MotionAccel_b[3]; // 机体坐标加速度
     float MotionAccel_n[3]; // 绝对系加速度
 
     float AccelLPF; // 加速度低通滤波系数
 
-    // 加速度在绝对系的向量表示
+    // bodyframe在绝对系的向量表示
     float xn[3];
     float yn[3];
     float zn[3];
 
-    float atanxz;
-    float atanyz;
+    // 加速度在机体系和XY两轴的夹角
+    // float atanxz;
+    // float atanyz;
 
+    // IMU量测值
+    float Gyro[3];  // 角速度
+    float Accel[3]; // 加速度
     // 位姿
     float Roll;
     float Pitch;

modules/motor/DJImotor/dji_motor.c

@@ -38,7 +38,8 @@ static uint8_t sender_enable_flag[6] = {0};
  */
 static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
 {
-    while (1);
+    while (1)
+        ;
 }
 
 /**
@@ -46,13 +47,13 @@ static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
  *
  * @param config
  */
-static void MotorSenderGrouping(CAN_Init_Config_s *config)
+static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
 {
     uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
     uint8_t motor_send_num;
     uint8_t motor_grouping;
 
-    switch (dji_motor_instance[idx]->motor_type)
+    switch (motor->motor_type)
     {
     case M2006:
     case M3508:
@@ -70,8 +71,8 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
         // 计算接收id并设置分组发送id
         config->rx_id = 0x200 + motor_id + 1;
         sender_enable_flag[motor_grouping] = 1;
-        dji_motor_instance[idx]->message_num = motor_send_num;
-        dji_motor_instance[idx]->sender_group = motor_grouping;
+        motor->message_num = motor_send_num;
+        motor->sender_group = motor_grouping;
 
         // 检查是否发生id冲突
         for (size_t i = 0; i < idx; ++i)
@@ -95,8 +96,8 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
 
         config->rx_id = 0x204 + motor_id + 1;
         sender_enable_flag[motor_grouping] = 1;
-        dji_motor_instance[idx]->message_num = motor_send_num;
-        dji_motor_instance[idx]->sender_group = motor_grouping;
+        motor->message_num = motor_send_num;
+        motor->sender_group = motor_grouping;
 
         for (size_t i = 0; i < idx; ++i)
         {
@@ -106,7 +107,8 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
         break;
 
     default: // other motors should not be registered here
-        while(1); // 其他电机不应该在这里注册
+        while (1)
+            ; // 其他电机不应该在这里注册
     }
 }
 
@@ -161,11 +163,11 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
     instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
 
     // 电机分组,因为至多4个电机可以共用一帧CAN控制报文
-    MotorSenderGrouping(&config->can_init_config);
+    MotorSenderGrouping(instance, &config->can_init_config);
 
     // 注册电机到CAN总线
     config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
-    config->can_init_config.id = instance;// set id,eq to address(it is identity)
+    config->can_init_config.id = instance;                        // set id,eq to address(it is identity)
     instance->motor_can_instance = CANRegister(&config->can_init_config);
 
     DJIMotorEnable(instance);

modules/motor/DJImotor/dji_motor.h

@@ -18,6 +18,7 @@
 #include "bsp_can.h"
 #include "controller.h"
 #include "motor_def.h"
+#include "stdint.h"
 
 #define DJI_MOTOR_CNT 12
 

modules/motor/motor_def.h

@@ -115,9 +115,9 @@ typedef struct
     float *speed_feedforward_ptr;   // 速度前馈数据指针
     float *current_feedforward_ptr; // 电流前馈数据指针
 
-    PID_Init_config_s current_PID;
-    PID_Init_config_s speed_PID;
-    PID_Init_config_s angle_PID;
+    PID_Init_Config_s current_PID;
+    PID_Init_Config_s speed_PID;
+    PID_Init_Config_s angle_PID;
 } Motor_Controller_Init_s;
 
 /* 用于初始化CAN电机的结构体,各类电机通用 */