145 lines
5.5 KiB
C
145 lines
5.5 KiB
C
/*
|
|
* Gimbal IMU Device - 接收来自云台的CAN IMU数据
|
|
*/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include "gimbal_imu.h"
|
|
|
|
#include <string.h>
|
|
|
|
#include "bsp/can.h"
|
|
|
|
/* Private function prototypes ---------------------------------------------- */
|
|
|
|
/**
|
|
* @brief 无符号整数转换为浮点数
|
|
*
|
|
* @param x_int 输入的整数值
|
|
* @param x_min 最小值
|
|
* @param x_max 最大值
|
|
* @param bits 位数
|
|
* @return float 转换后的浮点数
|
|
*/
|
|
static float uint_to_float(uint16_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_t *gimbal_imu, uint32_t id, const uint8_t *data) {
|
|
if (gimbal_imu == NULL || data == NULL) {
|
|
return;
|
|
}
|
|
|
|
/* 更新在线状态 */
|
|
gimbal_imu->header.online = true;
|
|
|
|
/* 判断是哪个ID的数据 */
|
|
if (id == gimbal_imu->param.accl_id) {
|
|
/* 解析加速度计数据 */
|
|
uint16_t acc_x = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
|
|
uint16_t acc_y = (uint16_t)data[4] | ((uint16_t)data[5] << 8);
|
|
uint16_t acc_z = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
|
|
|
|
gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16);
|
|
gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16);
|
|
gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16);
|
|
|
|
/* 温度数据 */
|
|
gimbal_imu->data.temp = (float)data[1];
|
|
|
|
} else if (id == gimbal_imu->param.gyro_id) {
|
|
/* 解析陀螺仪数据 */
|
|
uint16_t gyro_x = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
|
|
uint16_t gyro_y = (uint16_t)data[4] | ((uint16_t)data[5] << 8);
|
|
uint16_t gyro_z = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
|
|
|
|
gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16);
|
|
gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16);
|
|
gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16);
|
|
|
|
} else if (id == gimbal_imu->param.eulr_id) {
|
|
/* 解析欧拉角数据 */
|
|
uint16_t pit = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
|
|
uint16_t yaw = (uint16_t)data[4] | ((uint16_t)data[5] << 8);
|
|
uint16_t rol = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
|
|
|
|
gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 16);
|
|
gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 16);
|
|
gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 16);
|
|
|
|
} 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设备
|
|
*/
|
|
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
|
|
if (gimbal_imu == NULL || param == NULL) {
|
|
return -1;
|
|
}
|
|
|
|
/* 清空数据 */
|
|
memset(gimbal_imu, 0, sizeof(GIMBAL_IMU_t));
|
|
|
|
/* 复制参数 */
|
|
gimbal_imu->param = *param;
|
|
|
|
/* 设置设备类型 */
|
|
gimbal_imu->header.online = false;
|
|
|
|
/* 注册CAN ID到消息队列 */
|
|
BSP_CAN_RegisterId(param->can, param->accl_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
|
|
BSP_CAN_RegisterId(param->can, param->gyro_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
|
|
BSP_CAN_RegisterId(param->can, param->eulr_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
|
|
BSP_CAN_RegisterId(param->can, param->quat_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
|
|
*/
|
|
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu) {
|
|
if (gimbal_imu == NULL) {
|
|
return -1;
|
|
}
|
|
|
|
BSP_CAN_Message_t msg;
|
|
|
|
/* 读取所有可用的CAN消息 */
|
|
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
|
|
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
|
|
}
|
|
|
|
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
|
|
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
|
|
}
|
|
|
|
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
|
|
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
|
|
}
|
|
|
|
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
|
|
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
|
|
}
|
|
|
|
return 0;
|
|
}
|