166 lines
6.8 KiB
C
166 lines
6.8 KiB
C
/*
|
||
* Gimbal IMU Device - 接收来自云台的CAN IMU数据
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "gimbal_imu.h"
|
||
|
||
#include <string.h>
|
||
|
||
#include "bsp/fdcan.h"
|
||
#include "bsp/time.h"
|
||
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 无符号整数转换为浮点数
|
||
*
|
||
* @param x_int 输入的整数值
|
||
* @param x_min 最小值
|
||
* @param x_max 最大值
|
||
* @param bits 位数
|
||
* @return float 转换后的浮点数
|
||
*/
|
||
static float uint_to_float(uint32_t 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 解析CAN消息中的IMU数据
|
||
*/
|
||
static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_FROMCAN_t *gimbal_imu, uint32_t id, const uint8_t *data) {
|
||
if (gimbal_imu == NULL || data == NULL) {
|
||
return;
|
||
}
|
||
|
||
/* 判断是哪个ID的数据 */
|
||
if (id == gimbal_imu->param.accl_id) {
|
||
/* 解析加速度计数据 - 21位精度解包 */
|
||
uint32_t acc_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
|
||
uint32_t acc_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
|
||
uint32_t acc_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
|
||
|
||
gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
|
||
gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
|
||
gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
|
||
|
||
} else if (id == gimbal_imu->param.gyro_id) {
|
||
/* 解析陀螺仪数据 - 21位精度解包 */
|
||
uint32_t gyro_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
|
||
uint32_t gyro_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
|
||
uint32_t gyro_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
|
||
|
||
gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
|
||
gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
|
||
gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
|
||
|
||
} else if (id == gimbal_imu->param.eulr_id) {
|
||
/* 解析欧拉角数据 - 21位精度解包 */
|
||
uint32_t pit = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
|
||
uint32_t yaw = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
|
||
uint32_t rol = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
|
||
|
||
gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 21);
|
||
gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 21);
|
||
gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 21);
|
||
|
||
} else if (id == gimbal_imu->param.quat_id) {
|
||
/* 解析四元数数据 - 使用14位精度解包 */
|
||
uint16_t q0 = ((uint16_t)data[1] << 6) | ((uint16_t)data[2] >> 2);
|
||
uint16_t q1 = (((uint16_t)data[2] & 0x03) << 12) | ((uint16_t)data[3] << 4) | ((uint16_t)data[4] >> 4);
|
||
uint16_t q2 = (((uint16_t)data[4] & 0x0F) << 10) | ((uint16_t)data[5] << 2) | ((uint16_t)data[6] >> 6);
|
||
uint16_t q3 = (((uint16_t)data[6] & 0x3F) << 8) | (uint16_t)data[7];
|
||
|
||
gimbal_imu->data.quat.q0 = uint_to_float(q0, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||
gimbal_imu->data.quat.q1 = uint_to_float(q1, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||
gimbal_imu->data.quat.q2 = uint_to_float(q2, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||
gimbal_imu->data.quat.q3 = uint_to_float(q3, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
|
||
}
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 初始化Gimbal IMU设备
|
||
* @param gimbal_imu Gimbal IMU结构体指针
|
||
* @param param 参数配置
|
||
* @return 初始化结果
|
||
* - DEVICE_OK: 成功
|
||
* - DEVICE_ERR_NULL: 参数为空
|
||
*/
|
||
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_FROMCAN_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
|
||
if (gimbal_imu == NULL || param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
/* 清空数据 */
|
||
memset(gimbal_imu, 0, sizeof(GIMBAL_IMU_FROMCAN_t));
|
||
|
||
/* 复制参数 */
|
||
gimbal_imu->param = *param;
|
||
|
||
/* 设置设备类型 */
|
||
gimbal_imu->header.online = false;
|
||
|
||
/* 注册CAN ID到消息队列 */
|
||
BSP_FDCAN_RegisterId(param->can, param->accl_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||
BSP_FDCAN_RegisterId(param->can, param->gyro_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||
BSP_FDCAN_RegisterId(param->can, param->eulr_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||
BSP_FDCAN_RegisterId(param->can, param->quat_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
|
||
* @param gimbal_imu Gimbal IMU结构体指针
|
||
* @return 更新结果
|
||
* - DEVICE_OK: 成功
|
||
* - DEVICE_ERR_NULL: 参数为空
|
||
*/
|
||
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_FROMCAN_t *gimbal_imu) {
|
||
if (gimbal_imu == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
BSP_FDCAN_Message_t msg;
|
||
bool received = false;
|
||
|
||
/* 读取所有可用的CAN消息 */
|
||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
|
||
received = true;
|
||
}
|
||
|
||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
|
||
received = true;
|
||
}
|
||
|
||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
|
||
received = true;
|
||
}
|
||
|
||
while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
|
||
received = true;
|
||
}
|
||
|
||
/* 更新在线状态和时间戳 */
|
||
if (received) {
|
||
gimbal_imu->header.online = true;
|
||
gimbal_imu->header.last_online_time = BSP_TIME_Get();
|
||
} else {
|
||
/* 超时检测 - 100ms */
|
||
uint64_t now_time = BSP_TIME_Get();
|
||
if (now_time - gimbal_imu->header.last_online_time > 100000) { /* 100ms超时，单位微秒 */
|
||
gimbal_imu->header.online = false;
|
||
}
|
||
}
|
||
|
||
return DEVICE_OK;
|
||
}
|