149 lines
5.9 KiB
C
149 lines
5.9 KiB
C
/*
|
||
imu Task
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "task/user_task.h"
|
||
/* USER INCLUDE BEGIN */
|
||
#include "bsp/can.h"
|
||
#include "component/ahrs.h"
|
||
#include <string.h>
|
||
/* USER INCLUDE END */
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* CAN ID定义 - 使用标准帧 */
|
||
#define CAN_ID_IMU_ACCEL 0x100 /* 加速度计数据 */
|
||
#define CAN_ID_IMU_GYRO 0x101 /* 陀螺仪数据 */
|
||
#define CAN_ID_IMU_EULER 0x102 /* 欧拉角数据 */
|
||
#define CAN_ID_IMU_QUAT 0x103 /* 四元数数据 */
|
||
|
||
/* 数据范围定义（与DM_IMU解析一致） */
|
||
#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 QUAT_MIN (-1.0f)
|
||
#define QUAT_MAX (1.0f)
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* USER STRUCT BEGIN */
|
||
static AHRS_All_t imu_data;
|
||
|
||
/* USER STRUCT END */
|
||
|
||
/* Private function --------------------------------------------------------- */
|
||
/**
|
||
* @brief 浮点数转换为无符号整数函数
|
||
*/
|
||
static uint16_t float_to_uint(float x, float x_min, float x_max, int bits) {
|
||
float span = x_max - x_min;
|
||
float offset = x_min;
|
||
// 限幅
|
||
if (x > x_max) x = x_max;
|
||
if (x < x_min) x = x_min;
|
||
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
|
||
}
|
||
|
||
/* 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 */
|
||
|
||
/* USER CODE INIT END */
|
||
|
||
while (1) {
|
||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||
/* USER CODE BEGIN */
|
||
/* 从消息队列获取IMU数据 */
|
||
if (osMessageQueueGet(task_runtime.msgq.gimbal_imu, &imu_data, NULL, 0) == osOK) {
|
||
BSP_CAN_StdDataFrame_t can_frame;
|
||
|
||
/* 包1: 加速度计数据 (temp, acc_x, acc_y, acc_z) */
|
||
can_frame.id = CAN_ID_IMU_ACCEL;
|
||
can_frame.dlc = 8;
|
||
can_frame.data[0] = 0; // 预留
|
||
can_frame.data[1] = 0; // 温度数据（如需要可添加）
|
||
uint16_t acc_x = float_to_uint(imu_data.accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||
uint16_t acc_y = float_to_uint(imu_data.accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||
uint16_t acc_z = float_to_uint(imu_data.accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||
can_frame.data[2] = acc_x & 0xFF;
|
||
can_frame.data[3] = (acc_x >> 8) & 0xFF;
|
||
can_frame.data[4] = acc_y & 0xFF;
|
||
can_frame.data[5] = (acc_y >> 8) & 0xFF;
|
||
can_frame.data[6] = acc_z & 0xFF;
|
||
can_frame.data[7] = (acc_z >> 8) & 0xFF;
|
||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
|
||
|
||
/* 包2: 陀螺仪数据 (gyro_x, gyro_y, gyro_z) */
|
||
can_frame.id = CAN_ID_IMU_GYRO;
|
||
can_frame.dlc = 8;
|
||
can_frame.data[0] = 0; // 预留
|
||
can_frame.data[1] = 0; // 预留
|
||
uint16_t gyro_x = float_to_uint(imu_data.gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||
uint16_t gyro_y = float_to_uint(imu_data.gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||
uint16_t gyro_z = float_to_uint(imu_data.gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||
can_frame.data[2] = gyro_x & 0xFF;
|
||
can_frame.data[3] = (gyro_x >> 8) & 0xFF;
|
||
can_frame.data[4] = gyro_y & 0xFF;
|
||
can_frame.data[5] = (gyro_y >> 8) & 0xFF;
|
||
can_frame.data[6] = gyro_z & 0xFF;
|
||
can_frame.data[7] = (gyro_z >> 8) & 0xFF;
|
||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
|
||
|
||
/* 包3: 欧拉角数据 (pit, yaw, rol) */
|
||
can_frame.id = CAN_ID_IMU_EULER;
|
||
can_frame.dlc = 8;
|
||
can_frame.data[0] = 0; // 预留
|
||
can_frame.data[1] = 0; // 预留
|
||
uint16_t pit = float_to_uint(imu_data.eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX, 16);
|
||
uint16_t yaw = float_to_uint(imu_data.eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX, 16);
|
||
uint16_t rol = float_to_uint(imu_data.eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX, 16);
|
||
can_frame.data[2] = pit & 0xFF;
|
||
can_frame.data[3] = (pit >> 8) & 0xFF;
|
||
can_frame.data[4] = yaw & 0xFF;
|
||
can_frame.data[5] = (yaw >> 8) & 0xFF;
|
||
can_frame.data[6] = rol & 0xFF;
|
||
can_frame.data[7] = (rol >> 8) & 0xFF;
|
||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
|
||
|
||
/* 包4: 四元数数据 (q0, q1, q2, q3) - 使用14位精度打包 */
|
||
can_frame.id = CAN_ID_IMU_QUAT;
|
||
can_frame.dlc = 8;
|
||
uint16_t q0 = float_to_uint(imu_data.quat.q0, QUAT_MIN, QUAT_MAX, 14);
|
||
uint16_t q1 = float_to_uint(imu_data.quat.q1, QUAT_MIN, QUAT_MAX, 14);
|
||
uint16_t q2 = float_to_uint(imu_data.quat.q2, QUAT_MIN, QUAT_MAX, 14);
|
||
uint16_t q3 = float_to_uint(imu_data.quat.q3, QUAT_MIN, QUAT_MAX, 14);
|
||
// 打包: q0(14bit) q1(14bit) q2(14bit) q3(14bit) = 56bits
|
||
can_frame.data[0] = 0; // 预留
|
||
can_frame.data[1] = (q0 >> 6) & 0xFF;
|
||
can_frame.data[2] = ((q0 << 2) & 0xFC) | ((q1 >> 12) & 0x03);
|
||
can_frame.data[3] = (q1 >> 4) & 0xFF;
|
||
can_frame.data[4] = ((q1 << 4) & 0xF0) | ((q2 >> 10) & 0x0F);
|
||
can_frame.data[5] = (q2 >> 2) & 0xFF;
|
||
can_frame.data[6] = ((q2 << 6) & 0xC0) | ((q3 >> 8) & 0x3F);
|
||
can_frame.data[7] = q3 & 0xFF;
|
||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
|
||
}
|
||
|
||
/* USER CODE END */
|
||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
|
||
} |