mirror of
https://gitee.com/dlmu-cone/tronone-h7-scaffold
synced 2026-07-24 19:37:45 +08:00
add damiao imu module
This commit is contained in:
TuxMonkey
@@ -1,5 +1,251 @@
|
|||||||
//
|
|
||||||
// Created by ASUS on 2025/11/17.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "damiao_imu.h"
|
#include "damiao_imu.h"
|
||||||
|
#include "fdcan.h"
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
imu_t imu;
|
||||||
|
|
||||||
|
/**
|
||||||
|
************************************************************************
|
||||||
|
* @brief: float_to_uint: 浮点数转换为无符号整数函数
|
||||||
|
* @param[in]: x_float: 待转换的浮点数
|
||||||
|
* @param[in]: x_min: 范围最小值
|
||||||
|
* @param[in]: x_max: 范围最大值
|
||||||
|
* @param[in]: bits: 目标无符号整数的位数
|
||||||
|
* @retval: 无符号整数结果
|
||||||
|
* @details: 将给定的浮点数 x 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个指定位数的无符号整数
|
||||||
|
************************************************************************
|
||||||
|
**/
|
||||||
|
int float_to_uint(float x_float, float x_min, float x_max, int bits)
|
||||||
|
{
|
||||||
|
/* Converts a float to an unsigned int, given range and number of bits */
|
||||||
|
float span = x_max - x_min;
|
||||||
|
float offset = x_min;
|
||||||
|
return (int) ((x_float - offset) * ((float) ((1 << bits) - 1)) / span);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
************************************************************************
|
||||||
|
* @brief: uint_to_float: 无符号整数转换为浮点数函数
|
||||||
|
* @param[in]: x_int: 待转换的无符号整数
|
||||||
|
* @param[in]: x_min: 范围最小值
|
||||||
|
* @param[in]: x_max: 范围最大值
|
||||||
|
* @param[in]: bits: 无符号整数的位数
|
||||||
|
* @retval: 浮点数结果
|
||||||
|
* @details: 将给定的无符号整数 x_int 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个浮点数
|
||||||
|
************************************************************************
|
||||||
|
**/
|
||||||
|
float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||||||
|
{
|
||||||
|
/* converts unsigned int to float, given range and number of bits */
|
||||||
|
float span = x_max - x_min;
|
||||||
|
float offset = x_min;
|
||||||
|
return ((float) x_int) * span / ((float) ((1 << bits) - 1)) + offset;
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_init(uint8_t can_id, uint8_t mst_id, FDCAN_HandleTypeDef *hfdcan)
|
||||||
|
{
|
||||||
|
imu.can_id = can_id;
|
||||||
|
imu.mst_id = mst_id;
|
||||||
|
imu.can_handle = hfdcan;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
发送指令
|
||||||
|
*/
|
||||||
|
static void imu_send_cmd(uint8_t reg_id, uint8_t ac, uint32_t data)
|
||||||
|
{
|
||||||
|
|
||||||
|
if (imu.can_handle == NULL)
|
||||||
|
return;
|
||||||
|
|
||||||
|
FDCAN_TxHeaderTypeDef tx_header;
|
||||||
|
|
||||||
|
uint8_t buf[8] = {0xCC, reg_id, ac, 0xDD, 0, 0, 0, 0};
|
||||||
|
memcpy(buf + 4, &data, 4);
|
||||||
|
|
||||||
|
tx_header.DataLength = FDCAN_DLC_BYTES_8;
|
||||||
|
tx_header.IdType = FDCAN_STANDARD_ID;
|
||||||
|
tx_header.TxFrameType = FDCAN_DATA_FRAME;
|
||||||
|
tx_header.Identifier = imu.can_id;
|
||||||
|
tx_header.FDFormat = FDCAN_CLASSIC_CAN;
|
||||||
|
tx_header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
|
||||||
|
tx_header.BitRateSwitch = FDCAN_BRS_OFF;
|
||||||
|
tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
|
||||||
|
tx_header.MessageMarker = 0x00;
|
||||||
|
|
||||||
|
if (HAL_FDCAN_GetTxFifoFreeLevel(imu.can_handle) > 2)
|
||||||
|
{
|
||||||
|
HAL_FDCAN_AddMessageToTxFifoQ(imu.can_handle, &tx_header, buf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void imu_write_reg(uint8_t reg_id, uint32_t data)
|
||||||
|
{
|
||||||
|
imu_send_cmd(reg_id, CMD_WRITE, data);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_read_reg(uint8_t reg_id)
|
||||||
|
{
|
||||||
|
imu_send_cmd(reg_id, CMD_READ, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_reboot()
|
||||||
|
{
|
||||||
|
imu_write_reg(REBOOT_IMU, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_accel_calibration()
|
||||||
|
{
|
||||||
|
imu_write_reg(ACCEL_CALI, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_gyro_calibration()
|
||||||
|
{
|
||||||
|
imu_write_reg(GYRO_CALI, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void imu_change_com_port(imu_com_port_e port)
|
||||||
|
{
|
||||||
|
imu_write_reg(CHANGE_COM, (uint8_t) port);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_set_active_mode_delay(uint32_t delay)
|
||||||
|
{
|
||||||
|
imu_write_reg(SET_DELAY, delay);
|
||||||
|
}
|
||||||
|
|
||||||
|
//设置成主动模式
|
||||||
|
void imu_change_to_active()
|
||||||
|
{
|
||||||
|
imu_write_reg(CHANGE_ACTIVE, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_change_to_request()
|
||||||
|
{
|
||||||
|
imu_write_reg(CHANGE_ACTIVE, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_set_baud(imu_baudrate_e baud)
|
||||||
|
{
|
||||||
|
imu_write_reg(SET_BAUD, (uint8_t) baud);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_set_can_id(uint8_t can_id)
|
||||||
|
{
|
||||||
|
imu_write_reg(SET_CAN_ID, can_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_set_mst_id(uint8_t mst_id)
|
||||||
|
{
|
||||||
|
imu_write_reg(SET_MST_ID, mst_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_save_parameters()
|
||||||
|
{
|
||||||
|
imu_write_reg(SAVE_PARAM, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_restore_settings()
|
||||||
|
{
|
||||||
|
imu_write_reg(RESTORE_SETTING, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void imu_request_accel()
|
||||||
|
{
|
||||||
|
imu_read_reg(ACCEL_DATA);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_request_gyro()
|
||||||
|
{
|
||||||
|
imu_read_reg(GYRO_DATA);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_request_euler()
|
||||||
|
{
|
||||||
|
imu_read_reg(EULER_DATA);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_request_quat()
|
||||||
|
{
|
||||||
|
imu_read_reg(QUAT_DATA);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void IMU_UpdateAccel(uint8_t *pData)
|
||||||
|
{
|
||||||
|
uint16_t accel[3];
|
||||||
|
|
||||||
|
accel[0] = pData[3] << 8 | pData[2];
|
||||||
|
accel[1] = pData[5] << 8 | pData[4];
|
||||||
|
accel[2] = pData[7] << 8 | pData[6];
|
||||||
|
|
||||||
|
imu.accel[0] = uint_to_float(accel[0], ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||||||
|
imu.accel[1] = uint_to_float(accel[1], ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||||||
|
imu.accel[2] = uint_to_float(accel[2], ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void IMU_UpdateGyro(uint8_t *pData)
|
||||||
|
{
|
||||||
|
uint16_t gyro[3];
|
||||||
|
|
||||||
|
gyro[0] = pData[3] << 8 | pData[2];
|
||||||
|
gyro[1] = pData[5] << 8 | pData[4];
|
||||||
|
gyro[2] = pData[7] << 8 | pData[6];
|
||||||
|
|
||||||
|
imu.gyro[0] = uint_to_float(gyro[0], GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||||||
|
imu.gyro[1] = uint_to_float(gyro[1], GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||||||
|
imu.gyro[2] = uint_to_float(gyro[2], GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void IMU_UpdateEuler(uint8_t *pData)
|
||||||
|
{
|
||||||
|
int euler[3];
|
||||||
|
|
||||||
|
euler[0] = pData[3] << 8 | pData[2];
|
||||||
|
euler[1] = pData[5] << 8 | pData[4];
|
||||||
|
euler[2] = pData[7] << 8 | pData[6];
|
||||||
|
|
||||||
|
imu.pitch = uint_to_float(euler[0], PITCH_CAN_MIN, PITCH_CAN_MAX, 16);
|
||||||
|
imu.yaw = uint_to_float(euler[1], YAW_CAN_MIN, YAW_CAN_MAX, 16);
|
||||||
|
imu.roll = uint_to_float(euler[2], ROLL_CAN_MIN, ROLL_CAN_MAX, 16);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void IMU_UpdateQuaternion(uint8_t *pData)
|
||||||
|
{
|
||||||
|
int w = pData[1] << 6 | ((pData[2] & 0xF8) >> 2);
|
||||||
|
int x = (pData[2] & 0x03) << 12 | (pData[3] << 4) | ((pData[4] & 0xF0) >> 4);
|
||||||
|
int y = (pData[4] & 0x0F) << 10 | (pData[5] << 2) | (pData[6] & 0xC0) >> 6;
|
||||||
|
int z = (pData[6] & 0x3F) << 8 | pData[7];
|
||||||
|
|
||||||
|
imu.q[0] = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
imu.q[1] = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
imu.q[2] = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
imu.q[3] = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
}
|
||||||
|
|
||||||
|
void IMU_UpdateData(uint8_t *pData)
|
||||||
|
{
|
||||||
|
|
||||||
|
switch (pData[0])
|
||||||
|
{
|
||||||
|
case 1:
|
||||||
|
IMU_UpdateAccel(pData);
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
IMU_UpdateGyro(pData);
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
IMU_UpdateEuler(pData);
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
IMU_UpdateQuaternion(pData);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
@@ -1,8 +1,128 @@
|
|||||||
//
|
#ifndef __DM_IMU_H
|
||||||
// Created by ASUS on 2025/11/17.
|
#define __DM_IMU_H
|
||||||
//
|
|
||||||
|
|
||||||
#ifndef TRONONEH7_SCAFFOLD_DAMIAO_IMU_H
|
#include "stm32h7xx_hal.h"
|
||||||
#define TRONONEH7_SCAFFOLD_DAMIAO_IMU_H
|
|
||||||
|
|
||||||
#endif //TRONONEH7_SCAFFOLD_DAMIAO_IMU_H
|
|
||||||
|
#define ACCEL_CAN_MAX (235.2f)
|
||||||
|
#define ACCEL_CAN_MIN (-235.2f)
|
||||||
|
#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 TEMP_MIN (0.0f)
|
||||||
|
#define TEMP_MAX (60.0f)
|
||||||
|
#define Quaternion_MIN (-1.0f)
|
||||||
|
#define Quaternion_MAX (1.0f)
|
||||||
|
|
||||||
|
#define CMD_READ 0
|
||||||
|
#define CMD_WRITE 1
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
COM_USB = 0,
|
||||||
|
COM_RS485,
|
||||||
|
COM_CAN,
|
||||||
|
COM_VOFA
|
||||||
|
|
||||||
|
} imu_com_port_e;
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
CAN_BAUD_1M = 0,
|
||||||
|
CAN_BAUD_500K,
|
||||||
|
CAN_BAUD_400K,
|
||||||
|
CAN_BAUD_250K,
|
||||||
|
CAN_BAUD_200K,
|
||||||
|
CAN_BAUD_100K,
|
||||||
|
CAN_BAUD_50K,
|
||||||
|
CAN_BAUD_25K
|
||||||
|
|
||||||
|
} imu_baudrate_e;
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
REBOOT_IMU = 0,
|
||||||
|
ACCEL_DATA,
|
||||||
|
GYRO_DATA,
|
||||||
|
EULER_DATA,
|
||||||
|
QUAT_DATA,
|
||||||
|
SET_ZERO,
|
||||||
|
ACCEL_CALI,
|
||||||
|
GYRO_CALI,
|
||||||
|
MAG_CALI,
|
||||||
|
CHANGE_COM,
|
||||||
|
SET_DELAY,
|
||||||
|
CHANGE_ACTIVE,
|
||||||
|
SET_BAUD,
|
||||||
|
SET_CAN_ID,
|
||||||
|
SET_MST_ID,
|
||||||
|
DATA_OUTPUT_SELECTION,
|
||||||
|
SAVE_PARAM = 254,
|
||||||
|
RESTORE_SETTING = 255
|
||||||
|
} reg_id_e;
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint8_t can_id;
|
||||||
|
uint8_t mst_id;
|
||||||
|
|
||||||
|
FDCAN_HandleTypeDef *can_handle;
|
||||||
|
|
||||||
|
float pitch;
|
||||||
|
float roll;
|
||||||
|
float yaw;
|
||||||
|
|
||||||
|
float gyro[3];
|
||||||
|
float accel[3];
|
||||||
|
|
||||||
|
float q[4];
|
||||||
|
|
||||||
|
float cur_temp;
|
||||||
|
|
||||||
|
} imu_t;
|
||||||
|
|
||||||
|
void imu_init(uint8_t can_id, uint8_t mst_id, FDCAN_HandleTypeDef *hfdcan);
|
||||||
|
|
||||||
|
void imu_write_reg(uint8_t reg_id, uint32_t data);
|
||||||
|
|
||||||
|
void imu_read_reg(uint8_t reg_id);
|
||||||
|
|
||||||
|
void imu_reboot();
|
||||||
|
|
||||||
|
void imu_accel_calibration();
|
||||||
|
|
||||||
|
void imu_gyro_calibration();
|
||||||
|
|
||||||
|
void imu_change_com_port(imu_com_port_e port);
|
||||||
|
|
||||||
|
void imu_set_active_mode_delay(uint32_t delay);
|
||||||
|
|
||||||
|
void imu_change_to_active();
|
||||||
|
|
||||||
|
void imu_change_to_request();
|
||||||
|
|
||||||
|
void imu_set_baud(imu_baudrate_e baud);
|
||||||
|
|
||||||
|
void imu_set_can_id(uint8_t can_id);
|
||||||
|
|
||||||
|
void imu_set_mst_id(uint8_t mst_id);
|
||||||
|
|
||||||
|
void imu_save_parameters();
|
||||||
|
|
||||||
|
void imu_restore_settings();
|
||||||
|
|
||||||
|
void imu_request_accel();
|
||||||
|
|
||||||
|
void imu_request_gyro();
|
||||||
|
|
||||||
|
void imu_request_euler();
|
||||||
|
|
||||||
|
void imu_request_quat();
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|||||||
Reference in New Issue
Block a user