修线

CMakeLists.txt

@@ -73,6 +73,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/task/atti_esti.c
     User/task/blink.c
     User/task/ctrl_chassis.c
+    User/task/monitor.c
     User/task/imu.c
     User/task/init.c
     User/task/user_task.c

User/bsp/pwm.h

@@ -41,8 +41,8 @@ typedef enum {
 
 /* Exported functions prototypes -------------------------------------------- */
 int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
-int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); //设置占空比，范围0.0~1.0
+int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
-int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); //设置频率，单位Hz
+int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
 int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
 uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
 uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);

User/component/user_math.c

@@ -48,13 +48,11 @@ inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
 inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
 
 /**
- * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
- * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
- *
+ * \param sp 设定值
- * \param sp 被操作的值
+ * \param fb 反馈值
- * \param fb 变化量
  * \param range 被操作的值变化范围，正数时起效
- *
  * \return 函数运行结果
  */
 inline float CircleError(float sp, float fb, float range) {
@@ -71,9 +69,7 @@ inline float CircleError(float sp, float fb, float range) {
 }
 
 /**
- * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
+ * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
- * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
- *
  * \param origin 被操作的值
  * \param delta 变化量
  * \param range 被操作的值变化范围，正数时起效

User/component/user_math.h

@@ -21,12 +21,12 @@ extern "C" {
 #define M_DEG2RAD_MULT (0.01745329251f)
 #define M_RAD2DEG_MULT (57.2957795131f)
 
-#ifndef M_PI
+#ifndef M_PI_2
-#define M_PI 3.14159265358979323846f
+#define M_PI_2 1.57079632679f
 #endif
 
-#ifndef M_PI_2
+#ifndef M_PI
-#define M_PI_2 1.57079632679489661923f
+#define M_PI 3.14159265358979323846f
 #endif
 
 #ifndef M_2PI
@@ -86,21 +86,17 @@ float Sign(float in);
 void ResetMoveVector(MoveVector_t *mv);
 
 /**
- * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
- * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
- *
+ * \param sp 设定值
- * \param sp 被操作的值
+ * \param fb 反馈值
- * \param fb 变化量
  * \param range 被操作的值变化范围，正数时起效
- *
  * \return 函数运行结果
  */
 float CircleError(float sp, float fb, float range);
 
 /**
- * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
+ * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
- * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
- *
  * \param origin 被操作的值
  * \param delta 变化量
  * \param range 被操作的值变化范围，正数时起效

User/device/dm_imu.c

@@ -0,0 +1,271 @@
+/*
+    DM_IMU数据获取（CAN）
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "dm_imu.h"
+
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+#include <string.h>
+
+/* Private define ----------------------------------------------------------- */
+#define DM_IMU_OFFLINE_TIMEOUT      1000    // 设备离线判定时间1000ms
+
+#define ACCEL_CAN_MAX (58.8f)
+#define ACCEL_CAN_MIN	(-58.8f)
+#define GYRO_CAN_MAX	(34.88f)
+#define GYRO_CAN_MIN	(-34.88f)
+#define PITCH_CAN_MAX	(90.0f)
+#define PITCH_CAN_MIN	(-90.0f)
+#define ROLL_CAN_MAX	(180.0f)
+#define ROLL_CAN_MIN	(-180.0f)
+#define YAW_CAN_MAX		(180.0f)
+#define YAW_CAN_MIN 	(-180.0f)
+#define TEMP_MIN			(0.0f)
+#define TEMP_MAX			(60.0f)
+#define Quaternion_MIN	(-1.0f)
+#define Quaternion_MAX	(1.0f)
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+
+static uint8_t count = 0; // 计数器，用于判断设备是否离线
+/**
+ * @brief: 无符号整数转换为浮点数函数
+ */
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+}
+
+/**
+ * @brief 解析加速度计数据
+ */
+static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int8_t temp = data[1];
+    uint16_t acc_x_raw = (data[3] << 8) | data[2];
+    uint16_t acc_y_raw = (data[5] << 8) | data[4];
+    uint16_t acc_z_raw = (data[7] << 8) | data[6];
+    imu->data.temp = (float)temp;
+    imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析陀螺仪数据
+ */
+static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t gyro_x_raw = (data[3] << 8) | data[2];
+    uint16_t gyro_y_raw = (data[5] << 8) | data[4];
+    uint16_t gyro_z_raw = (data[7] << 8) | data[6];
+    imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析欧拉角数据
+ */
+static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t pit_raw = (data[3] << 8) | data[2];
+    uint16_t yaw_raw = (data[5] << 8) | data[4];
+    uint16_t rol_raw = (data[7] << 8) | data[6];
+    imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 解析四元数数据
+ */
+static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
+    int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
+    int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
+    int z = ((data[6] & 0x3F) << 8) | data[7];
+    imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
+    return DEVICE_OK;
+}
+
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
+    if (imu == NULL || param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 初始化设备头部
+    imu->header.online = false;
+    imu->header.last_online_time = 0;
+    
+    // 配置参数
+    imu->param.can = param->can;
+    imu->param.can_id = param->can_id;
+    imu->param.device_id = param->device_id;
+    imu->param.master_id = param->master_id;
+    
+    // 清零数据
+    memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
+    
+    // 注册CAN接收队列，用于接收回复报文
+    int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
+    if (result != BSP_OK) {
+        return DEVICE_ERR;
+    }
+
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 请求IMU数据
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
+    uint8_t tx_data[4] = {
+        imu->param.device_id & 0xFF,        // id_L
+        (imu->param.device_id >> 8) & 0xFF, // id_H
+        (uint8_t)rid,                       // RID
+        0xCC                                // 固定值
+    };
+    
+    // 发送标准数据帧
+    BSP_CAN_StdDataFrame_t frame = {
+        .id = imu->param.can_id,
+        .dlc = 4,
+    };
+    memcpy(frame.data, tx_data, 4);
+    int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
+    return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_CAN_Message_t msg;
+    int8_t result;
+    bool data_received = false;
+    
+    // 持续接收所有可用消息
+    while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
+        // 验证回复数据格式（至少检查数据长度）
+        if (msg.dlc < 3) {
+            continue; // 跳过无效消息
+        }
+        
+        // 根据数据位的第0位确定反馈报文类型
+        uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
+        
+        // 根据RID类型解析数据
+        int8_t parse_result = DEVICE_ERR;
+        switch (rid) {
+            case 0x01: // RID_ACCL
+                parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
+                break;
+            case 0x02: // RID_GYRO
+                parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
+                break;
+            case 0x03: // RID_EULER
+                parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
+                break;
+            case 0x04: // RID_QUATERNION
+                parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
+                break;
+            default:
+                continue; // 跳过未知类型的消息
+        }
+        
+        // 如果解析成功，标记为收到数据
+        if (parse_result == DEVICE_OK) {
+            data_received = true;
+        }
+    }
+    
+    // 如果收到任何有效数据，更新设备状态
+    if (data_received) {
+        imu->header.online = true;
+        imu->header.last_online_time = BSP_TIME_Get_ms();
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}
+
+/** 
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
+ */ 
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    switch (count) {
+        case 0:
+            DM_IMU_Request(imu, RID_ACCL);
+            break;
+        case 1:
+            DM_IMU_Request(imu, RID_GYRO);
+            break;
+        case 2:
+            DM_IMU_Request(imu, RID_EULER);
+            break;
+        case 3:
+            DM_IMU_Request(imu, RID_QUATERNION);
+            DM_IMU_Update(imu); // 更新所有数据
+            break;
+    }
+    count++;
+    if (count >= 4) {
+        count = 0; // 重置计数器
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR;
+}
+
+/**
+ * @brief 检查设备是否在线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return false;
+    }
+    
+    uint32_t current_time = BSP_TIME_Get_ms();
+    return imu->header.online && 
+           (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
+}

User/device/dm_imu.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "component/ahrs.h"
+#include "bsp/can.h"
+/* Exported constants ------------------------------------------------------- */
+
+#define DM_IMU_CAN_ID_DEFAULT 0x6FF
+#define DM_IMU_ID_DEFAULT 0x42
+#define DM_IMU_MST_ID_DEFAULT 0x43
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+  BSP_CAN_t can;       // CAN总线句柄
+  uint16_t can_id;     // CAN通信ID
+  uint8_t device_id;   // 设备ID
+  uint8_t master_id;   // 主机ID
+} DM_IMU_Param_t;
+typedef enum {
+  RID_ACCL = 0x01, // 加速度计
+  RID_GYRO = 0x02, // 陀螺仪
+  RID_EULER = 0x03, // 欧拉角
+  RID_QUATERNION = 0x04, // 四元数
+} DM_IMU_RID_t;
+
+typedef struct {
+  AHRS_Accl_t accl; // 加速度计
+  AHRS_Gyro_t gyro; // 陀螺仪
+  AHRS_Eulr_t euler; // 欧拉角
+  AHRS_Quaternion_t quat; // 四元数
+  float temp; // 温度
+} DM_IMU_Data_t;
+
+typedef struct {
+  DEVICE_Header_t header;
+  DM_IMU_Param_t param; // IMU参数配置
+  DM_IMU_Data_t data; // IMU数据
+} DM_IMU_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ * @param imu DM IMU设备结构体指针
+ * @param param IMU参数配置指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
+
+/**
+ * @brief 请求IMU数据
+ * @param imu DM IMU设备结构体指针
+ * @param rid 请求的数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
+
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu);
+
+/**
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
+
+/**
+ * @brief 检查设备是否在线
+ * @param imu DM IMU设备结构体指针
+ * @return true 在线，false 离线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu);
+
+
+#ifdef __cplusplus
+}
+#endif

User/task/1.c

@@ -1,95 +0,0 @@
-/*
-    imu Task
-    
-*/
-
-/* Includes ----------------------------------------------------------------- */
-#include "task/user_task.h"
-/* USER INCLUDE BEGIN */
-#include "bsp/pwm.h"
-#include "component/ahrs.h"
-#include "component/pid.h"
-#include "device/bmi088.h"
-/* USER INCLUDE END */
-
-/* Private typedef ---------------------------------------------------------- */
-/* Private define ----------------------------------------------------------- */
-/* Private macro ------------------------------------------------------------ */
-/* Private variables -------------------------------------------------------- */
-/* USER STRUCT BEGIN */
-
-BMI088_t bmi088;
-
-AHRS_t gimbal_ahrs;
-AHRS_Magn_t magn;
-AHRS_Eulr_t eulr_to_send;
-
-KPID_t imu_temp_ctrl_pid;
-
-BMI088_Cali_t cali_bmi088= {
-  .gyro_offset = {0.0f,0.0f,0.0f},
-};
-
-/* USER STRUCT END */
-
-/* Private function --------------------------------------------------------- */
-/* Exported functions ------------------------------------------------------- */
-void Task_imu(void *argument) {
-  (void)argument; /* 未使用argument，消除警告 */
-
-  
-  /* 计算任务运行到指定频率需要等待的tick数 */
-  const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
-
-  osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */
-
-  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
-  /* USER CODE INIT BEGIN */
-  BMI088_Init(&bmi088,&cali_bmi088);
-  AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
-
-  /* USER CODE INIT END */
-  
-  while (1) {
-    tick += delay_tick; /* 计算下一个唤醒时刻 */
-    /* USER CODE BEGIN */
-    BMI088_WaitNew();
-    BMI088_AcclStartDmaRecv();
-    BMI088_AcclWaitDmaCplt();
-
-    BMI088_GyroStartDmaRecv();
-    BMI088_GyroWaitDmaCplt();
-
-    /* 锁住RTOS内核防止数据解析过程中断，造成错误 */
-    osKernelLock();
-    /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
-  BMI088_ParseAccl(&bmi088);
-  /* 扩大加速度数据10倍，并交换x和y */
-  float temp_x = bmi088.accl.x * 10.0f;
-  float temp_y = bmi088.accl.y * 10.0f;
-  bmi088.accl.x = temp_y;
-  bmi088.accl.y = -temp_x;
-  bmi088.accl.z *= 10.0f;
-
-  BMI088_ParseGyro(&bmi088);
-  /* 交换陀螺仪x和y */
-  float temp_gyro_x = bmi088.gyro.x;
-  bmi088.gyro.x = bmi088.gyro.y;
-  bmi088.gyro.y = -temp_gyro_x;
-    // IST8310_Parse(&ist8310);
-
-    /* 根据设备接收到的数据进行姿态解析 */
-    AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
-
-    /* 根据解析出来的四元数计算欧拉角 */
-  AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
-  /* 交换pit和rol */
-  float temp_rol = eulr_to_send.rol;
-  eulr_to_send.rol = eulr_to_send.pit;
-  eulr_to_send.pit = temp_rol;
-    osKernelUnlock();
-    /* USER CODE END */
-    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
-  }
-  
-}

