finish all bsp utilities

modules/imu/ins_task.c

@@ -16,7 +16,7 @@
 #include "bsp_temperature.h"
 #include "spi.h"
 
-INS_t INS;
+static INS_t INS;
 IMU_Param_t IMU_Param;
 PID_t TempCtrl = {0};
 
@@ -31,6 +31,11 @@ float RefTemp = 40;
 
 static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
 
+attitude_t *GetINSptr()
+{
+    return (attitude_t *)&INS.Roll;
+}
+
 void INS_Init(void)
 {
     // while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR);
@@ -51,7 +56,7 @@ void INS_Init(void)
                                 .Ki = 20,
                                 .Kd = 0,
                                 .Improve = 0x01}; // enable integratiaon limit
-    PID_Init(&TempCtrl,&config);
+    PID_Init(&TempCtrl, &config);
 
     // noise of accel is relatively big and of high freq,thus lpf is used
     INS.AccelLPF = 0.0085;

modules/imu/ins_task.h

@@ -23,12 +23,20 @@
 
 #define INS_TASK_PERIOD 1
 
+typedef struct
+{
+    float Roll;
+    float Pitch;
+    float Yaw;
+    float YawTotalAngle;
+} attitude_t;
+
 typedef struct
 {
     float q[4]; // 四元数估计值
 
-    float Gyro[3];  // 角速度
-    float Accel[3]; // 加速度
+    float Gyro[3];          // 角速度
+    float Accel[3];         // 加速度
     float MotionAccel_b[3]; // 机体坐标加速度
     float MotionAccel_n[3]; // 绝对系加速度
 
@@ -49,10 +57,9 @@ typedef struct
     float YawTotalAngle;
 } INS_t;
 
-
 /**
- * @brief 用于修正安装误差的参数,demo中可无视
- * 
+ * @brief 用于修正安装误差的参数
+ *
  */
 typedef struct
 {
@@ -65,16 +72,79 @@ typedef struct
     float Roll;
 } IMU_Param_t;
 
-extern INS_t INS;
+/**
+ * @brief 反馈位姿结构体指针,apps通过初始化时保存该指针以实现数据访问
+ *
+ * @return attitude_t*
+ */
+attitude_t *GetINSptr();
 
+/**
+ * @brief 初始化惯导解算系统
+ *
+ */
 void INS_Init(void);
+
+/**
+ * @brief 此函数放入实时系统中,以1kHz频率运行
+ *        p.s. osDelay(1);
+ *
+ */
 void INS_Task(void);
+
+/**
+ * @brief 温控函数
+ *
+ */
 void IMU_Temperature_Ctrl(void);
 
+/**
+ * @brief 四元数更新函数,即实现dq/dt=0.5Ωq
+ *
+ * @param q  四元数
+ * @param gx
+ * @param gy
+ * @param gz
+ * @param dt 距离上次调用的时间间隔
+ */
 void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
+
+/**
+ * @brief 四元数转换成欧拉角 ZYX
+ *
+ * @param q
+ * @param Yaw
+ * @param Pitch
+ * @param Roll
+ */
 void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
+
+/**
+ * @brief ZYX欧拉角转换为四元数
+ *
+ * @param Yaw
+ * @param Pitch
+ * @param Roll
+ * @param q
+ */
 void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
+
+/**
+ * @brief 机体系到惯性系的变换函数
+ *
+ * @param vecBF body frame
+ * @param vecEF earth frame
+ * @param q
+ */
 void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
+
+/**
+ * @brief 惯性系转换到机体系
+ *
+ * @param vecEF
+ * @param vecBF
+ * @param q
+ */
 void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
 
 #endif