robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加crc

Browse Source
This commit is contained in:
Robofish 2025-10-07 19:45:47 +08:00
parent da38e37d02
commit b5b1219f4a
14 changed files with 810 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -58,6 +58,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# User/component sources
User/component/ahrs.c
User/component/cmd.c
User/component/crc16.c
User/component/crc8.c
User/component/filter.c
User/component/kinematics.c
User/component/limiter.c
@ -68,8 +70,10 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/component/vmc.c
# User/device sources
User/device/ai.c
User/device/bmi088.c
User/device/buzzer.c
User/device/dm_imu.c
User/device/dr16.c
User/device/motor.c
User/device/motor_dm.c

2
Core/Inc/FreeRTOSConfig.h
View File

@ -187,7 +187,7 @@ standard names. */
/* USER CODE BEGIN Defines */
/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
// #define configAPPLICATION_ALLOCATED_HEAP 1
#define configAPPLICATION_ALLOCATED_HEAP 1
/* USER CODE END Defines */
#endif /* FREERTOS_CONFIG_H */

6
User/component/component_config.yaml
View File

@ -6,6 +6,12 @@ cmd:
dependencies:
- component/ahrs
enabled: true
crc16:
dependencies: []
enabled: true
crc8:
dependencies: []
enabled: true
filter:
dependencies:
- component/ahrs

62
User/component/crc16.c Normal file
View File

@ -0,0 +1,62 @@
#include "crc16.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint16_t crc16_tab[256] = {
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) {
return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
}
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
while (len--) crc = CRC16_Byte(crc, *buf++);
return crc;
}
bool CRC16_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
return expected ==
((const uint16_t *)((const uint8_t *)buf +
(len % 2)))[len / sizeof(uint16_t) - 1];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
User/component/crc16.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC16_INIT 0XFFFF
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
bool CRC16_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

52
User/component/crc8.c Normal file
View File

@ -0,0 +1,52 @@
#include "crc8.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint8_t crc8_tab[256] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
0xd7, 0x89, 0x6b, 0x35,
};
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
/* loop over the buffer data */
while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
return crc;
}
bool CRC8_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
return expected == buf[len - sizeof(uint8_t)];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
User/component/crc8.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC8_INIT 0xFF
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
bool CRC8_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

14
User/component/user_math.c
View File