User/task/atti_esti.c

@@ -4,7 +4,6 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "bsp/mm.h"

User/task/config.yaml

@@ -26,3 +26,10 @@
   function: Task_atti_esti
   name: atti_esti
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_monitor
+  name: monitor
+  stack: 256

User/task/init.c

@@ -34,6 +34,7 @@ void Task_Init(void *argument) {
   task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu);
   task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
   task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
+  task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */

User/task/monitor.c

@@ -0,0 +1,44 @@
+/*
+    monitor Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_monitor(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / MONITOR_FREQ;
+
+  osDelay(MONITOR_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/user_task.c

@@ -22,10 +22,15 @@ const osThreadAttr_t attr_imu = {
 const osThreadAttr_t attr_ctrl_chassis = {
     .name = "ctrl_chassis",
     .priority = osPriorityNormal,
-    .stack_size = 512 * 4,
+    .stack_size = 256 * 4,
 };
 const osThreadAttr_t attr_atti_esti = {
     .name = "atti_esti",
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
 };
+const osThreadAttr_t attr_monitor = {
+    .name = "monitor",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};

User/task/user_task.h

@@ -17,6 +17,7 @@ extern "C" {
 #define IMU_FREQ (500.0)
 #define CTRL_CHASSIS_FREQ (500.0)
 #define ATTI_ESTI_FREQ (500.0)
+#define MONITOR_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -24,6 +25,7 @@ extern "C" {
 #define IMU_INIT_DELAY (0)
 #define CTRL_CHASSIS_INIT_DELAY (0)
 #define ATTI_ESTI_INIT_DELAY (0)
+#define MONITOR_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -37,6 +39,7 @@ typedef struct {
         osThreadId_t imu;
         osThreadId_t ctrl_chassis;
         osThreadId_t atti_esti;
+        osThreadId_t monitor;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -68,6 +71,7 @@ typedef struct {
         UBaseType_t imu;
         UBaseType_t ctrl_chassis;
         UBaseType_t atti_esti;
+        UBaseType_t monitor;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -76,6 +80,7 @@ typedef struct {
         float imu;
         float ctrl_chassis;
         float atti_esti;
+        float monitor;
     } freq;
 
     /* 任务最近运行时间 */
@@ -84,6 +89,7 @@ typedef struct {
         float imu;
         float ctrl_chassis;
         float atti_esti;
+        float monitor;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -97,6 +103,7 @@ extern const osThreadAttr_t attr_blink;
 extern const osThreadAttr_t attr_imu;
 extern const osThreadAttr_t attr_ctrl_chassis;
 extern const osThreadAttr_t attr_atti_esti;
+extern const osThreadAttr_t attr_monitor;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
@@ -104,6 +111,7 @@ void Task_blink(void *argument);
 void Task_imu(void *argument);
 void Task_ctrl_chassis(void *argument);
 void Task_atti_esti(void *argument);
+void Task_monitor(void *argument);
 
 #ifdef __cplusplus
 }