diff --git a/CMakeLists.txt b/CMakeLists.txt index 16dcd0a..3b8e9f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h index fee994f..16c89ac 100644 --- a/Core/Inc/FreeRTOSConfig.h +++ b/Core/Inc/FreeRTOSConfig.h @@ -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 */ diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml index 00edef2..752688e 100644 --- a/User/component/component_config.yaml +++ b/User/component/component_config.yaml @@ -6,6 +6,12 @@ cmd: dependencies: - component/ahrs enabled: true +crc16: + dependencies: [] + enabled: true +crc8: + dependencies: [] + enabled: true filter: dependencies: - component/ahrs diff --git a/User/component/crc16.c b/User/component/crc16.c new file mode 100644 index 0000000..0d17eb0 --- /dev/null +++ b/User/component/crc16.c @@ -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 */ diff --git a/User/component/crc16.h b/User/component/crc16.h new file mode 100644 index 0000000..68b0a87 --- /dev/null +++ b/User/component/crc16.h @@ -0,0 +1,30 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#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 diff --git a/User/component/crc8.c b/User/component/crc8.c new file mode 100644 index 0000000..66f4ad2 --- /dev/null +++ b/User/component/crc8.c @@ -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 */ diff --git a/User/component/crc8.h b/User/component/crc8.h new file mode 100644 index 0000000..a376c71 --- /dev/null +++ b/User/component/crc8.h @@ -0,0 +1,30 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +/* 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 diff --git a/User/component/user_math.c b/User/component/user_math.c index 49a4723..5e0b0c4 100644 --- a/User/component/user_math.c +++ b/User/component/user_math.c @@ -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 被操作的值变化范围，正数时起效 diff --git a/User/component/user_math.h b/User/component/user_math.h index 21e1396..6e61ca2 100644 --- a/User/component/user_math.h +++ b/User/component/user_math.h @@ -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 被操作的值变化范围，正数时起效 diff --git a/User/device/ai.c b/User/device/ai.c new file mode 100644 index 0000000..edea20c --- /dev/null +++ b/User/device/ai.c @@ -0,0 +1,152 @@ +// /* +// AI +// */ + +// /* Includes ----------------------------------------------------------------- */ +// #include "ai.h" + +// #include + +// #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; +// } +// } + diff --git a/User/device/ai.h b/User/device/ai.h new file mode 100644 index 0000000..67c4ab4 --- /dev/null +++ b/User/device/ai.h @@ -0,0 +1,97 @@ +/* + AI +*/ + +#pragma once + +#include +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "component/ahrs.h" +#include "component/filter.h" +#include "component/user_math.h" +#include "device/device.h" +#include +#include +#include + +/* 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 diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c new file mode 100644 index 0000000..e5380d9 --- /dev/null +++ b/User/device/dm_imu.c @@ -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 + +/* 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<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); +} diff --git a/User/device/dm_imu.h b/User/device/dm_imu.h new file mode 100644 index 0000000..3965980 --- /dev/null +++ b/User/device/dm_imu.h @@ -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 diff --git a/User/device/motor_lz.h b/User/device/motor_lz.h index 76a72ae..d2a8002 100644 --- a/User/device/motor_lz.h +++ b/User/device/motor_lz.h @@ -111,12 +111,6 @@ typedef struct { */ int8_t MOTOR_LZ_Init(void); -/** - * @brief 反初始化灵足电机驱动系统 - * @return 设备状态码 - */ -int8_t MOTOR_LZ_DeInit(void); - /** * @brief 注册一个灵足电机 * @param param 电机参数