@ -48,13 +48,11 @@ inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 被操作的值
* \param fb 变化量
* \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param sp 设定值
* \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
*/
inline float CircleError(float sp, float fb, float range) {
@ -71,9 +69,7 @@ inline float CircleError(float sp, float fb, float range) {
}
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
*
* \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效

22
User/component/user_math.h
View File

@ -21,12 +21,14 @@ extern "C" {
#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)
#ifndef M_PI_2
#define M_PI_2 1.57079632679f
#endif
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923f
#endif
#ifndef M_2PI
#define M_2PI 6.28318530717958647692f
#endif
@ -84,21 +86,17 @@ float Sign(float in);
void ResetMoveVector(MoveVector_t *mv);
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 被操作的值
* \param fb 变化量
* \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param sp 设定值
* \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
*/
float CircleError(float sp, float fb, float range);
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
*
* \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效

152
User/device/ai.c Normal file
View File

@ -0,0 +1,152 @@
// /*
// AI
// */
// /* Includes ----------------------------------------------------------------- */
// #include "ai.h"
// #include <string.h>
// #include "bsp/time.h"
// #include "bsp/uart.h"
// #include "component/crc16.h"
// #include "component/crc8.h"
// #include "component/user_math.h"
// #include "component/filter.h"
// /* Private define ----------------------------------------------------------- */
// #define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
// /* Private macro ------------------------------------------------------------ */
// /* Private typedef ---------------------------------------------------------- */
// /* Private variables -------------------------------------------------------- */
// static volatile uint32_t drop_message = 0;
// static uint8_t rxbuf[AI_LEN_RX_BUFF];
// static bool inited = false;
// static osThreadId_t thread_alert;
// /* Private function -------------------------------------------------------- */
// static void Ai_RxCpltCallback(void) {
// osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
// }
// static void Ai_IdleLineCallback(void) {
// osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
// }
// /* Exported functions ------------------------------------------------------- */
// int8_t AI_Init(AI_t *ai) {
// UNUSED(ai);
// ASSERT(ai);
// if (inited) return DEVICE_ERR_INITED;
// VERIFY((thread_alert = osThreadGetId()) != NULL);
// BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
// Ai_RxCpltCallback);
// BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
// Ai_IdleLineCallback);
// inited = true;
// return 0;
// }
// int8_t AI_Restart(void) {
// __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
// __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
// return DEVICE_OK;
// }
// int8_t AI_StartReceiving(AI_t *ai) {
// UNUSED(ai);
// if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
// AI_LEN_RX_BUFF) == HAL_OK)
// return DEVICE_OK;
// return DEVICE_ERR;
// }
// bool AI_WaitDmaCplt(void) {
// return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
// SIGNAL_AI_RAW_REDY);
// }
// int8_t AI_ParseHost(AI_t *ai) {
// if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
// goto error;
// ai->ai_online = true;
// memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
// memset(rxbuf, 0, AI_LEN_RX_BUFF);
// return DEVICE_OK;
// error:
// drop_message++;
// return DEVICE_ERR;
// }
// void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
// cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
// cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
// cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
// cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
// cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
// cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
// }
// int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
// if (ai == NULL) return DEVICE_ERR_NULL;
// if (cmd_host == NULL) return DEVICE_ERR_NULL;
// ai->ai_online = false;
// memset(&(ai->from_host), 0, sizeof(ai->from_host));
// memset(cmd_host, 0, sizeof(*cmd_host));
// return 0;
// }
// int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
// ai->to_host.mcu.id = AI_ID_MCU;
// memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
// sizeof(*quat));
// ai->to_host.mcu.package.data.notice = 0;
// if (ai->status == AI_STATUS_AUTOAIM)
// ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
// else if (ai->status == AI_STATUS_HITSWITCH)
// ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
// else if (ai->status == AI_STATUS_AUTOMATIC)
// ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
// ai->to_host.mcu.package.crc16 = CRC16_Calc(
// (const uint8_t *)&(ai->to_host.mcu.package),
// sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
// return DEVICE_OK;
// }
// int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
// (void)ref;
// ai->to_host.ref.id = AI_ID_REF;
// ai->to_host.ref.package.crc16 = CRC16_Calc(
// (const uint8_t *)&(ai->to_host.ref.package),
// sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
// return DEVICE_OK;
// }
// int8_t AI_StartSend(AI_t *ai, bool ref_update) {
// if (ref_update) {
// if (HAL_UART_Transmit_DMA(
// BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
// sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
// return DEVICE_OK;
// else
// return DEVICE_ERR;
// } else {
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
// (uint8_t *)&(ai->to_host.mcu),
// sizeof(ai->to_host.mcu)) == HAL_OK)
// return DEVICE_OK;
// else
// return DEVICE_ERR;
// }
// }

97
User/device/ai.h Normal file
View File

@ -0,0 +1,97 @@
/*
AI
*/
#pragma once
#include <sys/cdefs.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/user_math.h"
#include "device/device.h"
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 电控 -> 视觉 MCU数据结构体*/
typedef struct __packed {
AHRS_Quaternion_t quat; /* 四元数 */
uint8_t notice; /* 控制命令 */
} AI_Protucol_UpDataMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/
typedef struct __packed {
uint16_t team; /* 本身队伍 */
uint16_t time; /* 比赛开始时间 */
} AI_Protocol_UpDataReferee_t;
/* 电控 -> 视觉 数据包结构体*/
typedef struct __packed {
AHRS_Eulr_t eulr; /* 欧拉角 */
MoveVector_t move_vec; /* 运动向量 */
uint8_t notice; /* 控制命令 */
} AI_Protocol_DownData_t;
/* 电控 -> 视觉 裁判系统数据包 */
typedef struct __packed {
uint8_t id; /* 包ID */
AI_Protocol_UpDataReferee_t package; /* 数据包 */
uint16_t crc16; /* CRC16校验 */
} AI_UpPackageReferee_t;
/* 电控 -> 视觉 MUC数据包 */
typedef struct __packed {
uint8_t id;
AI_Protucol_UpDataMCU_t package;
uint16_t crc16;
} AI_UpPackageMCU_t;
/* 视觉 -> 电控 数据包 */
typedef struct __packed {
uint8_t id; /* 包ID */
AI_Protucol_DownData_t package; /* 数据包 */
uint16_t crc16; /* CRC16校验 */
} AI_DownPackage_t;
typedef struct __packed {
DEVICE_Header_t header; /* 设备通用头部 */
AI_DownPackage_t from_host;
struct {
AI_UpPackageReferee_t ref;
AI_UpPackageMCU_t mcu;
} to_host;
} AI_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t AI_Init(AI_t *ai);
int8_t AI_Restart(AI_t *ai);
int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(uint32_t timeout);
int8_t AI_ParseHost(AI_t *ai);
int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data);
int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
int8_t AI_HandleOffline(AI_t *ai);
int8_t AI_StartSend(AI_t *ai, bool ref_online);
#ifdef __cplusplus
}
#endif

