129 lines
5.7 KiB
C
129 lines
5.7 KiB
C
/*
|
||
* 云台模组
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "gimbal.h"
|
||
#include "bsp/can.h"
|
||
#include "bsp/time.h"
|
||
#include <math.h>
|
||
#include "component/filter/filter.h"
|
||
#include "component/pid.h"
|
||
#include "device/motor_dm.h"
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
#define CAN_CHANNEL BSP_CAN_2
|
||
|
||
/* 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 -------------------------------------------------------- */
|
||
/* 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 ------------------------------------------------------- */
|
||
static void CanBridge_SendimuOnCan(const AHRS_All_t *imu_data) {
|
||
|
||
BSP_CAN_StdDataFrame_t can_frame;
|
||
|
||
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);
|
||
}
|
||
|
||
static void CanBridge_UpdateimuFromCan(void) {
|
||
BSP_CAN_Message_t rx_msg;
|
||
|
||
} |