152 lines
6.4 KiB
C
152 lines
6.4 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 uint32_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 (uint32_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 */
|
||
BSP_CAN_Init();
|
||
/* 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: 加速度计数据 (acc_x, acc_y, acc_z) - 使用21位精度 */
|
||
can_frame.id = CAN_ID_IMU_ACCEL;
|
||
can_frame.dlc = 8;
|
||
uint32_t acc_x = float_to_uint(imu_data.accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
|
||
uint32_t acc_y = float_to_uint(imu_data.accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
|
||
uint32_t acc_z = float_to_uint(imu_data.accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
|
||
// 打包: acc_x(21bit) acc_y(21bit) acc_z(21bit) = 63bits
|
||
can_frame.data[0] = (acc_x >> 13) & 0xFF;
|
||
can_frame.data[1] = (acc_x >> 5) & 0xFF;
|
||
can_frame.data[2] = ((acc_x << 3) & 0xF8) | ((acc_y >> 18) & 0x07);
|
||
can_frame.data[3] = (acc_y >> 10) & 0xFF;
|
||
can_frame.data[4] = (acc_y >> 2) & 0xFF;
|
||
can_frame.data[5] = ((acc_y << 6) & 0xC0) | ((acc_z >> 15) & 0x3F);
|
||
can_frame.data[6] = (acc_z >> 7) & 0xFF;
|
||
can_frame.data[7] = (acc_z << 1) & 0xFE;
|
||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
|
||
|
||
/* 包2: 陀螺仪数据 (gyro_x, gyro_y, gyro_z) - 使用21位精度 */
|
||
can_frame.id = CAN_ID_IMU_GYRO;
|
||
can_frame.dlc = 8;
|
||
uint32_t gyro_x = float_to_uint(imu_data.gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
|
||
uint32_t gyro_y = float_to_uint(imu_data.gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
|
||
uint32_t gyro_z = float_to_uint(imu_data.gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
|
||
// 打包: gyro_x(21bit) gyro_y(21bit) gyro_z(21bit) = 63bits
|
||
can_frame.data[0] = (gyro_x >> 13) & 0xFF;
|
||
can_frame.data[1] = (gyro_x >> 5) & 0xFF;
|
||
can_frame.data[2] = ((gyro_x << 3) & 0xF8) | ((gyro_y >> 18) & 0x07);
|
||
can_frame.data[3] = (gyro_y >> 10) & 0xFF;
|
||
can_frame.data[4] = (gyro_y >> 2) & 0xFF;
|
||
can_frame.data[5] = ((gyro_y << 6) & 0xC0) | ((gyro_z >> 15) & 0x3F);
|
||
can_frame.data[6] = (gyro_z >> 7) & 0xFF;
|
||
can_frame.data[7] = (gyro_z << 1) & 0xFE;
|
||
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
|
||
|
||
/* 包3: 欧拉角数据 (pit, yaw, rol) - 使用21位精度 */
|
||
can_frame.id = CAN_ID_IMU_EULER;
|
||
can_frame.dlc = 8;
|
||
uint32_t pit = float_to_uint(imu_data.eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX, 21);
|
||
uint32_t yaw = float_to_uint(imu_data.eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX, 21);
|
||
uint32_t rol = float_to_uint(imu_data.eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX, 21);
|
||
// 打包: pit(21bit) yaw(21bit) rol(21bit) = 63bits
|
||
can_frame.data[0] = (pit >> 13) & 0xFF;
|
||
can_frame.data[1] = (pit >> 5) & 0xFF;
|
||
can_frame.data[2] = ((pit << 3) & 0xF8) | ((yaw >> 18) & 0x07);
|
||
can_frame.data[3] = (yaw >> 10) & 0xFF;
|
||
can_frame.data[4] = (yaw >> 2) & 0xFF;
|
||
can_frame.data[5] = ((yaw << 6) & 0xC0) | ((rol >> 15) & 0x3F);
|
||
can_frame.data[6] = (rol >> 7) & 0xFF;
|
||
can_frame.data[7] = (rol << 1) & 0xFE;
|
||
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); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
|
||
} |