271
User/device/dm_imu.c Normal file
View File

@ -0,0 +1,271 @@
/*
DM_IMU数据获取（CAN）
*/
/* Includes ----------------------------------------------------------------- */
#include "dm_imu.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include <string.h>
/* Private define ----------------------------------------------------------- */
#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms
#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 TEMP_MIN (0.0f)
#define TEMP_MAX (60.0f)
#define Quaternion_MIN (-1.0f)
#define Quaternion_MAX (1.0f)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function --------------------------------------------------------- */
static uint8_t count = 0; // 计数器，用于判断设备是否离线
/**
* @brief: 无符号整数转换为浮点数函数
*/
static float uint_to_float(int 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 解析加速度计数据
*/
static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
int8_t temp = data[1];
uint16_t acc_x_raw = (data[3] << 8) | data[2];
uint16_t acc_y_raw = (data[5] << 8) | data[4];
uint16_t acc_z_raw = (data[7] << 8) | data[6];
imu->data.temp = (float)temp;
imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
return DEVICE_OK;
}
/**
* @brief 解析陀螺仪数据
*/
static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
uint16_t gyro_x_raw = (data[3] << 8) | data[2];
uint16_t gyro_y_raw = (data[5] << 8) | data[4];
uint16_t gyro_z_raw = (data[7] << 8) | data[6];
imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
return DEVICE_OK;
}
/**
* @brief 解析欧拉角数据
*/
static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
uint16_t pit_raw = (data[3] << 8) | data[2];
uint16_t yaw_raw = (data[5] << 8) | data[4];
uint16_t rol_raw = (data[7] << 8) | data[6];
imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
return DEVICE_OK;
}
/**
* @brief 解析四元数数据
*/
static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
int z = ((data[6] & 0x3F) << 8) | data[7];
imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化DM IMU设备
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
if (imu == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
// 初始化设备头部
imu->header.online = false;
imu->header.last_online_time = 0;
// 配置参数
imu->param.can = param->can;
imu->param.can_id = param->can_id;
imu->param.device_id = param->device_id;
imu->param.master_id = param->master_id;
// 清零数据
memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
// 注册CAN接收队列，用于接收回复报文
int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
if (result != BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 请求IMU数据
*/
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
// 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
uint8_t tx_data[4] = {
imu->param.device_id & 0xFF, // id_L
(imu->param.device_id >> 8) & 0xFF, // id_H
(uint8_t)rid, // RID
0xCC // 固定值
};
// 发送标准数据帧
BSP_CAN_StdDataFrame_t frame = {
.id = imu->param.can_id,
.dlc = 4,
};
memcpy(frame.data, tx_data, 4);
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
*/
int8_t DM_IMU_Update(DM_IMU_t *imu) {
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
BSP_CAN_Message_t msg;
int8_t result;
bool data_received = false;
// 持续接收所有可用消息
while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
// 验证回复数据格式（至少检查数据长度）
if (msg.dlc < 3) {
continue; // 跳过无效消息
}
// 根据数据位的第0位确定反馈报文类型
uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
// 根据RID类型解析数据
int8_t parse_result = DEVICE_ERR;
switch (rid) {
case 0x01: // RID_ACCL
parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
break;
case 0x02: // RID_GYRO
parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
break;
case 0x03: // RID_EULER
parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
break;
case 0x04: // RID_QUATERNION
parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
break;
default:
continue; // 跳过未知类型的消息
}
// 如果解析成功，标记为收到数据
if (parse_result == DEVICE_OK) {
data_received = true;
}
}
// 如果收到任何有效数据，更新设备状态
if (data_received) {
imu->header.online = true;
imu->header.last_online_time = BSP_TIME_Get_ms();
return DEVICE_OK;
}
return DEVICE_ERR;
}
/**
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
*/
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
switch (count) {
case 0:
DM_IMU_Request(imu, RID_ACCL);
break;
case 1:
DM_IMU_Request(imu, RID_GYRO);
break;
case 2:
DM_IMU_Request(imu, RID_EULER);
break;
case 3:
DM_IMU_Request(imu, RID_QUATERNION);
DM_IMU_Update(imu); // 更新所有数据
break;
}
count++;
if (count >= 4) {
count = 0; // 重置计数器
return DEVICE_OK;
}
return DEVICE_ERR;
}
/**
* @brief 检查设备是否在线
*/
bool DM_IMU_IsOnline(DM_IMU_t *imu) {
if (imu == NULL) {
return false;
}
uint32_t current_time = BSP_TIME_Get_ms();
return imu->header.online &&
(current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
}

90
User/device/dm_imu.h Normal file
View File

@ -0,0 +1,90 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "component/ahrs.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define DM_IMU_CAN_ID_DEFAULT 0x6FF
#define DM_IMU_ID_DEFAULT 0x42
#define DM_IMU_MST_ID_DEFAULT 0x43
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
BSP_CAN_t can; // CAN总线句柄
uint16_t can_id; // CAN通信ID
uint8_t device_id; // 设备ID
uint8_t master_id; // 主机ID
} DM_IMU_Param_t;
typedef enum {
RID_ACCL = 0x01, // 加速度计
RID_GYRO = 0x02, // 陀螺仪
RID_EULER = 0x03, // 欧拉角
RID_QUATERNION = 0x04, // 四元数
} DM_IMU_RID_t;
typedef struct {
AHRS_Accl_t accl; // 加速度计
AHRS_Gyro_t gyro; // 陀螺仪
AHRS_Eulr_t euler; // 欧拉角
AHRS_Quaternion_t quat; // 四元数
float temp; // 温度
} DM_IMU_Data_t;
typedef struct {
DEVICE_Header_t header;
DM_IMU_Param_t param; // IMU参数配置
DM_IMU_Data_t data; // IMU数据
} DM_IMU_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化DM IMU设备
* @param imu DM IMU设备结构体指针
* @param param IMU参数配置指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
/**
* @brief 请求IMU数据
* @param imu DM IMU设备结构体指针
* @param rid 请求的数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
/**
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
* @param imu DM IMU设备结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Update(DM_IMU_t *imu);
/**
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
* @param imu DM IMU设备结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
/**
* @brief 检查设备是否在线
* @param imu DM IMU设备结构体指针
* @return true 在线，false 离线
*/
bool DM_IMU_IsOnline(DM_IMU_t *imu);
#ifdef __cplusplus
}
#endif

6
User/device/motor_lz.h
View File

@ -111,12 +111,6 @@ typedef struct {
*/
int8_t MOTOR_LZ_Init(void);
/**
* @brief 反初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_DeInit(void);
/**
* @brief 注册一个灵足电机
* @param param 电机参数