diff --git a/.DS_Store b/.DS_Store index ec05ad8..d7f26ce 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/assets/.DS_Store b/assets/.DS_Store new file mode 100644 index 0000000..ea98324 Binary files /dev/null and b/assets/.DS_Store differ diff --git a/assets/Finance_Data/.DS_Store b/assets/Finance_Data/.DS_Store new file mode 100644 index 0000000..ad9e73c Binary files /dev/null and b/assets/Finance_Data/.DS_Store differ diff --git a/assets/User_code/.DS_Store b/assets/User_code/.DS_Store new file mode 100644 index 0000000..aa72674 Binary files /dev/null and b/assets/User_code/.DS_Store differ diff --git a/assets/User_code/bsp/.DS_Store b/assets/User_code/bsp/.DS_Store new file mode 100644 index 0000000..ec9a612 Binary files /dev/null and b/assets/User_code/bsp/.DS_Store differ diff --git a/assets/User_code/bsp/.gitkeep b/assets/User_code/bsp/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/assets/User_code/bsp/bsp.h b/assets/User_code/bsp/bsp.h new file mode 100644 index 0000000..1f8cbfd --- /dev/null +++ b/assets/User_code/bsp/bsp.h @@ -0,0 +1,28 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define BSP_OK (0) +#define BSP_ERR (-1) +#define BSP_ERR_NULL (-2) +#define BSP_ERR_INITED (-3) +#define BSP_ERR_NO_DEV (-4) +#define BSP_ERR_TIMEOUT (-5) + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/bsp/can.c b/assets/User_code/bsp/can.c new file mode 100644 index 0000000..c2fe26b --- /dev/null +++ b/assets/User_code/bsp/can.c @@ -0,0 +1,659 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/can.h" +#include "bsp/bsp.h" +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */ +#define CAN_TX_MAILBOX_NUM 3 /* CAN发送邮箱数量 */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct BSP_CAN_QueueNode { + BSP_CAN_t can; /* CAN通道 */ + uint32_t can_id; /* 解析后的CAN ID */ + osMessageQueueId_t queue; /* 消息队列ID */ + uint8_t queue_size; /* 队列大小 */ + struct BSP_CAN_QueueNode *next; /* 指向下一个节点的指针 */ +} BSP_CAN_QueueNode_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static BSP_CAN_QueueNode_t *queue_list = NULL; +static osMutexId_t queue_mutex = NULL; +static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void); +static bool inited = false; +static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */ +static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */ + +/* Private function prototypes ---------------------------------------------- */ +static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan); +static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id); +static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); +static void BSP_CAN_RxFifo0Callback(void); +static void BSP_CAN_RxFifo1Callback(void); +static void BSP_CAN_TxCompleteCallback(void); +static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header); +static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); +static void BSP_CAN_TxQueueInit(BSP_CAN_t can); +static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg); +static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg); +static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can); + +/* Private functions -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/** + * @brief 根据CAN句柄获取BSP_CAN实例 + */ +static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) { + if (hcan == NULL) return BSP_CAN_ERR; + +/* AUTO GENERATED CAN_GET */ + else + return BSP_CAN_ERR; +} + +/** + * @brief 查找指定CAN ID的消息队列 + * @note 调用前需要获取互斥锁 + */ +static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id) { + BSP_CAN_QueueNode_t *node = queue_list; + while (node != NULL) { + if (node->can == can && node->can_id == can_id) { + return node->queue; + } + node = node->next; + } + return NULL; +} + +/** + * @brief 创建指定CAN ID的消息队列 + * @note 内部函数，已包含互斥锁保护 + */ +static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) { + if (queue_size == 0) { + queue_size = BSP_CAN_DEFAULT_QUEUE_SIZE; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + BSP_CAN_QueueNode_t *node = queue_list; + while (node != NULL) { + if (node->can == can && node->can_id == can_id) { + osMutexRelease(queue_mutex); + return BSP_ERR; // 已存在 + } + node = node->next; + } + BSP_CAN_QueueNode_t *new_node = (BSP_CAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_CAN_QueueNode_t)); + if (new_node == NULL) { + osMutexRelease(queue_mutex); + return BSP_ERR_NULL; + } + new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_CAN_Message_t), NULL); + if (new_node->queue == NULL) { + BSP_Free(new_node); + osMutexRelease(queue_mutex); + return BSP_ERR; + } + new_node->can = can; + new_node->can_id = can_id; + new_node->queue_size = queue_size; + new_node->next = queue_list; + queue_list = new_node; + osMutexRelease(queue_mutex); + return BSP_OK; +} + + +/** + * @brief 获取帧类型 + */ +static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header) { + if (header->RTR == CAN_RTR_REMOTE) { + return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_REMOTE : BSP_CAN_FRAME_STD_REMOTE; + } else { + return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_DATA : BSP_CAN_FRAME_STD_DATA; + } +} + +/** + * @brief 默认ID解析器（直接返回原始ID） + */ +static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { + (void)frame_type; // 避免未使用参数警告 + return original_id; +} + +/** + * @brief 初始化发送队列 + */ +static void BSP_CAN_TxQueueInit(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return; + + tx_queues[can].head = 0; + tx_queues[can].tail = 0; +} + +/** + * @brief 向发送队列添加消息（无锁） + */ +static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) { + if (can >= BSP_CAN_NUM || msg == NULL) return false; + + BSP_CAN_TxQueue_t *queue = &tx_queues[can]; + uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE; + + // 队列满 + if (next_head == queue->tail) { + return false; + } + + // 复制消息 + queue->buffer[queue->head] = *msg; + + // 更新头指针（原子操作） + queue->head = next_head; + + return true; +} + + +/** + * @brief 从发送队列取出消息（无锁） + */ +static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) { + if (can >= BSP_CAN_NUM || msg == NULL) return false; + + BSP_CAN_TxQueue_t *queue = &tx_queues[can]; + + // 队列空 + if (queue->head == queue->tail) { + return false; + } + + // 复制消息 + *msg = queue->buffer[queue->tail]; + + // 更新尾指针（原子操作） + queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE; + + return true; +} + +/** + * @brief 检查发送队列是否为空 + */ +static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return true; + + return tx_queues[can].head == tx_queues[can].tail; +} + +/** + * @brief 处理所有CAN实例的发送队列 + */ +static void BSP_CAN_TxCompleteCallback(void) { + // 处理所有CAN实例的发送队列 + for (int i = 0; i < BSP_CAN_NUM; i++) { + BSP_CAN_t can = (BSP_CAN_t)i; + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); + if (hcan == NULL) continue; + + BSP_CAN_TxMessage_t msg; + uint32_t mailbox; + + // 尝试发送队列中的消息 + while (!BSP_CAN_TxQueueIsEmpty(can)) { + // 检查是否有空闲邮箱 + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { + break; // 没有空闲邮箱，等待下次中断 + } + + // 从队列中取出消息 + if (!BSP_CAN_TxQueuePop(can, &msg)) { + break; + } + + // 发送消息 + if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) { + // 发送失败，消息已经从队列中移除，直接丢弃 + break; + } + } + } +} + + +/** + * @brief FIFO0接收处理函数 + */ +static void BSP_CAN_RxFifo0Callback(void) { + CAN_RxHeaderTypeDef rx_header; + uint8_t rx_data[BSP_CAN_MAX_DLC]; + for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) { + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx); + if (hcan == NULL) continue; + while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) { + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) { + uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId; + BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header); + uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type); + osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id); + if (queue != NULL) { + BSP_CAN_Message_t msg = {0}; + msg.frame_type = frame_type; + msg.original_id = original_id; + msg.parsed_id = parsed_id; + msg.dlc = rx_header.DLC; + if (rx_header.RTR == CAN_RTR_DATA) { + memcpy(msg.data, rx_data, rx_header.DLC); + } + msg.timestamp = HAL_GetTick(); + osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE); + } + } + } + } +} + +/** + * @brief FIFO1接收处理函数 + */ +static void BSP_CAN_RxFifo1Callback(void) { + CAN_RxHeaderTypeDef rx_header; + uint8_t rx_data[BSP_CAN_MAX_DLC]; + for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) { + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx); + if (hcan == NULL) continue; + while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) { + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) { + uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId; + BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header); + uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type); + osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id); + if (queue != NULL) { + BSP_CAN_Message_t msg = {0}; + msg.frame_type = frame_type; + msg.original_id = original_id; + msg.parsed_id = parsed_id; + msg.dlc = rx_header.DLC; + if (rx_header.RTR == CAN_RTR_DATA) { + memcpy(msg.data, rx_data, rx_header.DLC); + } + msg.timestamp = HAL_GetTick(); + osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE); + } + } + } + } +} + +/* HAL Callback Functions --------------------------------------------------- */ +void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB](); + } +} + +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB](); + } +} + +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB](); + } +} + +void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB](); + } +} + +void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB](); + } +} + +void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB](); + } +} + +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB](); + } +} + +void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB](); + } +} + +void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB](); + } +} + +void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB](); + } +} + +void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB]) + CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB](); + } +} + +void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB]) + CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB](); + } +} + +void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_ERROR_CB]) + CAN_Callback[bsp_can][HAL_CAN_ERROR_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t BSP_CAN_Init(void) { + if (inited) { + return BSP_ERR_INITED; + } + + // 清零回调函数数组 + memset(CAN_Callback, 0, sizeof(CAN_Callback)); + + // 初始化发送队列 + for (int i = 0; i < BSP_CAN_NUM; i++) { + BSP_CAN_TxQueueInit((BSP_CAN_t)i); + } + + // 初始化ID解析器为默认解析器 + id_parser = BSP_CAN_DefaultIdParser; + + // 创建互斥锁 + queue_mutex = osMutexNew(NULL); + if (queue_mutex == NULL) { + return BSP_ERR; + } + +/* AUTO GENERATED CAN_INIT */ + + inited = true; + return BSP_OK; +} + + +CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) { + return NULL; + } + + switch (can) { +/* AUTO GENERATED BSP_CAN_GET_HANDLE */ + default: + return NULL; + } +} + +int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type, + void (*callback)(void)) { + if (!inited) { + return BSP_ERR_INITED; + } + if (callback == NULL) { + return BSP_ERR_NULL; + } + if (can >= BSP_CAN_NUM) { + return BSP_ERR; + } + if (type >= BSP_CAN_CB_NUM) { + return BSP_ERR; + } + + CAN_Callback[can][type] = callback; + return BSP_OK; +} + +int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, + uint32_t id, uint8_t *data, uint8_t dlc) { + if (!inited) { + return BSP_ERR_INITED; + } + if (can >= BSP_CAN_NUM) { + return BSP_ERR; + } + if (data == NULL && format != BSP_CAN_FORMAT_STD_REMOTE && format != BSP_CAN_FORMAT_EXT_REMOTE) { + return BSP_ERR_NULL; + } + if (dlc > BSP_CAN_MAX_DLC) { + return BSP_ERR; + } + + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); + if (hcan == NULL) { + return BSP_ERR_NULL; + } + + // 准备发送消息 + BSP_CAN_TxMessage_t tx_msg = {0}; + + switch (format) { + case BSP_CAN_FORMAT_STD_DATA: + tx_msg.header.StdId = id; + tx_msg.header.IDE = CAN_ID_STD; + tx_msg.header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_EXT_DATA: + tx_msg.header.ExtId = id; + tx_msg.header.IDE = CAN_ID_EXT; + tx_msg.header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_STD_REMOTE: + tx_msg.header.StdId = id; + tx_msg.header.IDE = CAN_ID_STD; + tx_msg.header.RTR = CAN_RTR_REMOTE; + break; + case BSP_CAN_FORMAT_EXT_REMOTE: + tx_msg.header.ExtId = id; + tx_msg.header.IDE = CAN_ID_EXT; + tx_msg.header.RTR = CAN_RTR_REMOTE; + break; + default: + return BSP_ERR; + } + + tx_msg.header.DLC = dlc; + tx_msg.header.TransmitGlobalTime = DISABLE; + + // 复制数据 + if (data != NULL && dlc > 0) { + memcpy(tx_msg.data, data, dlc); + } + + // 尝试直接发送到邮箱 + uint32_t mailbox; + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { + HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox); + if (result == HAL_OK) { + return BSP_OK; // 发送成功 + } + } + + // 邮箱满，尝试放入队列 + if (BSP_CAN_TxQueuePush(can, &tx_msg)) { + return BSP_OK; // 成功放入队列 + } + + // 队列也满，丢弃数据 + return BSP_ERR; // 数据丢弃 +} + +int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) { + if (frame == NULL) { + return BSP_ERR_NULL; + } + return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc); +} + +int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame) { + if (frame == NULL) { + return BSP_ERR_NULL; + } + return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc); +} + +int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame) { + if (frame == NULL) { + return BSP_ERR_NULL; + } + BSP_CAN_Format_t format = frame->is_extended ? BSP_CAN_FORMAT_EXT_REMOTE : BSP_CAN_FORMAT_STD_REMOTE; + return BSP_CAN_Transmit(can, format, frame->id, NULL, frame->dlc); +} + +int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) { + if (!inited) { + return BSP_ERR_INITED; + } + return BSP_CAN_CreateIdQueue(can, can_id, queue_size); +} + + +int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) { + if (!inited) { + return BSP_ERR_INITED; + } + if (msg == NULL) { + return BSP_ERR_NULL; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) { + return BSP_ERR_NO_DEV; + } + osStatus_t result = osMessageQueueGet(queue, msg, NULL, timeout); + return (result == osOK) ? BSP_OK : BSP_ERR; +} + +int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id) { + if (!inited) { + return -1; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return -1; + } + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) { + return -1; + } + return (int32_t)osMessageQueueGetCount(queue); +} + +int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id) { + if (!inited) { + return BSP_ERR_INITED; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) { + return BSP_ERR_NO_DEV; + } + BSP_CAN_Message_t temp_msg; + while (osMessageQueueGet(queue, &temp_msg, NULL, BSP_CAN_TIMEOUT_IMMEDIATE) == osOK) { + // 清空 + } + return BSP_OK; +} + +int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) { + if (!inited) { + return BSP_ERR_INITED; + } + if (parser == NULL) { + return BSP_ERR_NULL; + } + + id_parser = parser; + return BSP_OK; +} + +uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { + if (id_parser != NULL) { + return id_parser(original_id, frame_type); + } + return BSP_CAN_DefaultIdParser(original_id, frame_type); +} + + diff --git a/assets/User_code/bsp/can.h b/assets/User_code/bsp/can.h new file mode 100644 index 0000000..e6b5f71 --- /dev/null +++ b/assets/User_code/bsp/can.h @@ -0,0 +1,259 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/bsp.h" +#include "bsp/mm.h" +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +#define BSP_CAN_MAX_DLC 8 +#define BSP_CAN_DEFAULT_QUEUE_SIZE 10 +#define BSP_CAN_TIMEOUT_IMMEDIATE 0 +#define BSP_CAN_TIMEOUT_FOREVER osWaitForever +#define BSP_CAN_TX_QUEUE_SIZE 32 /* 发送队列大小 */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + BSP_CAN_1, + BSP_CAN_2, + BSP_CAN_NUM, + BSP_CAN_ERR, +} BSP_CAN_t; + +typedef enum { + HAL_CAN_TX_MAILBOX0_CPLT_CB, + HAL_CAN_TX_MAILBOX1_CPLT_CB, + HAL_CAN_TX_MAILBOX2_CPLT_CB, + HAL_CAN_TX_MAILBOX0_ABORT_CB, + HAL_CAN_TX_MAILBOX1_ABORT_CB, + HAL_CAN_TX_MAILBOX2_ABORT_CB, + HAL_CAN_RX_FIFO0_MSG_PENDING_CB, + HAL_CAN_RX_FIFO0_FULL_CB, + HAL_CAN_RX_FIFO1_MSG_PENDING_CB, + HAL_CAN_RX_FIFO1_FULL_CB, + HAL_CAN_SLEEP_CB, + HAL_CAN_WAKEUP_FROM_RX_MSG_CB, + HAL_CAN_ERROR_CB, + BSP_CAN_CB_NUM, +} BSP_CAN_Callback_t; + +/* CAN消息格式枚举 - 用于发送和接收消息时指定格式 */ +typedef enum { + BSP_CAN_FORMAT_STD_DATA, /* 标准数据帧 */ + BSP_CAN_FORMAT_EXT_DATA, /* 扩展数据帧 */ + BSP_CAN_FORMAT_STD_REMOTE, /* 标准远程帧 */ + BSP_CAN_FORMAT_EXT_REMOTE, /* 扩展远程帧 */ +} BSP_CAN_Format_t; + +/* CAN帧类型枚举 - 用于区分不同类型的CAN帧 */ +typedef enum { + BSP_CAN_FRAME_STD_DATA, /* 标准数据帧 */ + BSP_CAN_FRAME_EXT_DATA, /* 扩展数据帧 */ + BSP_CAN_FRAME_STD_REMOTE, /* 标准远程帧 */ + BSP_CAN_FRAME_EXT_REMOTE, /* 扩展远程帧 */ +} BSP_CAN_FrameType_t; + +/* CAN消息结构体 - 支持不同类型帧 */ +typedef struct { + BSP_CAN_FrameType_t frame_type; /* 帧类型 */ + uint32_t original_id; /* 原始ID（未解析） */ + uint32_t parsed_id; /* 解析后的实际ID */ + uint8_t dlc; /* 数据长度 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ + uint32_t timestamp; /* 时间戳（可选） */ +} BSP_CAN_Message_t; + +/* 标准数据帧结构 */ +typedef struct { + uint32_t id; /* CAN ID */ + uint8_t dlc; /* 数据长度 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ +} BSP_CAN_StdDataFrame_t; + +/* 扩展数据帧结构 */ +typedef struct { + uint32_t id; /* 扩展CAN ID */ + uint8_t dlc; /* 数据长度 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ +} BSP_CAN_ExtDataFrame_t; + +/* 远程帧结构 */ +typedef struct { + uint32_t id; /* CAN ID */ + uint8_t dlc; /* 请求的数据长度 */ + bool is_extended; /* 是否为扩展帧 */ +} BSP_CAN_RemoteFrame_t; + +/* ID解析回调函数类型 */ +typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type); + +/* CAN发送消息结构体 */ +typedef struct { + CAN_TxHeaderTypeDef header; /* 发送头 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ +} BSP_CAN_TxMessage_t; + +/* 无锁环形队列结构体 */ +typedef struct { + BSP_CAN_TxMessage_t buffer[BSP_CAN_TX_QUEUE_SIZE]; /* 缓冲区 */ + volatile uint32_t head; /* 队列头 */ + volatile uint32_t tail; /* 队列尾 */ +} BSP_CAN_TxQueue_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化 CAN 模块 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_Init(void); + +/** + * @brief 获取 CAN 句柄 + * @param can CAN 枚举 + * @return CAN_HandleTypeDef 指针，失败返回 NULL + */ +CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can); + +/** + * @brief 注册 CAN 回调函数 + * @param can CAN 枚举 + * @param type 回调类型 + * @param callback 回调函数指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type, + void (*callback)(void)); + +/** + * @brief 发送 CAN 消息 + * @param can CAN 枚举 + * @param format 消息格式 + * @param id CAN ID + * @param data 数据指针 + * @param dlc 数据长度 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, + uint32_t id, uint8_t *data, uint8_t dlc); + +/** + * @brief 发送标准数据帧 + * @param can CAN 枚举 + * @param frame 标准数据帧指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame); + +/** + * @brief 发送扩展数据帧 + * @param can CAN 枚举 + * @param frame 扩展数据帧指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame); + +/** + * @brief 发送远程帧 + * @param can CAN 枚举 + * @param frame 远程帧指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame); + + +/** + * @brief 获取发送队列中待发送消息数量 + * @param can CAN 枚举 + * @return 队列中消息数量，-1表示错误 + */ +int32_t BSP_CAN_GetTxQueueCount(BSP_CAN_t can); + +/** + * @brief 清空发送队列 + * @param can CAN 枚举 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can); + +/** + * @brief 注册 CAN ID 接收队列 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @param queue_size 队列大小，0使用默认值 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); + + + +/** + * @brief 获取 CAN 消息 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @param msg 存储消息的结构体指针 + * @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout); + +/** + * @brief 获取指定ID队列中的消息数量 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @return 消息数量，-1表示队列不存在 + */ +int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id); + +/** + * @brief 清空指定ID队列中的所有消息 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id); + +/** + * @brief 注册ID解析器 + * @param parser ID解析回调函数 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser); + + +/** + * @brief 解析CAN ID + * @param original_id 原始ID + * @param frame_type 帧类型 + * @return 解析后的ID + */ +uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/bsp/describe.csv b/assets/User_code/bsp/describe.csv new file mode 100644 index 0000000..5206a52 --- /dev/null +++ b/assets/User_code/bsp/describe.csv @@ -0,0 +1,11 @@ +uart,请开启uart的dma和中断 +can,请开启can中断，使用函数前请确保can已经初始化。一定要开启can发送中断！！！ +gpio,会自动读取cubemx中配置为gpio的引脚，并自动区分输入输出和中断。 +spi,请开启spi的dma和中断 +i2c,要求开始spi中断 +mm,这是套了一层的动态内存分配 +time,获取时间戳函数，需要开启freerots +dwt,需要开启dwt，获取时间 +i2c,请开启i2c的dma和中断 +pwm,用于选择那些勇于输出pwm + diff --git a/assets/User_code/bsp/dwt.c b/assets/User_code/bsp/dwt.c new file mode 100644 index 0000000..c16af9e --- /dev/null +++ b/assets/User_code/bsp/dwt.c @@ -0,0 +1,138 @@ +/** + ****************************************************************************** + * @file dwt.c + * @author Wang Hongxi + * @version V1.1.0 + * @date 2022/3/8 + * @brief + ****************************************************************************** + * @attention + * + ****************************************************************************** + */ +#include "bsp/dwt.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +DWT_Time_t SysTime; +static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us; +static uint32_t CYCCNT_RountCount; +static uint32_t CYCCNT_LAST; +uint64_t CYCCNT64; +static void DWT_CNT_Update(void); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +void DWT_Init(uint32_t CPU_Freq_mHz) +{ + /* 使能DWT外设 */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + /* DWT CYCCNT寄存器计数清0 */ + DWT->CYCCNT = (uint32_t)0u; + + /* 使能Cortex-M DWT CYCCNT寄存器 */ + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + + CPU_FREQ_Hz = CPU_Freq_mHz * 1000000; + CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000; + CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000; + CYCCNT_RountCount = 0; +} + +float DWT_GetDeltaT(uint32_t *cnt_last) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + float dt = ((uint32_t)(cnt_now - *cnt_last)) / ((float)(CPU_FREQ_Hz)); + *cnt_last = cnt_now; + + DWT_CNT_Update(); + + return dt; +} + +double DWT_GetDeltaT64(uint32_t *cnt_last) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + double dt = ((uint32_t)(cnt_now - *cnt_last)) / ((double)(CPU_FREQ_Hz)); + *cnt_last = cnt_now; + + DWT_CNT_Update(); + + return dt; +} + +void DWT_SysTimeUpdate(void) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3; + + DWT_CNT_Update(); + + CYCCNT64 = (uint64_t)CYCCNT_RountCount * (uint64_t)UINT32_MAX + (uint64_t)cnt_now; + CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz; + CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz; + SysTime.s = CNT_TEMP1; + SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms; + CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms; + SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us; +} + +float DWT_GetTimeline_s(void) +{ + DWT_SysTimeUpdate(); + + float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f; + + return DWT_Timelinef32; +} + +float DWT_GetTimeline_ms(void) +{ + DWT_SysTimeUpdate(); + + float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f; + + return DWT_Timelinef32; +} + +uint64_t DWT_GetTimeline_us(void) +{ + DWT_SysTimeUpdate(); + + uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us; + + return DWT_Timelinef32; +} + +static void DWT_CNT_Update(void) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + + if (cnt_now < CYCCNT_LAST) + CYCCNT_RountCount++; + + CYCCNT_LAST = cnt_now; +} + +void DWT_Delay(float Delay) +{ + uint32_t tickstart = DWT->CYCCNT; + float wait = Delay; + + while ((DWT->CYCCNT - tickstart) < wait * (float)CPU_FREQ_Hz) + { + } +} diff --git a/assets/User_code/bsp/dwt.h b/assets/User_code/bsp/dwt.h new file mode 100644 index 0000000..65a731e --- /dev/null +++ b/assets/User_code/bsp/dwt.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @file dwt.h + * @author Wang Hongxi + * @version V1.1.0 + * @date 2022/3/8 + * @brief + ****************************************************************************** + * @attention + * + ****************************************************************************** + */ +#ifndef _DWT_H +#define _DWT_H + +#include "main.h" +#include "stdint.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +typedef struct +{ + uint32_t s; + uint16_t ms; + uint16_t us; +} DWT_Time_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +void DWT_Init(uint32_t CPU_Freq_mHz); +float DWT_GetDeltaT(uint32_t *cnt_last); +double DWT_GetDeltaT64(uint32_t *cnt_last); +float DWT_GetTimeline_s(void); +float DWT_GetTimeline_ms(void); +uint64_t DWT_GetTimeline_us(void); +void DWT_Delay(float Delay); +void DWT_SysTimeUpdate(void); + +extern DWT_Time_t SysTime; + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#endif /* DWT_H_ */ diff --git a/assets/User_code/bsp/gpio.c b/assets/User_code/bsp/gpio.c new file mode 100644 index 0000000..5c3113c --- /dev/null +++ b/assets/User_code/bsp/gpio.c @@ -0,0 +1,98 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/gpio.h" + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + uint16_t pin; + GPIO_TypeDef *gpio; +} BSP_GPIO_MAP_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = { +/* AUTO GENERATED BSP_GPIO_MAP */ +}; + +static void (*GPIO_Callback[16])(void); + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + for (uint8_t i = 0; i < 16; i++) { + if (GPIO_Pin & (1 << i)) { + if (GPIO_Callback[i]) { + GPIO_Callback[i](); + } + } + } +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + + // 从GPIO映射中获取对应的pin值 + uint16_t pin = GPIO_Map[gpio].pin; + + for (uint8_t i = 0; i < 16; i++) { + if (pin & (1 << i)) { + GPIO_Callback[i] = callback; + break; + } + } + return BSP_OK; +} + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { +/* AUTO GENERATED BSP_GPIO_ENABLE_IRQ */ + default: + return BSP_ERR; + } + return BSP_OK; +} + +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { +/* AUTO GENERATED BSP_GPIO_DISABLE_IRQ */ + default: + return BSP_ERR; + } + return BSP_OK; +} +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value); + return BSP_OK; +} + +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin); + return BSP_OK; +} + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return false; + return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET; +} \ No newline at end of file diff --git a/assets/User_code/bsp/gpio.h b/assets/User_code/bsp/gpio.h new file mode 100644 index 0000000..0f30a9b --- /dev/null +++ b/assets/User_code/bsp/gpio.h @@ -0,0 +1,47 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +typedef enum { +/* AUTO GENERATED BSP_GPIO_ENUM */ + BSP_GPIO_NUM, + BSP_GPIO_ERR, +} BSP_GPIO_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)); + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio); +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio); + +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value); +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio); + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/bsp/i2c.c b/assets/User_code/bsp/i2c.c new file mode 100644 index 0000000..6120f59 --- /dev/null +++ b/assets/User_code/bsp/i2c.c @@ -0,0 +1,188 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp\i2c.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*I2C_Callback[BSP_I2C_NUM][BSP_I2C_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_I2C_t I2C_Get(I2C_HandleTypeDef *hi2c) { +/* AUTO GENERATED I2C_GET */ + else + return BSP_I2C_ERR; +} + +void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_MASTER_TX_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_MASTER_TX_CPLT_CB](); + } +} + +void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_MASTER_RX_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_MASTER_RX_CPLT_CB](); + } +} + +void HAL_I2C_SlaveTxCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_TX_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_TX_CPLT_CB](); + } +} + +void HAL_I2C_SlaveRxCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_RX_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_RX_CPLT_CB](); + } +} + +void HAL_I2C_ListenCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_LISTEN_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_LISTEN_CPLT_CB](); + } +} + +void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_MEM_TX_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_MEM_TX_CPLT_CB](); + } +} + +void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_MEM_RX_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_MEM_RX_CPLT_CB](); + } +} + +void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_ERROR_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_ERROR_CB](); + } +} + +void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c) { + BSP_I2C_t bsp_i2c = I2C_Get(hi2c); + if (bsp_i2c != BSP_I2C_ERR) { + if (I2C_Callback[bsp_i2c][HAL_I2C_ABORT_CPLT_CB]) + I2C_Callback[bsp_i2c][HAL_I2C_ABORT_CPLT_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ +I2C_HandleTypeDef *BSP_I2C_GetHandle(BSP_I2C_t i2c) { + switch (i2c) { +/* AUTO GENERATED BSP_I2C_GET_HANDLE */ + default: + return NULL; + } +} + +int8_t BSP_I2C_RegisterCallback(BSP_I2C_t i2c, BSP_I2C_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + I2C_Callback[i2c][type] = callback; + return BSP_OK; +} + +int8_t BSP_I2C_Transmit(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data, + uint16_t size, bool dma) { + if (i2c >= BSP_I2C_NUM) return BSP_ERR; + I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c); + if (hi2c == NULL) return BSP_ERR; + + if (dma) { + return HAL_I2C_Master_Transmit_DMA(hi2c, devAddr, data, size); + } else { + return HAL_I2C_Master_Transmit(hi2c, devAddr, data, size, 10); + } +} + +int8_t BSP_I2C_Receive(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data, + uint16_t size, bool dma) { + if (i2c >= BSP_I2C_NUM) return BSP_ERR; + I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c); + if (hi2c == NULL) return BSP_ERR; + + if (dma) { + return HAL_I2C_Master_Receive_DMA(hi2c, devAddr, data, size); + } else { + return HAL_I2C_Master_Receive(hi2c, devAddr, data, size, 10); + } +} + +uint8_t BSP_I2C_MemReadByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr) { + if (i2c >= BSP_I2C_NUM) return 0xFF; + I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c); + if (hi2c == NULL) return 0xFF; + + uint8_t data; + HAL_I2C_Mem_Read(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, &data, 1, HAL_MAX_DELAY); + return data; +} + +int8_t BSP_I2C_MemWriteByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr, + uint8_t data) { + if (i2c >= BSP_I2C_NUM) return BSP_ERR; + I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c); + if (hi2c == NULL) return BSP_ERR; + + return HAL_I2C_Mem_Write(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, &data, 1, HAL_MAX_DELAY); +} + +int8_t BSP_I2C_MemRead(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr, + uint8_t *data, uint16_t size, bool dma) { + if (i2c >= BSP_I2C_NUM || data == NULL || size == 0) return BSP_ERR; + I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c); + if (hi2c == NULL) return BSP_ERR; + + if (dma) { + return HAL_I2C_Mem_Read_DMA(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size); + } + else { + return HAL_I2C_Mem_Read(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size, HAL_MAX_DELAY); + } +} + + +int8_t BSP_I2C_MemWrite(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr, + uint8_t *data, uint16_t size, bool dma) { + if (i2c >= BSP_I2C_NUM || data == NULL || size == 0) return BSP_ERR; + I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c); + if (hi2c == NULL) return BSP_ERR; + + if (dma) { + return HAL_I2C_Mem_Write_DMA(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size); + } else { + return HAL_I2C_Mem_Write(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size, HAL_MAX_DELAY); + } +} \ No newline at end of file diff --git a/assets/User_code/bsp/i2c.h b/assets/User_code/bsp/i2c.h new file mode 100644 index 0000000..79086e7 --- /dev/null +++ b/assets/User_code/bsp/i2c.h @@ -0,0 +1,78 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用I2C的新设备，需要先在此添加对应的枚举值 */ + +/* I2C实体枚举，与设备对应 */ +typedef enum { +/* AUTO GENERATED BSP_I2C_NAME */ + /* USER BSP_I2C BEGIN*/ + /* USER_I2C_XXX */ + /* USER BSP_I2C END */ + BSP_I2C_NUM, + BSP_I2C_ERR, +} BSP_I2C_t; + +/* I2C支持的中断回调函数类型*/ +typedef enum { + HAL_I2C_MASTER_TX_CPLT_CB, + HAL_I2C_MASTER_RX_CPLT_CB, + HAL_I2C_SLAVE_TX_CPLT_CB, + HAL_I2C_SLAVE_RX_CPLT_CB, + HAL_I2C_LISTEN_CPLT_CB, + HAL_I2C_MEM_TX_CPLT_CB, + HAL_I2C_MEM_RX_CPLT_CB, + HAL_I2C_ERROR_CB, + HAL_I2C_ABORT_CPLT_CB, + BSP_I2C_CB_NUM, +} BSP_I2C_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ +I2C_HandleTypeDef *BSP_I2C_GetHandle(BSP_I2C_t i2c); +int8_t BSP_I2C_RegisterCallback(BSP_I2C_t i2c, BSP_I2C_Callback_t type, + void (*callback)(void)); + +int8_t BSP_I2C_Transmit(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data, + uint16_t size, bool dma); +int8_t BSP_I2C_Receive(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data, + uint16_t size, bool dma); + + +uint8_t BSP_I2C_MemReadByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr); +int8_t BSP_I2C_MemWriteByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr, + uint8_t data); + +int8_t BSP_I2C_MemRead(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr, + uint8_t *data, uint16_t size, bool dma); +int8_t BSP_I2C_MemWrite(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr, + uint8_t *data, uint16_t size, bool dma); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/bsp/mm.c b/assets/User_code/bsp/mm.c new file mode 100644 index 0000000..13d20c0 --- /dev/null +++ b/assets/User_code/bsp/mm.c @@ -0,0 +1,30 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/mm.h" + +#include "FreeRTOS.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); } + +inline void BSP_Free(void *pv) { vPortFree(pv); } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/assets/User_code/bsp/mm.h b/assets/User_code/bsp/mm.h new file mode 100644 index 0000000..d24634e --- /dev/null +++ b/assets/User_code/bsp/mm.h @@ -0,0 +1,32 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +void *BSP_Malloc(size_t size); +void BSP_Free(void *pv); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/bsp/pwm.c b/assets/User_code/bsp/pwm.c new file mode 100644 index 0000000..3caedb0 --- /dev/null +++ b/assets/User_code/bsp/pwm.c @@ -0,0 +1,110 @@ +/* Includes ----------------------------------------------------------------- */ +#include "tim.h" +#include "bsp/pwm.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + TIM_HandleTypeDef *tim; + uint16_t channel; +} BSP_PWM_Config_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = { +/* AUTO GENERATED BSP_PWM_MAP */ +}; + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + if (duty_cycle > 1.0f) { + duty_cycle = 1.0f; + } + if (duty_cycle < 0.0f) { + duty_cycle = 0.0f; + } + // 获取ARR值（周期值） + uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim); + + // 计算比较值：CCR = duty_cycle * (ARR + 1) + uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1)); + + __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr); + + return BSP_OK; +} + +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency + uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler; + uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1; + + if (period > UINT16_MAX) { + return BSP_ERR; // Frequency too low + } + __HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period); + + return BSP_OK; +} + +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].tim->Init.AutoReloadPreload; +} + +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) { + return PWM_Map[ch].tim; +} + + +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].channel; +} + +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length); + return BSP_OK; +} + +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} \ No newline at end of file diff --git a/assets/User_code/bsp/pwm.h b/assets/User_code/bsp/pwm.h new file mode 100644 index 0000000..c50c127 --- /dev/null +++ b/assets/User_code/bsp/pwm.h @@ -0,0 +1,48 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "tim.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* PWM通道 */ +typedef enum { +/* AUTO GENERATED BSP_PWM_ENUM */ + BSP_PWM_NUM, + BSP_PWM_ERR, +} BSP_PWM_Channel_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch); +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch); +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch); +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length); +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/bsp/spi.c b/assets/User_code/bsp/spi.c new file mode 100644 index 0000000..d8ac00e --- /dev/null +++ b/assets/User_code/bsp/spi.c @@ -0,0 +1,181 @@ +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/spi.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) { + if (hspi->Instance == SPI1) + return BSP_SPI_BMI088; + else + return BSP_SPI_ERR; +} + +void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) { + SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB](); + } + } +} + +void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB](); + } +} + +void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) { + switch (spi) { + case BSP_SPI_BMI088: + return &hspi1; + default: + return NULL; + } +} + +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + SPI_Callback[spi][type] = callback; + return BSP_OK; +} + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;; + } else { + return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;; + } +} + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) { + if (spi >= BSP_SPI_NUM) return 0xFF; + uint8_t tmp[2] = {reg | 0x80, 0x00}; + BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true); + return tmp[1]; +} + +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + uint8_t tmp[2] = {reg & 0x7f, data}; + return BSP_SPI_Transmit(spi, tmp, 2u, true); +} + +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg | 0x80; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Receive(spi, data, size, true); +} + +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg & 0x7f; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Transmit(spi, data, size, true); +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/assets/User_code/bsp/spi.h b/assets/User_code/bsp/spi.h new file mode 100644 index 0000000..8c0afae --- /dev/null +++ b/assets/User_code/bsp/spi.h @@ -0,0 +1,70 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */ + +/* SPI实体枚举，与设备对应 */ +typedef enum { +/* AUTO GENERATED BSP_SPI_NAME */ + BSP_SPI_NUM, + BSP_SPI_ERR, +} BSP_SPI_t; + +/* SPI支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_SPI_TX_CPLT_CB, + BSP_SPI_RX_CPLT_CB, + BSP_SPI_TX_RX_CPLT_CB, + BSP_SPI_TX_HALF_CPLT_CB, + BSP_SPI_RX_HALF_CPLT_CB, + BSP_SPI_TX_RX_HALF_CPLT_CB, + BSP_SPI_ERROR_CB, + BSP_SPI_ABORT_CPLT_CB, + BSP_SPI_CB_NUM, +} BSP_SPI_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi); +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)); + + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma); + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg); +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data); +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/bsp/time.c b/assets/User_code/bsp/time.c new file mode 100644 index 0000000..21918ed --- /dev/null +++ b/assets/User_code/bsp/time.c @@ -0,0 +1,81 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/time.h" +#include "bsp.h" + +#include +#include "FreeRTOS.h" +#include "main.h" +#include "task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +uint32_t BSP_TIME_Get_ms() { return xTaskGetTickCount(); } + +uint64_t BSP_TIME_Get_us() { + uint32_t tick_freq = osKernelGetTickFreq(); + uint32_t ticks_old = xTaskGetTickCount()*(1000/tick_freq); + uint32_t tick_value_old = SysTick->VAL; + uint32_t ticks_new = xTaskGetTickCount()*(1000/tick_freq); + uint32_t tick_value_new = SysTick->VAL; + if (ticks_old == ticks_new) { + return ticks_new * 1000 + 1000 - tick_value_old * 1000 / (SysTick->LOAD + 1); + } else { + return ticks_new * 1000 + 1000 - tick_value_new * 1000 / (SysTick->LOAD + 1); + } +} + +uint64_t BSP_TIME_Get() __attribute__((alias("BSP_TIME_Get_us"))); + +int8_t BSP_TIME_Delay_ms(uint32_t ms) { + uint32_t tick_period = 1000u / osKernelGetTickFreq(); + uint32_t ticks = ms / tick_period; + + switch (osKernelGetState()) { + case osKernelError: + case osKernelReserved: + case osKernelLocked: + case osKernelSuspended: + return BSP_ERR; + + case osKernelRunning: + osDelay(ticks ? ticks : 1); + break; + + case osKernelInactive: + case osKernelReady: + HAL_Delay(ms); + break; + } + return BSP_OK; +} + +/*阻塞us延迟*/ +int8_t BSP_TIME_Delay_us(uint32_t us) { + uint64_t start = BSP_TIME_Get_us(); + while (BSP_TIME_Get_us() - start < us) { + // 等待us时间 + } + return BSP_OK; +} + +int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms"))); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/assets/User_code/bsp/time.h b/assets/User_code/bsp/time.h new file mode 100644 index 0000000..c69085b --- /dev/null +++ b/assets/User_code/bsp/time.h @@ -0,0 +1,43 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +uint32_t BSP_TIME_Get_ms(); + +uint64_t BSP_TIME_Get_us(); + +uint64_t BSP_TIME_Get(); + +int8_t BSP_TIME_Delay_ms(uint32_t ms); + +/*微秒阻塞延时，一般别用*/ +int8_t BSP_TIME_Delay_us(uint32_t us); + +int8_t BSP_TIME_Delay(uint32_t ms); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/bsp/uart.c b/assets/User_code/bsp/uart.c new file mode 100644 index 0000000..a403fdc --- /dev/null +++ b/assets/User_code/bsp/uart.c @@ -0,0 +1,153 @@ +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/uart.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) { +/* AUTO GENERATED UART_GET */ + else + return BSP_UART_ERR; +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB](); + } + } +} + +void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB](); + } + } +} + +void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) { + UART_Callback[bsp_uart][BSP_UART_ERROR_CB](); + } + } +} + +void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB](); + } + } +} + +void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB](); + } + } +} + +void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB](); + } + } +} + +/* Exported functions ------------------------------------------------------- */ +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) { + if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) { + __HAL_UART_CLEAR_IDLEFLAG(huart); + if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) { + UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB](); + } + } +} + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { + switch (uart) { +/* AUTO GENERATED BSP_UART_GET_HANDLE */ + default: + return NULL; + } +} + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR; + UART_Callback[uart][type] = callback; + return BSP_OK; +} + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/assets/User_code/bsp/uart.h b/assets/User_code/bsp/uart.h new file mode 100644 index 0000000..06ff849 --- /dev/null +++ b/assets/User_code/bsp/uart.h @@ -0,0 +1,68 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */ + +/* UART实体枚举，与设备对应 */ +typedef enum { +/* AUTO GENERATED BSP_UART_NAME */ + BSP_UART_NUM, + BSP_UART_ERR, +} BSP_UART_t; + +/* UART支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_UART_TX_HALF_CPLT_CB, + BSP_UART_TX_CPLT_CB, + BSP_UART_RX_HALF_CPLT_CB, + BSP_UART_RX_CPLT_CB, + BSP_UART_ERROR_CB, + BSP_UART_ABORT_CPLT_CB, + BSP_UART_ABORT_TX_CPLT_CB, + BSP_UART_ABORT_RX_CPLT_CB, + + BSP_UART_IDLE_LINE_CB, + BSP_UART_CB_NUM, +} BSP_UART_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart); + +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart); + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)); + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/.gitkeep b/assets/User_code/component/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/assets/User_code/component/FreeRTOS_CLI.c b/assets/User_code/component/FreeRTOS_CLI.c new file mode 100644 index 0000000..32421de --- /dev/null +++ b/assets/User_code/component/FreeRTOS_CLI.c @@ -0,0 +1,352 @@ +/* + * FreeRTOS+CLI V1.0.4 + * Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * http://www.FreeRTOS.org + * http://aws.amazon.com/freertos + * + * 1 tab == 4 spaces! + */ + +/* Standard includes. */ +#include +#include + +/* FreeRTOS includes. */ +#include "FreeRTOS.h" +#include "task.h" + +/* Utils includes. */ +#include "FreeRTOS_CLI.h" + +/* If the application writer needs to place the buffer used by the CLI at a +fixed address then set configAPPLICATION_PROVIDES_cOutputBuffer to 1 in +FreeRTOSConfig.h, then declare an array with the following name and size in +one of the application files: + char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ]; +*/ +#ifndef configAPPLICATION_PROVIDES_cOutputBuffer + #define configAPPLICATION_PROVIDES_cOutputBuffer 0 +#endif + +typedef struct xCOMMAND_INPUT_LIST +{ + const CLI_Command_Definition_t *pxCommandLineDefinition; + struct xCOMMAND_INPUT_LIST *pxNext; +} CLI_Definition_List_Item_t; + +/* + * The callback function that is executed when "help" is entered. This is the + * only default command that is always present. + */ +static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString ); + +/* + * Return the number of parameters that follow the command name. + */ +static int8_t prvGetNumberOfParameters( const char *pcCommandString ); + +/* The definition of the "help" command. This command is always at the front +of the list of registered commands. */ +static const CLI_Command_Definition_t xHelpCommand = +{ + "help", + "\r\nhelp:\r\n Lists all the registered commands\r\n\r\n", + prvHelpCommand, + 0 +}; + +/* The definition of the list of commands. Commands that are registered are +added to this list. */ +static CLI_Definition_List_Item_t xRegisteredCommands = +{ + &xHelpCommand, /* The first command in the list is always the help command, defined in this file. */ + NULL /* The next pointer is initialised to NULL, as there are no other registered commands yet. */ +}; + +/* A buffer into which command outputs can be written is declared here, rather +than in the command console implementation, to allow multiple command consoles +to share the same buffer. For example, an application may allow access to the +command interpreter by UART and by Ethernet. Sharing a buffer is done purely +to save RAM. Note, however, that the command console itself is not re-entrant, +so only one command interpreter interface can be used at any one time. For that +reason, no attempt at providing mutual exclusion to the cOutputBuffer array is +attempted. + +configAPPLICATION_PROVIDES_cOutputBuffer is provided to allow the application +writer to provide their own cOutputBuffer declaration in cases where the +buffer needs to be placed at a fixed address (rather than by the linker). */ + +#if( configAPPLICATION_PROVIDES_cOutputBuffer == 0 ) + static char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ]; +#else + extern char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ]; +#endif + + +/*---------------------------------------------------------- */ + +BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister ) +{ +static CLI_Definition_List_Item_t *pxLastCommandInList = &xRegisteredCommands; +CLI_Definition_List_Item_t *pxNewListItem; +BaseType_t xReturn = pdFAIL; + + /* Check the parameter is not NULL. */ + configASSERT( pxCommandToRegister ); + + /* Create a new list item that will reference the command being registered. */ + pxNewListItem = ( CLI_Definition_List_Item_t * ) pvPortMalloc( sizeof( CLI_Definition_List_Item_t ) ); + configASSERT( pxNewListItem ); + + if( pxNewListItem != NULL ) + { + taskENTER_CRITICAL(); + { + /* Reference the command being registered from the newly created + list item. */ + pxNewListItem->pxCommandLineDefinition = pxCommandToRegister; + + /* The new list item will get added to the end of the list, so + pxNext has nowhere to point. */ + pxNewListItem->pxNext = NULL; + + /* Add the newly created list item to the end of the already existing + list. */ + pxLastCommandInList->pxNext = pxNewListItem; + + /* Set the end of list marker to the new list item. */ + pxLastCommandInList = pxNewListItem; + } + taskEXIT_CRITICAL(); + + xReturn = pdPASS; + } + + return xReturn; +} +/*---------------------------------------------------------- */ + +BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen ) +{ +static const CLI_Definition_List_Item_t *pxCommand = NULL; +BaseType_t xReturn = pdTRUE; +const char *pcRegisteredCommandString; +size_t xCommandStringLength; + + /* Note: This function is not re-entrant. It must not be called from more + thank one task. */ + + if( pxCommand == NULL ) + { + /* Search for the command string in the list of registered commands. */ + for( pxCommand = &xRegisteredCommands; pxCommand != NULL; pxCommand = pxCommand->pxNext ) + { + pcRegisteredCommandString = pxCommand->pxCommandLineDefinition->pcCommand; + xCommandStringLength = strlen( pcRegisteredCommandString ); + + /* To ensure the string lengths match exactly, so as not to pick up + a sub-string of a longer command, check the byte after the expected + end of the string is either the end of the string or a space before + a parameter. */ + if( ( pcCommandInput[ xCommandStringLength ] == ' ' ) || ( pcCommandInput[ xCommandStringLength ] == 0x00 ) ) + { + if( strncmp( pcCommandInput, pcRegisteredCommandString, xCommandStringLength ) == 0 ) + { + /* The command has been found. Check it has the expected + number of parameters. If cExpectedNumberOfParameters is -1, + then there could be a variable number of parameters and no + check is made. */ + if( pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters >= 0 ) + { + if( prvGetNumberOfParameters( pcCommandInput ) != pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters ) + { + xReturn = pdFALSE; + } + } + + break; + } + } + } + } + + if( ( pxCommand != NULL ) && ( xReturn == pdFALSE ) ) + { + /* The command was found, but the number of parameters with the command + was incorrect. */ + strncpy( pcWriteBuffer, "Incorrect command parameter(s). Enter \"help\" to view a list of available commands.\r\n\r\n", xWriteBufferLen ); + pxCommand = NULL; + } + else if( pxCommand != NULL ) + { + /* Call the callback function that is registered to this command. */ + xReturn = pxCommand->pxCommandLineDefinition->pxCommandInterpreter( pcWriteBuffer, xWriteBufferLen, pcCommandInput ); + + /* If xReturn is pdFALSE, then no further strings will be returned + after this one, and pxCommand can be reset to NULL ready to search + for the next entered command. */ + if( xReturn == pdFALSE ) + { + pxCommand = NULL; + } + } + else + { + /* pxCommand was NULL, the command was not found. */ + strncpy( pcWriteBuffer, "Command not recognised. Enter 'help' to view a list of available commands.\r\n\r\n", xWriteBufferLen ); + xReturn = pdFALSE; + } + + return xReturn; +} +/*---------------------------------------------------------- */ + +char *FreeRTOS_CLIGetOutputBuffer( void ) +{ + return cOutputBuffer; +} +/*---------------------------------------------------------- */ + +const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength ) +{ +UBaseType_t uxParametersFound = 0; +const char *pcReturn = NULL; + + *pxParameterStringLength = 0; + + while( uxParametersFound < uxWantedParameter ) + { + /* Index the character pointer past the current word. If this is the start + of the command string then the first word is the command itself. */ + while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) ) + { + pcCommandString++; + } + + /* Find the start of the next string. */ + while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) == ' ' ) ) + { + pcCommandString++; + } + + /* Was a string found? */ + if( *pcCommandString != 0x00 ) + { + /* Is this the start of the required parameter? */ + uxParametersFound++; + + if( uxParametersFound == uxWantedParameter ) + { + /* How long is the parameter? */ + pcReturn = pcCommandString; + while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) ) + { + ( *pxParameterStringLength )++; + pcCommandString++; + } + + if( *pxParameterStringLength == 0 ) + { + pcReturn = NULL; + } + + break; + } + } + else + { + break; + } + } + + return pcReturn; +} +/*---------------------------------------------------------- */ + +static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString ) +{ +static const CLI_Definition_List_Item_t * pxCommand = NULL; +BaseType_t xReturn; + + ( void ) pcCommandString; + + if( pxCommand == NULL ) + { + /* Reset the pxCommand pointer back to the start of the list. */ + pxCommand = &xRegisteredCommands; + } + + /* Return the next command help string, before moving the pointer on to + the next command in the list. */ + strncpy( pcWriteBuffer, pxCommand->pxCommandLineDefinition->pcHelpString, xWriteBufferLen ); + pxCommand = pxCommand->pxNext; + + if( pxCommand == NULL ) + { + /* There are no more commands in the list, so there will be no more + strings to return after this one and pdFALSE should be returned. */ + xReturn = pdFALSE; + } + else + { + xReturn = pdTRUE; + } + + return xReturn; +} +/*---------------------------------------------------------- */ + +static int8_t prvGetNumberOfParameters( const char *pcCommandString ) +{ +int8_t cParameters = 0; +BaseType_t xLastCharacterWasSpace = pdFALSE; + + /* Count the number of space delimited words in pcCommandString. */ + while( *pcCommandString != 0x00 ) + { + if( ( *pcCommandString ) == ' ' ) + { + if( xLastCharacterWasSpace != pdTRUE ) + { + cParameters++; + xLastCharacterWasSpace = pdTRUE; + } + } + else + { + xLastCharacterWasSpace = pdFALSE; + } + + pcCommandString++; + } + + /* If the command string ended with spaces, then there will have been too + many parameters counted. */ + if( xLastCharacterWasSpace == pdTRUE ) + { + cParameters--; + } + + /* The value returned is one less than the number of space delimited words, + as the first word should be the command itself. */ + return cParameters; +} + diff --git a/assets/User_code/component/FreeRTOS_CLI.h b/assets/User_code/component/FreeRTOS_CLI.h new file mode 100644 index 0000000..e6d8266 --- /dev/null +++ b/assets/User_code/component/FreeRTOS_CLI.h @@ -0,0 +1,108 @@ +/* + * FreeRTOS+CLI V1.0.4 + * Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * http://www.FreeRTOS.org + * http://aws.amazon.com/freertos + * + * 1 tab == 4 spaces! + */ + +#ifndef COMMAND_INTERPRETER_H +#define COMMAND_INTERPRETER_H + +/* This config should be defined in FreeRTOSConfig.h. But due to the limition of CubeMX I put it here. */ +#define configCOMMAND_INT_MAX_OUTPUT_SIZE 512 + +/* The prototype to which callback functions used to process command line +commands must comply. pcWriteBuffer is a buffer into which the output from +executing the command can be written, xWriteBufferLen is the length, in bytes of +the pcWriteBuffer buffer, and pcCommandString is the entire string as input by +the user (from which parameters can be extracted).*/ +typedef BaseType_t (*pdCOMMAND_LINE_CALLBACK)( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString ); + +/* The structure that defines command line commands. A command line command +should be defined by declaring a const structure of this type. */ +typedef struct xCOMMAND_LINE_INPUT +{ + const char * const pcCommand; /* The command that causes pxCommandInterpreter to be executed. For example "help". Must be all lower case. */ + const char * const pcHelpString; /* String that describes how to use the command. Should start with the command itself, and end with "\r\n". For example "help: Returns a list of all the commands\r\n". */ + const pdCOMMAND_LINE_CALLBACK pxCommandInterpreter; /* A pointer to the callback function that will return the output generated by the command. */ + int8_t cExpectedNumberOfParameters; /* Commands expect a fixed number of parameters, which may be zero. */ +} CLI_Command_Definition_t; + +/* For backward compatibility. */ +#define xCommandLineInput CLI_Command_Definition_t + +/* + * Register the command passed in using the pxCommandToRegister parameter. + * Registering a command adds the command to the list of commands that are + * handled by the command interpreter. Once a command has been registered it + * can be executed from the command line. + */ +BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister ); + +/* + * Runs the command interpreter for the command string "pcCommandInput". Any + * output generated by running the command will be placed into pcWriteBuffer. + * xWriteBufferLen must indicate the size, in bytes, of the buffer pointed to + * by pcWriteBuffer. + * + * FreeRTOS_CLIProcessCommand should be called repeatedly until it returns pdFALSE. + * + * pcCmdIntProcessCommand is not reentrant. It must not be called from more + * than one task - or at least - by more than one task at a time. + */ +BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen ); + +/*---------------------------------------------------------- */ + +/* + * A buffer into which command outputs can be written is declared in the + * main command interpreter, rather than in the command console implementation, + * to allow application that provide access to the command console via multiple + * interfaces to share a buffer, and therefore save RAM. Note, however, that + * the command interpreter itself is not re-entrant, so only one command + * console interface can be used at any one time. For that reason, no attempt + * is made to provide any mutual exclusion mechanism on the output buffer. + * + * FreeRTOS_CLIGetOutputBuffer() returns the address of the output buffer. + */ +char *FreeRTOS_CLIGetOutputBuffer( void ); + +/* + * Return a pointer to the xParameterNumber'th word in pcCommandString. + */ +const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength ); + +#endif /* COMMAND_INTERPRETER_H */ + + + + + + + + + + + + + diff --git a/assets/User_code/component/ahrs.c b/assets/User_code/component/ahrs.c new file mode 100644 index 0000000..ffdb870 --- /dev/null +++ b/assets/User_code/component/ahrs.c @@ -0,0 +1,417 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#include "ahrs.h" + +#include + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define BETA_IMU (0.033f) +#define BETA_AHRS (0.041f) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 2 * proportional gain (Kp) */ +static float beta = BETA_IMU; + +/** + * @brief 不使用磁力计计算姿态 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @return int8_t 0对应没有错误 + */ +static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_IMU; + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, + q3q3; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _4q0 = 4.0f * ahrs->quat.q0; + _4q1 = 4.0f * ahrs->quat.q1; + _4q2 = 4.0f * ahrs->quat.q2; + _8q1 = 8.0f * ahrs->quat.q1; + _8q2 = 8.0f * ahrs->quat.q2; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Gradient decent algorithm corrective step */ + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 - + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 - + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax + + 4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay; + + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) { + if (ahrs == NULL) return -1; + + ahrs->inv_sample_freq = 1.0f / sample_freq; + + ahrs->quat.q0 = 1.0f; + ahrs->quat.q1 = 0.0f; + ahrs->quat.q2 = 0.0f; + ahrs->quat.q3 = 0.0f; + + if (magn) { + float yaw = -atan2(magn->y, magn->x); + + if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.997458339f; + ahrs->quat.q1 = 0.000336312107f; + ahrs->quat.q2 = -0.0057230792f; + ahrs->quat.q3 = 0.0740156546; + + } else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > M_PI)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + } + } + return 0; +} + +/** + * @brief 姿态运算更新一次 + * @note 输入数据必须是NED(North East Down) 参考坐标系 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_AHRS; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, + _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, + q2q2, q2q3, q3q3; + + if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro); + + float mx = magn->x; + float my = magn->y; + float mz = magn->z; + + /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */ + /* magnetometer normalisation) */ + if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + return AHRS_UpdateIMU(ahrs, accl, gyro); + } + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Normalise magnetometer measurement */ + recip_norm = InvSqrt(mx * mx + my * my + mz * mz); + mx *= recip_norm; + my *= recip_norm; + mz *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0mx = 2.0f * ahrs->quat.q0 * mx; + _2q0my = 2.0f * ahrs->quat.q0 * my; + _2q0mz = 2.0f * ahrs->quat.q0 * mz; + _2q1mx = 2.0f * ahrs->quat.q1 * mx; + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2; + _2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q0q1 = ahrs->quat.q0 * ahrs->quat.q1; + q0q2 = ahrs->quat.q0 * ahrs->quat.q2; + q0q3 = ahrs->quat.q0 * ahrs->quat.q3; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q1q2 = ahrs->quat.q1 * ahrs->quat.q2; + q1q3 = ahrs->quat.q1 * ahrs->quat.q3; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q2q3 = ahrs->quat.q2 * ahrs->quat.q3; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Reference direction of Earth's magnetic field */ + hx = mx * q0q0 - _2q0my * ahrs->quat.q3 + + _2q0mz * ahrs->quat.q2 + mx * q1q1 + + _2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 - + mx * q2q2 - mx * q3q3; + hy = _2q0mx * ahrs->quat.q3 + my * q0q0 - + _2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 - + my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3; + // _2bx = sqrtf(hx * hx + hy * hy); + // 改为invsqrt + _2bx = 1.f / InvSqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 + + mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 + + _2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + /* Gradient decent algorithm corrective step */ + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - + _2bz * ahrs->quat.q2 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q2 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + _2bz * ahrs->quat.q3 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + (-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + + (-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q1 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) { + if (eulr == NULL) return -1; + if (ahrs == NULL) return -1; + + const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q3); + const float cosr_cosp = + 1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2); + eulr->pit = atan2f(sinr_cosp, cosr_cosp); + + const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 - + ahrs->quat.q3 * ahrs->quat.q1); + + if (fabsf(sinp) >= 1.0f) + eulr->rol = copysignf(M_PI / 2.0f, sinp); + else + eulr->rol = asinf(sinp); + + const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 + + ahrs->quat.q1 * ahrs->quat.q2); + const float cosy_cosp = + 1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + eulr->yaw = atan2f(siny_cosp, cosy_cosp); + +#if 0 + eulr->yaw *= M_RAD2DEG_MULT; + eulr->rol *= M_RAD2DEG_MULT; + eulr->pit *= M_RAD2DEG_MULT; +#endif + + return 0; +} + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/assets/User_code/component/ahrs.h b/assets/User_code/component/ahrs.h new file mode 100644 index 0000000..2245b1f --- /dev/null +++ b/assets/User_code/component/ahrs.h @@ -0,0 +1,114 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 欧拉角（Euler angle） */ +typedef struct { + float yaw; /* 偏航角（Yaw angle） */ + float pit; /* 俯仰角（Pitch angle） */ + float rol; /* 翻滚角（Roll angle） */ +} AHRS_Eulr_t; + +/* 加速度计 Accelerometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Accl_t; + +/* 陀螺仪 Gyroscope */ +typedef struct { + float x; + float y; + float z; +} AHRS_Gyro_t; + +/* 磁力计 Magnetometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Magn_t; + +/* 四元数 */ +typedef struct { + float q0; + float q1; + float q2; + float q3; +} AHRS_Quaternion_t; + +/* 姿态解算算法主结构体 */ +typedef struct { + /* 四元数 */ + AHRS_Quaternion_t quat; + + float inv_sample_freq; /* 采样频率的的倒数 */ +} AHRS_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq); + +/** + * @brief 姿态运算更新一次 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn); + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs); + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/capacity.c b/assets/User_code/component/capacity.c new file mode 100644 index 0000000..fd64eb2 --- /dev/null +++ b/assets/User_code/component/capacity.c @@ -0,0 +1,70 @@ +/* + 剩余电量算法。 + 通过电压值计算剩余电量。 +*/ + +#include "capacity.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief 通过电压计算电池剩余电量 + * + * @param volt 电压值 + * @return float 剩余电量比例 + */ +float Capacity_GetBatteryRemain(float volt) { + float percentage; + float volt_2 = volt * volt; + float volt_3 = volt_2 * volt; + + if (volt < 19.5f) + percentage = 0.0f; + + else if (volt < 21.9f) + percentage = 0.005664f * volt_3 - 0.3386f * volt_2 + 6.765f * volt - 45.17f; + + else if (volt < 25.5f) + percentage = 0.02269f * volt_3 - 1.654f * volt_2 + 40.34f * volt - 328.4f; + + else + percentage = 1.0f; + + if (percentage < 0.0f) + percentage = 0.0f; + + else if (percentage > 1.0f) + percentage = 1.0f; + + return percentage; +} + +/** + * @brief + * + * @param vcap 电容电压 + * @param vbat 电池电压 + * @param v_cutoff 截止电压 + * @return float 电容剩余电量比例 + */ +float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff) { + float percentage = (vcap - v_cutoff) / (vbat - v_cutoff); + + if (percentage < 0.0f) + percentage = 0.0f; + + else if (percentage > 1.0f) + percentage = 1.0f; + + return percentage; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/assets/User_code/component/capacity.h b/assets/User_code/component/capacity.h new file mode 100644 index 0000000..b4010e9 --- /dev/null +++ b/assets/User_code/component/capacity.h @@ -0,0 +1,47 @@ +/* + 剩余电量算法。 + + 通过电压值计算剩余电量。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief 通过电压计算电池剩余电量 + * + * @param volt 电压值 + * @return float 剩余电量比例 + */ +float Capacity_GetBatteryRemain(float volt); + +/** + * @brief + * + * @param vcap 电容电压 + * @param vbat 电池电压 + * @param v_cutoff 截止电压 + * @return float 电容剩余电量比例 + */ +float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/crc16.c b/assets/User_code/component/crc16.c new file mode 100644 index 0000000..0d17eb0 --- /dev/null +++ b/assets/User_code/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/assets/User_code/component/crc16.h b/assets/User_code/component/crc16.h new file mode 100644 index 0000000..68b0a87 --- /dev/null +++ b/assets/User_code/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/assets/User_code/component/crc8.c b/assets/User_code/component/crc8.c new file mode 100644 index 0000000..66f4ad2 --- /dev/null +++ b/assets/User_code/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/assets/User_code/component/crc8.h b/assets/User_code/component/crc8.h new file mode 100644 index 0000000..a376c71 --- /dev/null +++ b/assets/User_code/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/assets/User_code/component/dependencies.csv b/assets/User_code/component/dependencies.csv new file mode 100644 index 0000000..c3f1e6d --- /dev/null +++ b/assets/User_code/component/dependencies.csv @@ -0,0 +1,8 @@ +ahrs,component/user_math.h +capacity,component/user_math.h +cmd,component/ahrs +error_detect,bsp/mm +pid,component/filter +filter,component/ahrs +mixer,component/user_math.h +ui,component/user_math.h \ No newline at end of file diff --git a/assets/User_code/component/describe.csv b/assets/User_code/component/describe.csv new file mode 100644 index 0000000..2752291 --- /dev/null +++ b/assets/User_code/component/describe.csv @@ -0,0 +1,14 @@ +pid,好用的 +ahrs,开源的AHRS算法，MadgwickAHRS +capacity,电池容量计算 +cmd,通用控制命令 +crc8,CRC8校验rm +crc16,CRC16校验rm +error_detect,错误检测 +filter,各类滤波器 +FreeRTOS_CLI,FreeRTOS命令行接口 +limiter,限幅器 +mixer,混控器 +ui,用户交互 +user_math,用户自定义数学函数 +pid,PID控制器 \ No newline at end of file diff --git a/assets/User_code/component/error_detect.c b/assets/User_code/component/error_detect.c new file mode 100644 index 0000000..b29e591 --- /dev/null +++ b/assets/User_code/component/error_detect.c @@ -0,0 +1,67 @@ +/* + 错误检测。 +*/ + +#include "error_detect.h" + +#include +#include + +#include "bsp/mm.h" + +static ErrorDetect_t ged; +static bool inited = false; + +int8_t ErrorDetect_Init(void) { + if (inited) return -1; + + memset(&ged, 0x00, sizeof(ged)); + + for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) { + ged.error[i].enable = true; + ged.error[i].priority = i; + ged.error[i].patient_lost = 500; + ged.error[i].patient_work = 500; + } + return 0; +} + +void ErrorDetect_Processing(uint32_t sys_time) { + for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) { + if (!ged.error[i].enable) continue; + + if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) { + ged.error[i].is_lost = true; + ged.error[i].found_lost = sys_time; + } else if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) { + } else { + ged.error[i].cycle_time = ged.error[i].showup - ged.error[i].showup_last; + } + } +} + +bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit) { + if (unit == ERROR_DETECT_UNIT_NO_DEV) { + for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) { + if (ged.error[i].error_exist) return true; + } + return false; + } else { + return ged.error[unit].error_exist; + } +} + +ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void) { + for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) { + if (ged.error[i].error_exist) return i; + } + return ERROR_DETECT_UNIT_NO_DEV; +} + +const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit) { + return &ged.error[unit]; +} + +void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current) { + ged.error[unit].showup = time_current; +} diff --git a/assets/User_code/component/error_detect.h b/assets/User_code/component/error_detect.h new file mode 100644 index 0000000..3dba7a2 --- /dev/null +++ b/assets/User_code/component/error_detect.h @@ -0,0 +1,82 @@ +/* + 错误检测。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +typedef enum { + /* Low priority */ + ERROR_DETECT_UNIT_NO_DEV = 0, + ERROR_DETECT_UNIT_REFEREE, + ERROR_DETECT_UNIT_CHASSIS_M1, + ERROR_DETECT_UNIT_CHASSIS_M2, + ERROR_DETECT_UNIT_CHASSIS_M3, + ERROR_DETECT_UNIT_CHASSIS_M4, + ERROR_DETECT_UNIT_TRIGGER, + ERROR_DETECT_UNIT_FEED, + ERROR_DETECT_UNIT_GIMBAL_YAW, + ERROR_DETECT_UNIT_GIMBAL_PIT, + ERROR_DETECT_UNIT_GYRO, + ERROR_DETECT_UNIT_ACCL, + ERROR_DETECT_UNIT_MAGN, + ERROR_DETECT_UNIT_DBUS, + ERROR_DETECT_UNIT_NUM, + /* High priority */ +} ErrorDetect_Unit_t; + +typedef struct { + bool enable; + uint8_t priority; + uint32_t patient_lost; + uint32_t patient_work; + + uint32_t showup; + uint32_t showup_last; + uint32_t cycle_time; + uint32_t duration_lost; + uint32_t duration_work; + uint32_t found_lost; + bool error_exist; + bool is_lost; + uint8_t data_is_error; + +} ErrorDetect_Error_t; + +typedef struct { + ErrorDetect_Error_t error[ERROR_DETECT_UNIT_NUM]; +} ErrorDetect_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +int8_t ErrorDetect_Init(void); +void ErrorDetect_Processing(uint32_t sys_time); +bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit); +ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void); +const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit); + +void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/filter.c b/assets/User_code/component/filter.c new file mode 100644 index 0000000..5375b8e --- /dev/null +++ b/assets/User_code/component/filter.c @@ -0,0 +1,185 @@ +/* + 各类滤波器。 +*/ + +#include "filter.h" + +#include "user_math.h" + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq) { + if (f == NULL) return; + + f->cutoff_freq = cutoff_freq; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (f->cutoff_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + const float fr = sample_freq / f->cutoff_freq; + const float ohm = tanf(M_PI / fr); + const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm; + + f->b0 = ohm * ohm / c; + f->b1 = 2.0f * f->b0; + f->b2 = f->b0; + + f->a1 = 2.0f * (ohm * ohm - 1.0f) / c; + f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* do the filtering */ + float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + + if (isinf(delay_element_0)) { + /* don't allow bad values to propagate via the filter */ + delay_element_0 = sample; + } + + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + /* return the value. Should be no need to check limits */ + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + const float dval = sample / (f->b0 + f->b1 + f->b2); + + if (isfinite(dval)) { + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + } else { + f->delay_element_1 = sample; + f->delay_element_2 = sample; + } + + return LowPassFilter2p_Apply(f, sample); +} + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth) { + if (f == NULL) return; + + f->notch_freq = notch_freq; + f->bandwidth = bandwidth; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (notch_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + + const float alpha = tanf(M_PI * bandwidth / sample_freq); + const float beta = -cosf(M_2PI * notch_freq / sample_freq); + const float a0_inv = 1.0f / (alpha + 1.0f); + + f->b0 = a0_inv; + f->b1 = 2.0f * beta * a0_inv; + f->b2 = a0_inv; + + f->a1 = f->b1; + f->a2 = (1.0f - alpha) * a0_inv; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +inline float NotchFilter_Apply(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* Direct Form II implementation */ + const float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + float dval = sample; + + if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) { + dval = dval / (f->b0 + f->b1 + f->b2); + } + + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + return NotchFilter_Apply(f, sample); +} diff --git a/assets/User_code/component/filter.h b/assets/User_code/component/filter.h new file mode 100644 index 0000000..ae2b072 --- /dev/null +++ b/assets/User_code/component/filter.h @@ -0,0 +1,120 @@ +/* + 各类滤波器。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 二阶低通滤波器 */ +typedef struct { + float cutoff_freq; /* 截止频率 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + + float delay_element_1; + float delay_element_2; + +} LowPassFilter2p_t; + +/* 带阻滤波器 */ +typedef struct { + float notch_freq; /* 阻止频率 */ + float bandwidth; /* 带宽 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + float delay_element_1; + float delay_element_2; + +} NotchFilter_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample); + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Apply(NotchFilter_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/limiter.c b/assets/User_code/component/limiter.c new file mode 100644 index 0000000..71e4bf1 --- /dev/null +++ b/assets/User_code/component/limiter.c @@ -0,0 +1,107 @@ +/* + 限制器 +*/ + +#include "limiter.h" + +#include +#include + +#define POWER_BUFF_THRESHOLD 20 +#define CHASSIS_POWER_CHECK_FREQ 10 +#define CHASSIS_POWER_FACTOR_PASS 0.9f +#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f + +#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len) { + /* power_limit小于0时不进行限制 */ + if (motor_out == NULL || speed == NULL || power_limit < 0) return -1; + + float sum_motor_out = 0.0f; + for (uint32_t i = 0; i < len; i++) { + /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */ + sum_motor_out += + fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE; + } + + /* 保持每个电机输出值缩小时比例不变 */ + if (sum_motor_out > power_limit) { + for (uint32_t i = 0; i < len; i++) { + motor_out[i] *= power_limit / sum_motor_out; + } + } + + return 0; +} + +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer) { + float target_power = 0.0f; + + /* 计算下一个检测周期的剩余缓冲能量 */ + float heat_buff = power_buffer - (float)(power_in - power_limit) / + (float)CHASSIS_POWER_CHECK_FREQ; + if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */ + target_power = power_limit * CHASSIS_POWER_FACTOR_PASS; + } else { + target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS; + } + + return target_power; +} + +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer) { + float target_power = 0.0f; + + /* 根据剩余缓冲能量计算输出功率 */ + target_power = power_limit * (power_buffer - 10.0f) / 20.0f; + if (target_power < 0.0f) target_power = 0.0f; + + return target_power; +} + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big) { + float heat_percent = heat / heat_limit; + float stable_freq = cooling_rate / heat_increase; + if (is_big) + return stable_freq; + else + return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq; +} diff --git a/assets/User_code/component/limiter.h b/assets/User_code/component/limiter.h new file mode 100644 index 0000000..d0aa92a --- /dev/null +++ b/assets/User_code/component/limiter.h @@ -0,0 +1,63 @@ +/* + 限制器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len); +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer); +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer); + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big); diff --git a/assets/User_code/component/mixer.c b/assets/User_code/component/mixer.c new file mode 100644 index 0000000..554b92e --- /dev/null +++ b/assets/User_code/component/mixer.c @@ -0,0 +1,94 @@ +/* + 混合器 +*/ + +#include "mixer.h" + +#include "math.h" + +/** + * @brief 初始化混合器 + * + * @param mixer 混合器 + * @param mode 混合器模式 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode) { + if (mixer == NULL) return -1; + + mixer->mode = mode; + return 0; +} + +/** + * @brief 计算输出 + * + * @param mixer 混合器 + * @param move_vec 运动向量 + * @param out 输出数组 + * @param len 输出数组长短 + * @param scale 输出放大因子 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out, + int8_t len, float scale) { + if (mixer == NULL) return -1; + + switch (mixer->mode) { + case MIXER_MECANUM: + if (len == 4) { + out[0] = move_vec->vx - move_vec->vy + move_vec->wz; + out[1] = move_vec->vx + move_vec->vy + move_vec->wz; + out[2] = -move_vec->vx + move_vec->vy + move_vec->wz; + out[3] = -move_vec->vx - move_vec->vy + move_vec->wz; + } else { + goto error; + } + break; + + case MIXER_PARLFIX4: + if (len == 4) { + out[0] = -move_vec->vx; + out[1] = move_vec->vx; + out[2] = move_vec->vx; + out[3] = -move_vec->vx; + } else { + goto error; + } + case MIXER_PARLFIX2: + if (len == 2) { + out[0] = -move_vec->vx; + out[1] = move_vec->vx; + } else { + goto error; + } + case MIXER_SINGLE: + if (len == 1) { + out[0] = move_vec->vx; + } else { + goto error; + } + case MIXER_OMNICROSS: + case MIXER_OMNIPLUS: + goto error; + } + + float abs_max = 0.f; + for (int8_t i = 0; i < len; i++) { + const float abs_val = fabsf(out[i]); + abs_max = (abs_val > abs_max) ? abs_val : abs_max; + } + if (abs_max > 1.f) { + for (int8_t i = 0; i < len; i++) { + out[i] /= abs_max; + } + } + for (int8_t i = 0; i < len; i++) { + out[i] *= scale; + } + return 0; + +error: + for (uint8_t i = 0; i < len; i++) out[i] = 0; + return -1; +} diff --git a/assets/User_code/component/mixer.h b/assets/User_code/component/mixer.h new file mode 100644 index 0000000..b8e4401 --- /dev/null +++ b/assets/User_code/component/mixer.h @@ -0,0 +1,76 @@ +/* + 混合器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** 四轮布局 */ +/* 前 */ +/* 2 1 */ +/* 3 4 */ + +/* 两轮布局 */ +/* 前 */ +/* 2 1 */ + +/* 混合器模式 */ +typedef enum { + MIXER_MECANUM, /* 麦克纳姆轮 */ + MIXER_PARLFIX4, /* 平行四驱动轮 */ + MIXER_PARLFIX2, /* 平行对侧两驱动轮 */ + MIXER_OMNICROSS, /* 叉形全向轮 */ + MIXER_OMNIPLUS, /* 十字全向轮 */ + MIXER_SINGLE, /* 单个摩擦轮 */ +} Mixer_Mode_t; + +typedef struct { + Mixer_Mode_t mode; +} Mixer_t; /* 混合器主结构体 */ + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化混合器 + * + * @param mixer 混合器 + * @param mode 混合器模式 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode); + +/** + * @brief 计算输出 + * + * @param mixer 混合器 + * @param move_vec 运动向量 + * @param out 输出数组 + * @param len 输出数组长短 + * @param scale 输出放大因子 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out, + int8_t len, float scale); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/pid.c b/assets/User_code/component/pid.c new file mode 100644 index 0000000..0a3c7d4 --- /dev/null +++ b/assets/User_code/component/pid.c @@ -0,0 +1,158 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp + + 参考资料： + https://github.com/PX4/Firmware/issues/12362 + https://dev.px4.io/master/en/flight_stack/controller_diagrams.html + https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form + https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/ + https://en.wikipedia.org/wiki/PID_controller + http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ +*/ + +#include "pid.h" + +#define SIGMA 0.000001f + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param) { + if (pid == NULL) return -1; + + if (!isfinite(param->p)) return -1; + if (!isfinite(param->i)) return -1; + if (!isfinite(param->d)) return -1; + if (!isfinite(param->i_limit)) return -1; + if (!isfinite(param->out_limit)) return -1; + pid->param = param; + + float dt_min = 1.0f / sample_freq; + if (isfinite(dt_min)) + pid->dt_min = dt_min; + else + return -1; + + LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq); + + pid->mode = mode; + PID_Reset(pid); + return 0; +} + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) { + if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) { + return pid->last.out; + } + + /* 计算误差值 */ + const float err = CircleError(sp, fb, pid->param->range); + + /* 计算P项 */ + const float k_err = err * pid->param->k; + + /* 计算D项 */ + const float k_fb = pid->param->k * fb; + const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb); + + float d; + switch (pid->mode) { + case KPID_MODE_CALC_D: + /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */ + /* 当sp不变时，err的微分等于负的fb的微分 */ + d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min); + break; + + case KPID_MODE_SET_D: + d = fb_dot; + break; + + case KPID_MODE_NO_D: + d = 0.0f; + break; + } + pid->last.err = err; + pid->last.k_fb = filtered_k_fb; + + if (!isfinite(d)) d = 0.0f; + + /* 计算PD输出 */ + float output = (k_err * pid->param->p) - (d * pid->param->d); + + /* 计算I项 */ + const float i = pid->i + (k_err * dt); + const float i_out = i * pid->param->i; + + if (pid->param->i > SIGMA) { + /* 检查是否饱和 */ + if (isfinite(i)) { + if ((fabsf(output + i_out) <= pid->param->out_limit) && + (fabsf(i) <= pid->param->i_limit)) { + /* 未饱和，使用新积分 */ + pid->i = i; + } + } + } + + /* 计算PID输出 */ + output += i_out; + + /* 限制输出 */ + if (isfinite(output)) { + if (pid->param->out_limit > SIGMA) { + output = AbsClip(output, pid->param->out_limit); + } + pid->last.out = output; + } + return pid->last.out; +} + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + + return 0; +} + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + pid->last.err = 0.0f; + pid->last.k_fb = 0.0f; + pid->last.out = 0.0f; + LowPassFilter2p_Reset(&(pid->dfilter), 0.0f); + + return 0; +} diff --git a/assets/User_code/component/pid.h b/assets/User_code/component/pid.h new file mode 100644 index 0000000..4b451eb --- /dev/null +++ b/assets/User_code/component/pid.h @@ -0,0 +1,107 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "filter.h" +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* PID模式 */ +typedef enum { + KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */ + KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */ + KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */ +} KPID_Mode_t; + +/* PID参数 */ +typedef struct { + float k; /* 控制器增益，设置为1用于并行模式 */ + float p; /* 比例项增益，设置为1用于标准形式 */ + float i; /* 积分项增益 */ + float d; /* 微分项增益 */ + float i_limit; /* 积分项上限 */ + float out_limit; /* 输出绝对值限制 */ + float d_cutoff_freq; /* D项低通截止频率 */ + float range; /* 计算循环误差时使用，大于0时启用 */ +} KPID_Params_t; + +/* PID主结构体 */ +typedef struct { + KPID_Mode_t mode; + const KPID_Params_t *param; + + float dt_min; /* 最小PID_Calc调用间隔 */ + float i; /* 积分 */ + + struct { + float err; /* 上次误差 */ + float k_fb; /* 上次反馈值 */ + float out; /* 上次输出 */ + } last; + + LowPassFilter2p_t dfilter; /* D项低通滤波器 */ +} KPID_t; + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param); + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt); + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid); + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/ui.c b/assets/User_code/component/ui.c new file mode 100644 index 0000000..c3126bd --- /dev/null +++ b/assets/User_code/component/ui.c @@ -0,0 +1,301 @@ +/* + UI相关命令 +*/ +#include "component/ui.h" + +#include + +/** + * @brief UI_绘制直线段 + * + * @param grapic_line 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 终点x坐标 + * @param y_end 终点y坐标 + * @return int8_t + */ +int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t x_end, + uint16_t y_end) { + if (grapic_line == NULL) return -1; + snprintf((char *)grapic_line->name, 2, "%s", name); + grapic_line->layer = layer; + grapic_line->type_op = type_op; + grapic_line->type_ele = 0; + grapic_line->color = color; + grapic_line->width = width; + grapic_line->x_start = x_start; + grapic_line->y_start = y_start; + grapic_line->x_end = x_end; + grapic_line->y_end = y_end; + return 0; +} + +/** + * @brief UI_绘制矩形 + * + * @param grapic_rectangle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 对角顶点x坐标 + * @param y_end 对角顶点y坐标 + * @return int8_t + */ +int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t width, uint16_t x_start, uint16_t y_start, + uint16_t x_end, uint16_t y_end) { + if (grapic_rectangle == NULL) return -1; + snprintf((char *)grapic_rectangle->name, 2, "%s", name); + grapic_rectangle->type_op = type_op; + grapic_rectangle->type_ele = 1; + grapic_rectangle->layer = layer; + grapic_rectangle->color = color; + grapic_rectangle->width = width; + grapic_rectangle->x_start = x_start; + grapic_rectangle->y_start = y_start; + grapic_rectangle->x_end = x_end; + grapic_rectangle->y_end = y_end; + return 0; +} + +/** + * @brief UI_绘制正圆 + * + * @param grapic_cycle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param radius 半径 + * @return int8_t + */ +int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t radius) { + if (grapic_cycle == NULL) return -1; + snprintf((char *)grapic_cycle->name, 2, "%s", name); + grapic_cycle->type_op = type_op; + grapic_cycle->layer = layer; + grapic_cycle->type_ele = 2; + grapic_cycle->color = color; + grapic_cycle->width = width; + grapic_cycle->x_start = x_center; + grapic_cycle->y_start = y_center; + grapic_cycle->radius = radius; + return 0; +} + +/** + * @brief UI_绘制椭圆 + * + * @param grapic_oval 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis, + uint16_t y_semiaxis) { + if (grapic_oval == NULL) return -1; + snprintf((char *)grapic_oval->name, 2, "%s", name); + grapic_oval->type_op = type_op; + grapic_oval->type_ele = 3; + grapic_oval->layer = layer; + grapic_oval->color = color; + grapic_oval->width = width; + grapic_oval->x_start = x_center; + grapic_oval->y_start = y_center; + grapic_oval->x_end = x_semiaxis; + grapic_oval->y_end = y_semiaxis; + return 0; +} + +/** + * @brief UI_绘制圆弧 + * + * @param grapic_arc 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param angle_start 起始角度 + * @param angle_end 终止角度 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t angle_start, + uint16_t angle_end, uint16_t width, uint16_t x_center, + uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) { + if (grapic_arc == NULL) return -1; + snprintf((char *)grapic_arc->name, 2, "%s", name); + grapic_arc->type_op = type_op; + grapic_arc->type_ele = 4; + grapic_arc->layer = layer; + grapic_arc->color = color; + grapic_arc->angle_start = angle_start; + grapic_arc->angle_end = angle_end; + grapic_arc->width = width; + grapic_arc->x_start = x_center; + grapic_arc->y_start = y_center; + grapic_arc->x_end = x_semiaxis; + grapic_arc->y_end = y_semiaxis; + return 0; +} + +/** + * @brief UI_绘制浮点数 + * + * @param grapic_float 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param digits 小数点后有效位数 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param float_high 32位浮点数 + * @param float_middle 32位浮点数 + * @param float_low 32位浮点数 + * @return int8_t + */ +int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t digits, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t float_high, + uint16_t float_middle, uint16_t float_low) { + if (grapic_floating == NULL) return -1; + snprintf((char *)grapic_floating->name, 2, "%s", name); + grapic_floating->type_op = type_op; + grapic_floating->type_ele = 5; + grapic_floating->layer = layer; + grapic_floating->color = color; + grapic_floating->angle_start = font_size; + grapic_floating->angle_end = digits; + grapic_floating->width = width; + grapic_floating->x_start = x_start; + grapic_floating->y_start = y_start; + grapic_floating->radius = float_high; + grapic_floating->x_end = float_middle; + grapic_floating->y_end = float_low; + return 0; +} + +/** + * @brief UI_绘制整型数 + * + * @param grapic_integer 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param int32_t_high 32位整型数 + * @param int32_t_middle 32位整型数 + * @param int32_t_low 32位整型数 + * @return int8_t + */ +int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t width, uint16_t x_start, + uint16_t y_start, uint16_t int32_t_high, + uint16_t int32_t_middle, uint16_t int32_t_low) { + if (grapic_integer == NULL) return -1; + snprintf((char *)grapic_integer->name, 2, "%s", name); + grapic_integer->type_op = type_op; + grapic_integer->type_ele = 6; + grapic_integer->layer = layer; + grapic_integer->color = color; + grapic_integer->angle_start = font_size; + grapic_integer->width = width; + grapic_integer->x_start = x_start; + grapic_integer->y_start = y_start; + grapic_integer->radius = int32_t_high; + grapic_integer->x_end = int32_t_middle; + grapic_integer->y_end = int32_t_low; + return 0; +} + +/** + * @brief UI_绘制字符 + * + * @param grapic_character 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param length 字符长度 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param character 字符串首地址 + * @return int8_t + */ +int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t length, uint16_t width, + uint16_t x_start, uint16_t y_start, + const char *character) { + if (grapic_character == NULL) return -1; + snprintf((char *)grapic_character->grapic.name, 2, "%s", name); + grapic_character->grapic.type_op = type_op; + grapic_character->grapic.type_ele = 7; + grapic_character->grapic.layer = layer; + grapic_character->grapic.color = color; + grapic_character->grapic.angle_start = font_size; + grapic_character->grapic.angle_end = length; + grapic_character->grapic.width = width; + grapic_character->grapic.x_start = x_start; + grapic_character->grapic.y_start = y_start; + snprintf((char *)grapic_character->character, 29, "%s", character); + return 0; +} + +/** + * @brief UI_删除图层 + * + * @param del 结构体 + * @param opt 操作 + * @param layer 图层 + * @return int8_t + */ +int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) { + if (del == NULL) return -1; + del->del_operation = opt; + del->layer = layer; + return 0; +} \ No newline at end of file diff --git a/assets/User_code/component/ui.h b/assets/User_code/component/ui.h new file mode 100644 index 0000000..4f742d3 --- /dev/null +++ b/assets/User_code/component/ui.h @@ -0,0 +1,284 @@ +/* + UI相关命令 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define UI_DEL_OPERATION_NOTHING (0) +#define UI_DEL_OPERATION_DEL (1) +#define UI_DEL_OPERATION_DEL_ALL (2) + +#define UI_GRAPIC_OPERATION_NOTHING (0) +#define UI_GRAPIC_OPERATION_ADD (1) +#define UI_GRAPIC_OPERATION_REWRITE (2) +#define UI_GRAPIC_OPERATION_DEL (3) + +#define UI_GRAPIC_LAYER_CONST (0) +#define UI_GRAPIC_LAYER_AUTOAIM (1) +#define UI_GRAPIC_LAYER_CHASSIS (2) +#define UI_GRAPIC_LAYER_CAP (3) +#define UI_GRAPIC_LAYER_GIMBAL (4) +#define UI_GRAPIC_LAYER_SHOOT (5) +#define UI_GRAPIC_LAYER_CMD (6) + +#define UI_DEFAULT_WIDTH (0x01) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ +#define UI_CHAR_DEFAULT_WIDTH (0x02) + +typedef enum { + RED_BLUE, + YELLOW, + GREEN, + ORANGE, + PURPLISH_RED, + PINK, + CYAN, + BLACK, + WHITE +} UI_Color_t; + +typedef struct __packed { + uint8_t op; + uint8_t num_layer; +} UI_InterStudent_UIDel_t; + +typedef struct __packed { + uint8_t name[3]; + uint8_t type_op : 3; + uint8_t type_ele : 3; + uint8_t layer : 4; + uint8_t color : 4; + uint16_t angle_start : 9; + uint16_t angle_end : 9; + uint16_t width : 10; + uint16_t x_start : 11; + uint16_t y_start : 11; + uint16_t radius : 10; + uint16_t x_end : 11; + uint16_t y_end : 11; +} UI_Ele_t; + +typedef struct __packed { + UI_Ele_t grapic; +} UI_Drawgrapic_1_t; + +typedef struct __packed { + UI_Ele_t grapic[2]; +} UI_Drawgrapic_2_t; + +typedef struct __packed { + UI_Ele_t grapic[5]; +} UI_Drawgrapic_5_t; + +typedef struct __packed { + UI_Ele_t grapic[7]; +} UI_Drawgrapic_7_t; + +typedef struct __packed { + UI_Ele_t grapic; + uint8_t character[30]; +} UI_Drawcharacter_t; + +typedef struct __packed { + uint8_t del_operation; + uint8_t layer; +} UI_Del_t; + +/** + * @brief UI_绘制直线段 + * + * @param grapic_line 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 终点x坐标 + * @param y_end 终点y坐标 + * @return int8_t + */ +int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t x_end, + uint16_t y_end); + +/** + * @brief UI_绘制矩形 + * + * @param grapic_rectangle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 对角顶点x坐标 + * @param y_end 对角顶点y坐标 + * @return int8_t + */ +int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t width, uint16_t x_start, uint16_t y_start, + uint16_t x_end, uint16_t y_end); + +/** + * @brief UI_绘制正圆 + * + * @param grapic_cycle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param radius 半径 + * @return int8_t + */ +int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t radius); + +/** + * @brief UI_绘制椭圆 + * + * @param grapic_oval 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis, + uint16_t y_semiaxis); + +/** + * @brief UI_绘制圆弧 + * + * @param grapic_arc 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param angle_start 起始角度 + * @param angle_end 终止角度 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t angle_start, + uint16_t angle_end, uint16_t width, uint16_t x_center, + uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis); + +/** + * @brief UI_绘制浮点数 + * + * @param grapic_float 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param digits 小数点后有效位数 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param float_high 32位浮点数 + * @param float_middle 32位浮点数 + * @param float_low 32位浮点数 + * @return int8_t + */ +int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t digits, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t float_high, + uint16_t float_middle, uint16_t float_low); + +/** + * @brief UI_绘制整型数 + * + * @param grapic_integer 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param int32_t_high 32位整型数 + * @param int32_t_middle 32位整型数 + * @param int32_t_low 32位整型数 + * @return int8_t + */ +int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t width, uint16_t x_start, + uint16_t y_start, uint16_t int32_t_high, + uint16_t int32_t_middle, uint16_t int32_t_low); + +/** + * @brief UI_绘制字符 + * + * @param grapic_character 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param length 字符长度 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param character 字符串首地址 + * @return int8_t + */ +int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t length, uint16_t width, + uint16_t x_start, uint16_t y_start, + const char *character); + +/** + * @brief UI_删除图层 + * + * @param del 结构体 + * @param opt 操作 + * @param layer 图层 + * @return int8_t + */ +int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/component/user_math.c b/assets/User_code/component/user_math.c new file mode 100644 index 0000000..5e0b0c4 --- /dev/null +++ b/assets/User_code/component/user_math.c @@ -0,0 +1,134 @@ +/* + 自定义的数学运算。 +*/ + +#include "user_math.h" +#include +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +inline float InvSqrt(float x) { +//#if 0 + /* Fast inverse square-root */ + /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */ + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; +//#else +// return 1.0f / sqrtf(x); +//#endif +} + +inline float AbsClip(float in, float limit) { + return (in < -limit) ? -limit : ((in > limit) ? limit : in); +} + +float fAbs(float in){ + return (in > 0) ? in : -in; +} + +inline void Clip(float *origin, float min, float max) { + if (*origin > max) *origin = max; + if (*origin < min) *origin = min; +} + +inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; } + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); } + +/** + * \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) { + float error = sp - fb; + if (range > 0.0f) { + float half_range = range / 2.0f; + + if (error > half_range) + error -= range; + else if (error < -half_range) + error += range; + } + return error; +} + +/** + * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况 + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +inline void CircleAdd(float *origin, float delta, float range) { + float out = *origin + delta; + if (range > 0.0f) { + if (out >= range) + out -= range; + else if (out < 0.0f) + out += range; + } + *origin = out; +} + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; } + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { + if (bullet_speed == 0.0f) return 0.f; + if (is17mm) { + if (bullet_speed == 15.0f) return 4670.f; + if (bullet_speed == 18.0f) return 5200.f; + if (bullet_speed == 30.0f) return 7350.f; + } else { + if (bullet_speed == 10.0f) return 4450.f; + if (bullet_speed == 16.0f) return 5800.f; + } + + /* 不为裁判系统设定值时,计算转速 */ + return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); +} + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line) { +// UNUSED(file); +// UNUSED(line); +// while (1) { +// __NOP(); +// } +// } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/assets/User_code/component/user_math.h b/assets/User_code/component/user_math.h new file mode 100644 index 0000000..6e61ca2 --- /dev/null +++ b/assets/User_code/component/user_math.h @@ -0,0 +1,179 @@ +/* + 自定义的数学运算。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#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_2PI +#define M_2PI 6.28318530717958647692f +#endif + +#ifndef __packed + #define __packed __attribute__((__packed__)) +#endif /* __packed */ + +#define max(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a > _b ? _a : _b; \ + }) + +#define min(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a < _b ? _a : _b; \ + }) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + + + +/* 移动向量 */ +typedef struct { + float vx; /* 前后平移 */ + float vy; /* 左右平移 */ + float wz; /* 转动 */ +} MoveVector_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +float InvSqrt(float x); + +float AbsClip(float in, float limit); + +float fAbs(float in); + +void Clip(float *origin, float min, float max); + +float Sign(float in); + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +void ResetMoveVector(MoveVector_t *mv); + +/** + * \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,range）范围内循环的情况 + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +void CircleAdd(float *origin, float delta, float range); + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +void CircleReverse(float *origin); + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm); + +#ifdef __cplusplus +} +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define ASSERT(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 未定DEBUG，表达式不会运行，断言被忽略 + * + */ +#define ASSERT(expr) ((void)(0)) +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define VERIFY(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 表达式会运行，忽略表达式结果 + * + */ +#define VERIFY(expr) ((void)(expr)) +#endif + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/assets/User_code/config.csv b/assets/User_code/config.csv new file mode 100644 index 0000000..db5a049 --- /dev/null +++ b/assets/User_code/config.csv @@ -0,0 +1,4 @@ +bsp,can,dwt,gpio,i2c,mm,spi,uart,pwm,time +component,ahrs,capacity,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math +device,dr16,bmi088,ist8310,motor,motor_rm,motor_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,ai +module,config, \ No newline at end of file diff --git a/assets/User_code/device/.DS_Store b/assets/User_code/device/.DS_Store new file mode 100644 index 0000000..0e3c551 Binary files /dev/null and b/assets/User_code/device/.DS_Store differ diff --git a/assets/User_code/device/.gitkeep b/assets/User_code/device/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/assets/User_code/device/ai.c b/assets/User_code/device/ai.c new file mode 100644 index 0000000..2632536 --- /dev/null +++ b/assets/User_code/device/ai.c @@ -0,0 +1,142 @@ +/* +AI +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "ai.h" + +#include +#include + +#include "bsp/time.h" +#include "bsp/uart.h" +#include "component/ahrs.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(AI_DownPackage_t)) + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +static uint8_t rxbuf[AI_LEN_RX_BUFF]; + +static bool inited = false; + +static osThreadId_t thread_alert; + +static uint32_t drop_message = 0; + +// uint16_t crc16; + +/* 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); + if (inited) return DEVICE_ERR_INITED; + thread_alert = osThreadGetId(); + + 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(AI_t *ai) { + UNUSED(ai); + __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) + if (BSP_UART_Receive(BSP_UART_AI, rxbuf, + AI_LEN_RX_BUFF, true) == 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) { + // crc16 = CRC16_Calc((const uint8_t *)&(rxbuf), sizeof(ai->from_host) - 2, CRC16_INIT); + if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host))) + goto error; + ai->header.online = true; + ai->header.last_online_time = BSP_TIME_Get(); + 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; +} + +int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *data){ + if (ai == NULL || data == NULL) return DEVICE_ERR_NULL; + ai->to_host.mcu.id = AI_ID_MCU; + ai->to_host.mcu.package.quat=*data; + ai->to_host.mcu.package.notice = ai->status; + ai->to_host.mcu.crc16 = CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2, CRC16_INIT); + return DEVICE_OK; +} + +int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data) { + if (ai == NULL || data == NULL) return DEVICE_ERR_NULL; + ai->to_host.ref = *data; + return DEVICE_OK; +} + +int8_t AI_HandleOffline(AI_t *ai) { + if (ai == NULL) return DEVICE_ERR_NULL; + if (BSP_TIME_Get() - ai->header.last_online_time > + 100000) { + ai->header.online = false; + } + return DEVICE_OK; +} + +int8_t AI_StartSend(AI_t *ai, bool ref_online){ + if (ai == NULL) return DEVICE_ERR_NULL; + + if (ref_online) { + // 发送裁判系统数据和MCU数据 + if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host), + sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu), true) == HAL_OK) + return DEVICE_OK; + else + return DEVICE_ERR; + } else { + // 只发送MCU数据 + if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host.mcu), + sizeof(ai->to_host.mcu), true) == HAL_OK) + return DEVICE_OK; + else + return DEVICE_ERR; + } +} + diff --git a/assets/User_code/device/ai.h b/assets/User_code/device/ai.h new file mode 100644 index 0000000..24b2a31 --- /dev/null +++ b/assets/User_code/device/ai.h @@ -0,0 +1,131 @@ +/* + 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 ----------------------------------------------------------- */ +#define AI_ID_MCU (0xC4) +#define AI_ID_REF (0xA8) +#define AI_ID_AI (0xA1) +/* Exported types ----------------------------------------------------------- */ + +typedef enum { + AI_ARMOR_HERO = 0, /*英雄机器人*/ + AI_ARMOR_INFANTRY, /*步兵机器人*/ + AI_ARMOR_SENTRY, /*哨兵机器人*/ + AI_ARMOR_ENGINEER, /*工程机器人*/ + AI_ARMOR_OUTPOST, /*前哨占*/ + AI_ARMOR_BASE, /*基地*/ + AI_ARMOR_NORMAL, /*由AI自动选择*/ +} AI_ArmorsType_t; + +typedef enum { + AI_STATUS_OFF = 0, /* 关闭 */ + AI_STATUS_AUTOAIM, /* 自瞄 */ + AI_STATUS_AUTOPICK, /* 自动取矿 */ + AI_STATUS_AUTOPUT, /* 自动兑矿 */ + AI_STATUS_AUTOHITBUFF, /* 自动打符 */ + AI_STATUS_AUTONAV, +} AI_Status_t; + +typedef enum { + AI_NOTICE_NONE = 0, + AI_NOTICE_SEARCH, + AI_NOTICE_FIRE, +}AI_Notice_t; + +/* 电控 -> 视觉 MCU数据结构体*/ +typedef struct __packed { + AHRS_Quaternion_t quat; /* 四元数 */ + // struct { + // AI_ArmorsType_t armor_type; + // AI_Status_t status; + // }notice; /* 控制命令 */ + uint8_t notice; +} AI_Protucol_UpDataMCU_t; + +/* 电控 -> 视觉 裁判系统数据结构体*/ +typedef struct __packed { + /* USER REFEREE BEGIN */ + uint16_t team; /* 本身队伍 */ + uint16_t time; /* 比赛开始时间 */ + /* USER REFEREE END */ +} 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_Protocol_DownData_t package; /* 数据包 */ + uint16_t crc16; /* CRC16校验 */ +} AI_DownPackage_t; + +typedef struct __packed { + DEVICE_Header_t header; /* 设备通用头部 */ + AI_DownPackage_t from_host; + AI_Status_t status; + 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(void); + +int8_t AI_ParseHost(AI_t *ai); + +int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat); + +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/assets/User_code/device/bmi088.c b/assets/User_code/device/bmi088.c new file mode 100644 index 0000000..0f4c154 --- /dev/null +++ b/assets/User_code/device/bmi088.c @@ -0,0 +1,381 @@ +/* + BMI088 陀螺仪+加速度计传感器。 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "bmi088.h" + +#include +#include +#include +#include + +#include "bsp/time.h" +#include "bsp/gpio.h" +#include "bsp/spi.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +#define BMI088_REG_ACCL_CHIP_ID (0x00) +#define BMI088_REG_ACCL_ERR (0x02) +#define BMI088_REG_ACCL_STATUS (0x03) +#define BMI088_REG_ACCL_X_LSB (0x12) +#define BMI088_REG_ACCL_X_MSB (0x13) +#define BMI088_REG_ACCL_Y_LSB (0x14) +#define BMI088_REG_ACCL_Y_MSB (0x15) +#define BMI088_REG_ACCL_Z_LSB (0x16) +#define BMI088_REG_ACCL_Z_MSB (0x17) +#define BMI088_REG_ACCL_SENSORTIME_0 (0x18) +#define BMI088_REG_ACCL_SENSORTIME_1 (0x19) +#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A) +#define BMI088_REG_ACCL_INT_STAT_1 (0x1D) +#define BMI088_REG_ACCL_TEMP_MSB (0x22) +#define BMI088_REG_ACCL_TEMP_LSB (0x23) +#define BMI088_REG_ACCL_CONF (0x40) +#define BMI088_REG_ACCL_RANGE (0x41) +#define BMI088_REG_ACCL_INT1_IO_CONF (0x53) +#define BMI088_REG_ACCL_INT2_IO_CONF (0x54) +#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58) +#define BMI088_REG_ACCL_SELF_TEST (0x6D) +#define BMI088_REG_ACCL_PWR_CONF (0x7C) +#define BMI088_REG_ACCL_PWR_CTRL (0x7D) +#define BMI088_REG_ACCL_SOFTRESET (0x7E) + +#define BMI088_REG_GYRO_CHIP_ID (0x00) +#define BMI088_REG_GYRO_X_LSB (0x02) +#define BMI088_REG_GYRO_X_MSB (0x03) +#define BMI088_REG_GYRO_Y_LSB (0x04) +#define BMI088_REG_GYRO_Y_MSB (0x05) +#define BMI088_REG_GYRO_Z_LSB (0x06) +#define BMI088_REG_GYRO_Z_MSB (0x07) +#define BMI088_REG_GYRO_INT_STAT_1 (0x0A) +#define BMI088_REG_GYRO_RANGE (0x0F) +#define BMI088_REG_GYRO_BANDWIDTH (0x10) +#define BMI088_REG_GYRO_LPM1 (0x11) +#define BMI088_REG_GYRO_SOFTRESET (0x14) +#define BMI088_REG_GYRO_INT_CTRL (0x15) +#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16) +#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18) +#define BMI088_REG_GYRO_SELF_TEST (0x3C) + +#define BMI088_CHIP_ID_ACCL (0x1E) +#define BMI088_CHIP_ID_GYRO (0x0F) + +#define BMI088_LEN_RX_BUFF (19) +/* Private macro ------------------------------------------------------------ */ +#define BMI088_ACCL_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET) +#define BMI088_ACCL_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET) + +#define BMI088_GYRO_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET) +#define BMI088_GYRO_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET) + +/* Private typedef ---------------------------------------------------------- */ +typedef enum { + BMI_ACCL, + BMI_GYRO, +} BMI_Device_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static uint8_t buffer[2]; +static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF]; + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) { + buffer[0] = (reg & 0x7f); + buffer[1] = data; + + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + break; + } +} + +static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) { + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + return buffer[1]; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + return buffer[0]; + } +} + +static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) { + if (data == NULL) return; + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true); +} + +static void BMI088_RxCpltCallback(void) { + if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) { + BMI088_ACCL_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY); + } + if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) { + BMI088_GYRO_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY); + } +} + +static void BMI088_AcclIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA); +} + +static void BMI088_GyroIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + if (cali == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + bmi088->cali = cali; + + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6); + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6); + BSP_TIME_Delay(30); + + /* Switch accl to SPI mode. */ + BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL); + + if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL) + return DEVICE_ERR_NO_DEV; + + if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO) + return DEVICE_ERR_NO_DEV; + + BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT); + + BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB, + BMI088_RxCpltCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback); + + /* Accl init. */ + /* Filter setting: Normal. */ + /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA); + + /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01); + + /* INT1 as output. Push-pull. Active low. Output. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08); + + /* Map data ready interrupt to INT1. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04); + + /* Turn on accl. Now we can read data. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04); + BSP_TIME_Delay(50); + + /* Gyro init. */ + /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01); + + /* Filter bw: 47Hz. */ + /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03); + + /* INT3 and INT4 as output. Push-pull. Active low. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00); + + /* Map data ready interrupt to INT3. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01); + + /* Enable new data interrupt. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80); + + BSP_TIME_Delay(10); + + inited = true; + + BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT); + return DEVICE_OK; +} + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro) { + return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f)); +} + +uint32_t BMI088_WaitNew() { + return osThreadFlagsWait( + SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_AcclStartDmaRecv() { + BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF); + return DEVICE_OK; +} + +uint32_t BMI088_AcclWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_GyroStartDmaRecv() { + BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u); + return DEVICE_OK; +} + +uint32_t BMI088_GyroWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_ParseAccl(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z)); + + bmi088->accl.x = (float)raw_x; + bmi088->accl.y = (float)raw_y; + bmi088->accl.z = (float)raw_z; + +#else + const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1); + const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3); + const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5); + + bmi088->accl.x = (float)*praw_x; + bmi088->accl.y = (float)*praw_y; + bmi088->accl.z = (float)*praw_z; + +#endif + + /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */ + bmi088->accl.x /= 5460.0f; + bmi088->accl.y /= 5460.0f; + bmi088->accl.z /= 5460.0f; + + int16_t raw_temp = + (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5)); + + if (raw_temp > 1023) raw_temp -= 2048; + + bmi088->temp = (float)raw_temp * 0.125f + 23.0f; + + return DEVICE_OK; +} + +int8_t BMI088_ParseGyro(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z)); + + bmi088->gyro.x = (float)raw_x; + bmi088->gyro.y = (float)raw_y; + bmi088->gyro.z = (float)raw_z; + +#else + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7); + const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9); + const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11); + + bmi088->gyro.x = (float)*raw_x; + bmi088->gyro.y = (float)*raw_y; + bmi088->gyro.z = (float)*raw_z; +#endif + + /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768. + * FS2000: 16.384.*/ + bmi088->gyro.x /= 32.768f; + bmi088->gyro.y /= 32.768f; + bmi088->gyro.z /= 32.768f; + + bmi088->gyro.x *= M_DEG2RAD_MULT; + bmi088->gyro.y *= M_DEG2RAD_MULT; + bmi088->gyro.z *= M_DEG2RAD_MULT; + + bmi088->gyro.x -= bmi088->cali->gyro_offset.x; + bmi088->gyro.y -= bmi088->cali->gyro_offset.y; + bmi088->gyro.z -= bmi088->cali->gyro_offset.z; + + return DEVICE_ERR_NULL; +} + +float BMI088_GetUpdateFreq(BMI088_t *bmi088) { + (void)bmi088; + return 400.0f; +} diff --git a/assets/User_code/device/bmi088.h b/assets/User_code/device/bmi088.h new file mode 100644 index 0000000..eb44e0c --- /dev/null +++ b/assets/User_code/device/bmi088.h @@ -0,0 +1,81 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "component/ahrs.h" +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + struct { + float x; + float y; + float z; + } gyro_offset; /* 陀螺仪偏置 */ +} BMI088_Cali_t; /* BMI088校准数据 */ + +typedef struct { + DEVICE_Header_t header; + AHRS_Accl_t accl; + AHRS_Gyro_t gyro; + + float temp; /* 温度 */ + + const BMI088_Cali_t *cali; +} BMI088_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali); +int8_t BMI088_Restart(void); + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro); + +/* Sensor use right-handed coordinate system. */ +/* + x < R(logo) + y + UP is z + All implementation should follow this rule. + */ +uint32_t BMI088_WaitNew(); + +/* + BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。 + 即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能 + 出现 BMI088_GyroStartDmaRecv()。 +*/ +int8_t BMI088_AcclStartDmaRecv(); +uint32_t BMI088_AcclWaitDmaCplt(); +int8_t BMI088_GyroStartDmaRecv(); +uint32_t BMI088_GyroWaitDmaCplt(); +int8_t BMI088_ParseAccl(BMI088_t *bmi088); +int8_t BMI088_ParseGyro(BMI088_t *bmi088); +float BMI088_GetUpdateFreq(BMI088_t *bmi088); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/buzzer.c b/assets/User_code/device/buzzer.c new file mode 100644 index 0000000..cfaf777 --- /dev/null +++ b/assets/User_code/device/buzzer.c @@ -0,0 +1,171 @@ +#include "device/buzzer.h" +#include "bsp/time.h" +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define MUSIC_DEFAULT_VOLUME 0.5f +#define MUSIC_A4_FREQ 440.0f // A4音符频率 + +/* USER MUSIC MENU BEGIN */ +// RM音乐 +const Tone_t RM[] = { + {NOTE_B, 5, 200}, + {NOTE_G, 4, 200}, + {NOTE_B, 5, 400}, + {NOTE_G, 4, 200}, + {NOTE_B, 5, 400}, + {NOTE_G, 4, 200}, + {NOTE_D, 5, 400}, + {NOTE_G, 4, 200}, + {NOTE_C, 5, 200}, + {NOTE_C, 5, 200}, + {NOTE_G, 4, 200}, + {NOTE_B, 5, 200}, + {NOTE_C, 5, 200} +}; + +// Nokia 经典铃声音符 +const Tone_t NOKIA[] = { + {NOTE_E, 5, 125}, {NOTE_D, 5, 125}, {NOTE_FS, 4, 250}, {NOTE_GS, 4, 250}, + {NOTE_CS, 5, 125}, {NOTE_B, 4, 125}, {NOTE_D, 4, 250}, {NOTE_E, 4, 250}, + {NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250}, + {NOTE_A, 4, 500} +}; +/* USER MUSIC MENU END */ + +static void BUZZER_Update(BUZZER_t *buzzer){ + buzzer->header.online = true; + buzzer->header.last_online_time = BSP_TIME_Get_ms(); +} + +// 根据音符和八度计算频率的辅助函数 +static float BUZZER_CalcFreq(NOTE_t note, uint8_t octave) { + if (note == NOTE_REST) { + return 0.0f; // 休止符返回0频率 + } + + // 将音符和八度转换为MIDI音符编号 + int midi_num = (int)note + (int)((octave + 1) * 12); + + // 使用A4 (440Hz) 作为参考，计算频率 + // 公式: freq = 440 * 2^((midi_num - 69)/12) + float freq = 440.0f * powf(2.0f, ((float)midi_num - 69.0f) / 12.0f); + + return freq; +} + +// 播放单个音符 +static int8_t BUZZER_PlayTone(BUZZER_t *buzzer, NOTE_t note, uint8_t octave, uint16_t duration_ms) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + float freq = BUZZER_CalcFreq(note, octave); + + if (freq > 0.0f) { + // 播放音符 + if (BUZZER_Set(buzzer, freq, MUSIC_DEFAULT_VOLUME) != DEVICE_OK) + return DEVICE_ERR; + + if (BUZZER_Start(buzzer) != DEVICE_OK) + return DEVICE_ERR; + } else { + // 休止符，停止播放 + BUZZER_Stop(buzzer); + } + + // 等待指定时间 + BSP_TIME_Delay_ms(duration_ms); + + // 停止当前音符，为下一个音符做准备 + BUZZER_Stop(buzzer); + BSP_TIME_Delay_ms(20); // 短暂间隔 + + return DEVICE_OK; +} + +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) { + if (buzzer == NULL) return DEVICE_ERR; + + buzzer->channel = channel; + buzzer->header.online = true; + + BUZZER_Stop(buzzer); + + return DEVICE_OK ; +} + +int8_t BUZZER_Start(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + BUZZER_Update(buzzer); + return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Stop(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + BUZZER_Update(buzzer); + return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + int result = DEVICE_OK ; + BUZZER_Update(buzzer); + if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK) + result = DEVICE_ERR; + + if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK) + result = DEVICE_ERR; + + return result; +} + +int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + const Tone_t *melody = NULL; + size_t melody_length = 0; + + // 根据音乐类型选择对应的音符数组 + switch (music) { + case MUSIC_RM: + melody = RM; + melody_length = sizeof(RM) / sizeof(Tone_t); + break; + case MUSIC_NOKIA: + melody = NOKIA; + melody_length = sizeof(NOKIA) / sizeof(Tone_t); + break; + default: + return DEVICE_ERR; + } + + // 播放整首音乐 + for (size_t i = 0; i < melody_length; i++) { + if (BUZZER_PlayTone(buzzer, melody[i].note, melody[i].octave, melody[i].duration_ms) != DEVICE_OK) { + BUZZER_Stop(buzzer); // 出错时停止播放 + return DEVICE_ERR; + } + } + + // 音乐播放完成后停止 + BUZZER_Stop(buzzer); + return DEVICE_OK; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/assets/User_code/device/buzzer.h b/assets/User_code/device/buzzer.h new file mode 100644 index 0000000..0c3a593 --- /dev/null +++ b/assets/User_code/device/buzzer.h @@ -0,0 +1,138 @@ +/** + * @file buzzer.h + * @brief 蜂鸣器设备驱动头文件 + * @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放 + * @author Generated by STM32CubeMX + * @date 2025年10月23日 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "bsp/pwm.h" // PWM底层硬件抽象层 +#include "device.h" // 设备通用头文件 +#include // 标准定义 +#include // 标准整型定义 + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/** + * @brief 音符枚举类型 + * @details 定义标准十二平均律音符，用于音乐播放 + */ +typedef enum { + NOTE_C = 0, ///< Do音符 + NOTE_CS = 1, ///< Do#音符 (升Do) + NOTE_D = 2, ///< Re音符 + NOTE_DS = 3, ///< Re#音符 (升Re) + NOTE_E = 4, ///< Mi音符 + NOTE_F = 5, ///< Fa音符 + NOTE_FS = 6, ///< Fa#音符 (升Fa) + NOTE_G = 7, ///< Sol音符 + NOTE_GS = 8, ///< Sol#音符 (升Sol) + NOTE_A = 9, ///< La音符 + NOTE_AS = 10, ///< La#音符 (升La) + NOTE_B = 11, ///< Si音符 + NOTE_REST = 255 ///< 休止符 (无声音) +} NOTE_t; + +/** + * @brief 音调结构体 + * @details 定义一个完整的音调信息，包括音符、八度和持续时间 + */ +typedef struct { + NOTE_t note; ///< 音符名称 (使用NOTE_t枚举) + uint8_t octave; ///< 八度 (0-8，通常使用3-7) + uint16_t duration_ms; ///< 持续时间，单位毫秒 +} Tone_t; + +/** + * @brief 预设音乐枚举类型 + * @details 定义可播放的预设音乐类型 + */ +typedef enum { + /* USER MUSIC MENU BEGIN */ + MUSIC_RM, ///< RM战队音乐 + MUSIC_NOKIA, ///< 诺基亚经典铃声 + /* USER MUSIC MENU END */ +} MUSIC_t; + +/** + * @brief 蜂鸣器设备结构体 + * @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道 + */ +typedef struct { + DEVICE_Header_t header; ///< 设备通用头信息 (在线状态、时间戳等) + BSP_PWM_Channel_t channel; ///< PWM输出通道 +} BUZZER_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化蜂鸣器设备 + * @param buzzer 蜂鸣器设备结构体指针 + * @param channel PWM输出通道 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 初始化后蜂鸣器处于停止状态 + */ +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); + +/** + * @brief 启动蜂鸣器播放 + * @param buzzer 蜂鸣器设备结构体指针 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 需要先调用BUZZER_Set设置频率和占空比 + */ +int8_t BUZZER_Start(BUZZER_t *buzzer); + +/** + * @brief 停止蜂鸣器播放 + * @param buzzer 蜂鸣器设备结构体指针 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + */ +int8_t BUZZER_Stop(BUZZER_t *buzzer); + +/** + * @brief 设置蜂鸣器频率和占空比 + * @param buzzer 蜂鸣器设备结构体指针 + * @param freq 频率 (Hz)，通常范围20Hz-20kHz + * @param duty_cycle 占空比 (0.0-1.0)，影响音量大小 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 设置后需要调用BUZZER_Start才能听到声音 + */ +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); + +/** + * @brief 播放预设音乐 + * @param buzzer 蜂鸣器设备结构体指针 + * @param music 音乐类型 (使用MUSIC_t枚举) + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 这是一个阻塞函数，会播放完整首音乐后返回 + */ +int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/config.yaml b/assets/User_code/device/config.yaml new file mode 100644 index 0000000..a24a6f6 --- /dev/null +++ b/assets/User_code/device/config.yaml @@ -0,0 +1,227 @@ +devices: + dr16: + name: "DR16" + description: "大疆遥控器接收机" + dependencies: + bsp: ["uart"] + component: ["user_math"] + bsp_requirements: + - type: "uart" + var_name: "BSP_UART_DR16" # 需要替换的变量名 + description: "用于接收遥控器数据" + thread_signals: + - name: "SIGNAL_DR16_RAW_REDY" + files: + header: "dr16.h" + source: "dr16.c" + + ops9: + name: "OPS9" + description: "ACTION OPS9 码盘" + dependencies: + bsp: ["uart"] + component: ["user_math"] + bsp_requirements: + - type: "uart" + var_name: "BSP_UART_OPS9" # 需要替换的变量名 + description: "用于接收码盘" + thread_signals: + - name: "SIGNAL_OPS9_RAW_REDY" + files: + header: "ops9.h" + source: "ops9.c" + + bmi088: + name: "BMI088" + description: "BMI088 陀螺仪+加速度计传感器" + dependencies: + bsp: ["spi", "gpio"] + component: ["user_math"] + bsp_requirements: + - type: "spi" + var_name: "BSP_SPI_BMI088" + description: "用于与 BMI088 通信的 SPI 总线" + - type: "gpio" + var_name: "BSP_GPIO_ACCL_CS" + description: "加速度计片选输出引脚" + gpio_type: "output" + - type: "gpio" + var_name: "BSP_GPIO_GYRO_CS" + description: "陀螺仪片选输出引脚" + gpio_type: "output" + - type: "gpio" + var_name: "BSP_GPIO_ACCL_INT" + description: "加速度计中断输入引脚" + gpio_type: "EXTI" + - type: "gpio" + var_name: "BSP_GPIO_GYRO_INT" + description: "陀螺仪中断输入引脚" + gpio_type: "EXTI" + thread_signals: + - name: "SIGNAL_BMI088_ACCL_RAW_REDY" + - name: "SIGNAL_BMI088_GYRO_RAW_REDY" + - name: "SIGNAL_BMI088_ACCL_NEW_DATA" + - name: "SIGNAL_BMI088_GYRO_NEW_DATA" + files: + header: "bmi088.h" + source: "bmi088.c" + + ist8310: + name: "IST8310" + description: "IST8310 地磁传感器" + dependencies: + bsp: ["i2c", "gpio"] + component: [] + bsp_requirements: + - type: "i2c" + var_name: "BSP_I2C_COMP" + description: "用于与 IST8310 通信的 I2C 总线" + - type: "gpio" + var_name: "CMPS_RST_Pin" + description: "IST8310 复位引脚" + gpio_type: "output" + - type: "gpio" + var_name: "CMPS_INT_Pin" + description: "IST8310 数据中断引脚" + gpio_type: "EXTI" + thread_signals: + - name: "SIGNAL_IST8310_MAGN_RAW_REDY" + - name: "SIGNAL_IST8310_MAGN_NEW_DATA" + files: + header: "ist8310.h" + source: "ist8310.c" + + motor_vesc: + name: "VESC 电调" + description: "VESC 电调驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + files: + header: "motor_vesc.h" + source: "motor_vesc.c" + + motor_odrive: + name: "ODrive 电机" + description: "ODrive 电机驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + files: + header: "motor_odrive.h" + source: "motor_odrive.c" + + motor_rm: + name: "RM 电机" + description: "RM 电机驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + files: + header: "motor_rm.h" + source: "motor_rm.c" + + motor: + name: "通用电机" + description: "通用电机驱动" + dependencies: + bsp: [] + component: [] + bsp_requirements: [] + thread_signals: [] + files: + header: "motor.h" + source: "motor.c" + + ws2812: + name: "WS2812 LED 灯" + description: "WS2812 RGB LED 灯驱动" + dependencies: + bsp: ["pwm", "time"] + component: [] + thread_signals: [] + files: + header: "ws2812.h" + source: "ws2812.c" + + buzzer: + name: "蜂鸣器" + description: "蜂鸣器驱动" + dependencies: + bsp: ["pwm"] + component: [] + bsp_requirements: + - type: "pwm" + var_name: "BSP_PWM_BUZZER" + description: "用于蜂鸣器的PWM通道" + thread_signals: [] + files: + header: "buzzer.h" + source: "buzzer.c" + + dm_imu: + name: "DM IMU" + description: "DM IMU 传感器" + dependencies: + bsp: ["can", "time"] + component: ["user_math"] + files: + header: "dm_imu.h" + source: "dm_imu.c" + + led: + name: "LED 灯" + description: "LED 灯驱动" + dependencies: + bsp: ["gpio", "pwm"] + component: [] + thread_signals: [] + files: + header: "led.h" + source: "led.c" + + motor_lk: + name: "LK 电机" + description: "LK 电机驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + files: + header: "motor_lk.h" + source: "motor_lk.c" + + motor_lz: + name: "LZ 电机" + description: "LZ 电机驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + files: + header: "motor_lz.h" + source: "motor_lz.c" + + servo: + name: "舵机" + description: "舵机驱动" + dependencies: + bsp: ["pwm"] + component: [] + thread_signals: [] + files: + header: "servo.h" + source: "servo.c" + + vofa: + name: "VOFA" + description: "VOFA 数据传输协议" + dependencies: + bsp: ["uart"] + component: [] + bsp_requirements: + - type: "uart" + var_name: "BSP_UART_VOFA" # 需要替换的变量名 + description: "用于VOFA数据传输" + thread_signals: [] + files: + header: "vofa.h" + source: "vofa.c" \ No newline at end of file diff --git a/assets/User_code/device/device.h b/assets/User_code/device/device.h new file mode 100644 index 0000000..3fd8bf5 --- /dev/null +++ b/assets/User_code/device/device.h @@ -0,0 +1,47 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define DEVICE_OK (0) +#define DEVICE_ERR (-1) +#define DEVICE_ERR_NULL (-2) +#define DEVICE_ERR_INITED (-3) +#define DEVICE_ERR_NO_DEV (-4) + +/* AUTO GENERATED SIGNALS BEGIN */ + +/* AUTO GENERATED SIGNALS END */ + +/* USER SIGNALS BEGIN */ + +/* USER SIGNALS END */ +/*设备层通用Header*/ +typedef struct { + bool online; + uint64_t last_online_time; +} DEVICE_Header_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/dm_imu.c b/assets/User_code/device/dm_imu.c new file mode 100644 index 0000000..e5380d9 --- /dev/null +++ b/assets/User_code/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/assets/User_code/device/dm_imu.h b/assets/User_code/device/dm_imu.h new file mode 100644 index 0000000..3965980 --- /dev/null +++ b/assets/User_code/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/assets/User_code/device/dr16.c b/assets/User_code/device/dr16.c new file mode 100644 index 0000000..120997b --- /dev/null +++ b/assets/User_code/device/dr16.c @@ -0,0 +1,169 @@ +/* + DR16接收机 + Example： + + DR16_Init(&dr16); + + while (1) { + DR16_StartDmaRecv(&dr16); + if (DR16_WaitDmaCplt(20)) { + DR16_ParseData(&dr16); + } else { + DR16_Offline(&dr16); + } +} +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dr16.h" +#include "bsp/uart.h" +#include "bsp/time.h" +#include "device.h" + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Private define ----------------------------------------------------------- */ +#define DR16_CH_VALUE_MIN (364u) +#define DR16_CH_VALUE_MID (1024u) +#define DR16_CH_VALUE_MAX (1684u) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +static void DR16_RxCpltCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); +} + +static bool DR16_DataCorrupted(const DR16_t *dr16) { + if (dr16 == NULL) return DEVICE_ERR_NULL; + + if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if (dr16->raw_data.sw_l == 0) return DEVICE_ERR; + + if (dr16->raw_data.sw_r == 0) return DEVICE_ERR; + + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ +int8_t DR16_Init(DR16_t *dr16) { + if (dr16 == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB, + DR16_RxCpltCallback); + + inited = true; + return DEVICE_OK; +} + +int8_t DR16_Restart(void) { + __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16)); + __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16)); + return DEVICE_OK; +} + +int8_t DR16_StartDmaRecv(DR16_t *dr16) { + if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16), + (uint8_t *)&(dr16->raw_data), + sizeof(dr16->raw_data)) == HAL_OK) + return DEVICE_OK; + return DEVICE_ERR; +} + +bool DR16_WaitDmaCplt(uint32_t timeout) { + return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) == + SIGNAL_DR16_RAW_REDY); +} + +int8_t DR16_ParseData(DR16_t *dr16){ + if (dr16 == NULL) return DEVICE_ERR_NULL; + + if (DR16_DataCorrupted(dr16)) { + return DEVICE_ERR; + } + + dr16->header.online = true; + dr16->header.last_online_time = BSP_TIME_Get_us(); + + memset(&(dr16->data), 0, sizeof(dr16->data)); + + float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN); + + // 解析摇杆数据 + dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range; + + // 解析拨杆位置 + dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l; + dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r; + + // 解析鼠标数据 + dr16->data.mouse.x = dr16->raw_data.x; + dr16->data.mouse.y = dr16->raw_data.y; + dr16->data.mouse.z = dr16->raw_data.z; + + dr16->data.mouse.l_click = dr16->raw_data.press_l; + dr16->data.mouse.r_click = dr16->raw_data.press_r; + + // 解析键盘按键 - 使用union简化代码 + uint16_t key_value = dr16->raw_data.key; + + // 解析键盘位映射（W-B键，位0-15） + for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) { + dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0; + } + + // 解析鼠标点击 + dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click; + dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click; + + // 解析第五通道 + dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range; + + return DEVICE_OK; +} + +int8_t DR16_Offline(DR16_t *dr16){ + if (dr16 == NULL) return DEVICE_ERR_NULL; + + dr16->header.online = false; + memset(&(dr16->data), 0, sizeof(dr16->data)); + + return DEVICE_OK; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/assets/User_code/device/dr16.h b/assets/User_code/device/dr16.h new file mode 100644 index 0000000..03fa526 --- /dev/null +++ b/assets/User_code/device/dr16.h @@ -0,0 +1,117 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "component/user_math.h" +#include "device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct __packed { + uint16_t ch_r_x : 11; + uint16_t ch_r_y : 11; + uint16_t ch_l_x : 11; + uint16_t ch_l_y : 11; + uint8_t sw_r : 2; + uint8_t sw_l : 2; + int16_t x; + int16_t y; + int16_t z; + uint8_t press_l; + uint8_t press_r; + uint16_t key; + uint16_t res; +} DR16_RawData_t; + +typedef enum { + DR16_SW_ERR = 0, + DR16_SW_UP = 1, + DR16_SW_MID = 3, + DR16_SW_DOWN = 2, +} DR16_SwitchPos_t; + +/* 键盘按键值 */ +typedef enum { + DR16_KEY_W = 0, + DR16_KEY_S, + DR16_KEY_A, + DR16_KEY_D, + DR16_KEY_SHIFT, + DR16_KEY_CTRL, + DR16_KEY_Q, + DR16_KEY_E, + DR16_KEY_R, + DR16_KEY_F, + DR16_KEY_G, + DR16_KEY_Z, + DR16_KEY_X, + DR16_KEY_C, + DR16_KEY_V, + DR16_KEY_B, + DR16_L_CLICK, + DR16_R_CLICK, + DR16_KEY_NUM, +} DR16_Key_t; + +typedef struct { + float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ + float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ + float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ + float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ + + float ch_res; /* 第五通道值 */ + + DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */ + DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse; /* 鼠标值 */ + + union { + bool key[DR16_KEY_NUM]; /* 键盘按键值 */ + uint16_t value; /* 键盘按键值的位映射 */ + } keyboard; + + uint16_t res; /* 保留，未启用 */ +} DR16_Data_t; + +typedef struct { + DEVICE_Header_t header; + DR16_RawData_t raw_data; + DR16_Data_t data; +} DR16_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t DR16_Init(DR16_t *dr16); +int8_t DR16_Restart(void); +int8_t DR16_StartDmaRecv(DR16_t *dr16); +bool DR16_WaitDmaCplt(uint32_t timeout); +int8_t DR16_ParseData(DR16_t *dr16); +int8_t DR16_Offline(DR16_t *dr16); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/ist8310.c b/assets/User_code/device/ist8310.c new file mode 100644 index 0000000..fa98506 --- /dev/null +++ b/assets/User_code/device/ist8310.c @@ -0,0 +1,178 @@ +/* + IST8310 地磁传感器。 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "ist8310.h" + +#include +#include +#include + +#include "bsp/time.h" +#include "bsp/gpio.h" +#include "bsp/i2c.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define IST8310_WAI (0x00) +#define IST8310_STAT1 (0x02) +#define IST8310_DATAXL (0x03) +#define IST8310_STAT2 (0x09) +#define IST8310_CNTL1 (0x0A) +#define IST8310_CNTL2 (0x0B) +#define IST8310_STR (0x0C) +#define IST8310_TEMPL (0x1C) +#define IST8310_TEMPH (0x1D) +#define IST8310_AVGCNTL (0x41) +#define IST8310_PDCNTL (0x42) + +#define IST8310_CHIP_ID (0x10) + +#define IST8310_IIC_ADDRESS (0x0E << 1) + +#define IST8310_LEN_RX_BUFF (6) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +#define IST8310_SET() \ + BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_SET) +#define IST8310_RESET() \ + BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_RESET) + +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +uint8_t ist8310_rxbuf[IST8310_LEN_RX_BUFF]; + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static void IST8310_WriteSingle(uint8_t reg, uint8_t data) { + BSP_I2C_MemWriteByte(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data); +} + +static uint8_t IST8310_ReadSingle(uint8_t reg) { + return BSP_I2C_MemReadByte(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg); +} + + +static void IST8310_Read(uint8_t reg, uint8_t *data, uint8_t len) { + if (data == NULL) return; + BSP_I2C_MemRead(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data, len, true); +} + +static void IST8310_MemRxCpltCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_RAW_REDY); +} + +static void IST8310_IntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_NEW_DATA); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali) { + if (ist8310 == NULL) return DEVICE_ERR_NULL; + if (cali == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + ist8310->cali = cali; + + IST8310_RESET(); + BSP_TIME_Delay(50); + IST8310_SET(); + BSP_TIME_Delay(50); + + if (IST8310_ReadSingle(IST8310_WAI) != IST8310_CHIP_ID) + return DEVICE_ERR_NO_DEV; + + BSP_GPIO_DisableIRQ(CMPS_INT_Pin); + + BSP_I2C_RegisterCallback(BSP_I2C_COMP, HAL_I2C_MEM_RX_CPLT_CB, + IST8310_MemRxCpltCallback); + BSP_GPIO_RegisterCallback(CMPS_INT_Pin, IST8310_IntCallback); + + /* Init. */ + /* 0x00: Stand-By mode. 0x01: Single measurement mode. */ + + /* 0x08: Data ready function enable. DRDY signal active low*/ + IST8310_WriteSingle(IST8310_CNTL2, 0x08); + + IST8310_WriteSingle(IST8310_AVGCNTL, 0x09); + IST8310_WriteSingle(IST8310_PDCNTL, 0xC0); + IST8310_WriteSingle(IST8310_CNTL1, 0x0B); + BSP_TIME_Delay(10); + + inited = true; + + BSP_GPIO_EnableIRQ(CMPS_INT_Pin); + return DEVICE_OK; +} + +bool IST8310_WaitNew(uint32_t timeout) { + return (osThreadFlagsWait(SIGNAL_IST8310_MAGN_NEW_DATA, osFlagsWaitAll, + timeout) == SIGNAL_IST8310_MAGN_NEW_DATA); +} + +int8_t IST8310_StartDmaRecv() { + IST8310_Read(IST8310_DATAXL, ist8310_rxbuf, IST8310_LEN_RX_BUFF); + return DEVICE_OK; +} + +uint32_t IST8310_WaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_IST8310_MAGN_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t IST8310_Parse(IST8310_t *ist8310) { + if (ist8310 == NULL) return DEVICE_ERR_NULL; + +#if 1 + /* Magn -> T */ + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, ist8310_rxbuf + 0, sizeof(raw_x)); + memcpy(&raw_y, ist8310_rxbuf + 2, sizeof(raw_y)); + memcpy(&raw_z, ist8310_rxbuf + 4, sizeof(raw_z)); + + ist8310->magn.x = (float)raw_x; + ist8310->magn.y = (float)raw_y; + ist8310->magn.z = (float)-raw_z; + +#else + const int16_t *raw_x = (int16_t *)(ist8310_rxbuf + 0); + const int16_t *raw_y = (int16_t *)(ist8310_rxbuf + 2); + const int16_t *raw_z = (int16_t *)(ist8310_rxbuf + 4); + + ist8310->magn.x = (float)*raw_x; + ist8310->magn.y = (float)*raw_y; + ist8310->magn.z = -(float)*raw_z; +#endif + + ist8310->magn.x *= 3.0f / 20.0f; + ist8310->magn.y *= 3.0f / 20.0f; + ist8310->magn.z *= 3.0f / 20.0f; + + ist8310->magn.x = (ist8310->magn.x - ist8310->cali->magn_offset.x) * + ist8310->cali->magn_scale.x; + ist8310->magn.y = (ist8310->magn.y - ist8310->cali->magn_offset.y) * + ist8310->cali->magn_scale.y; + ist8310->magn.z = (ist8310->magn.z - ist8310->cali->magn_offset.y) * + ist8310->cali->magn_scale.z; + + return DEVICE_OK; +} diff --git a/assets/User_code/device/ist8310.h b/assets/User_code/device/ist8310.h new file mode 100644 index 0000000..df16cdd --- /dev/null +++ b/assets/User_code/device/ist8310.h @@ -0,0 +1,49 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "component/ahrs.h" +#include "device/device.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + struct { + float x; + float y; + float z; + } magn_offset; /* 磁力计偏置 */ + + struct { + float x; + float y; + float z; + } magn_scale; /* 磁力计缩放 */ +} IST8310_Cali_t; /* IST8310校准数据 */ + +typedef struct { + DEVICE_Header_t header; + AHRS_Magn_t magn; + const IST8310_Cali_t *cali; +} IST8310_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali); +int8_t IST8310_Restart(void); + +bool IST8310_WaitNew(uint32_t timeout); +int8_t IST8310_StartDmaRecv(); +uint32_t IST8310_WaitDmaCplt(); +int8_t IST8310_Parse(IST8310_t *ist8310); + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/led.c b/assets/User_code/device/led.c new file mode 100644 index 0000000..f796a1c --- /dev/null +++ b/assets/User_code/device/led.c @@ -0,0 +1,37 @@ +/* + led控制 +*/ +/*Includes -----------------------------------------*/ +#include "device/led.h" +#include "device.h" + + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +#ifdef LED_PWM +int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle) +{ + if (duty_cycle < 0.0f || duty_cycle > 1.0f) { + return DEVICE_ERR_NULL; // 错误：占空比超出范围 + } + uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX); + BSP_PWM_Start(channel); + BSP_PWM_SetComp(channel, pulse); + return DEVICE_OK; +} +#endif + +#ifdef LED_GPIO +int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value) +{ + if (value) { + BSP_GPIO_WritePin(gpio, true); + } else { + BSP_GPIO_WritePin(gpio, false); + } + return DEVICE_OK; +} +#endif + + diff --git a/assets/User_code/device/led.h b/assets/User_code/device/led.h new file mode 100644 index 0000000..421bac1 --- /dev/null +++ b/assets/User_code/device/led.h @@ -0,0 +1,63 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +/* USER DEFIN BEGIN */ + +/* USER DEFIN END */ +#include +#ifdef LED_GPIO +#include "bsp/gpio.h" +#endif +#ifdef LED_PWM +#include "bsp/pwm.h" +#endif +#include "bsp/bsp.h" +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + + +typedef struct { +#ifdef LED_GPIO +BSP_GPIO_t gpio; +#endif +#ifdef LED_PWM +BSP_PWM_Channel_t channel; +#endif +} DEVICE_LED_t; + + + extern DEVICE_LED_t LED_Map; +/* Exported functions prototypes -------------------------------------------- */ +#ifdef LED_PWM +int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle) +{ + if (duty_cycle < 0.0f || duty_cycle > 1.0f) { + return DEVICE_ERR_NULL; // 错误：占空比超出范围 + } + uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX); + BSP_PWM_Start(channel); + BSP_PWM_SetComp(channel, pulse); + return DEVICE_OK; +} +#endif + +#ifdef LED_GPIO +int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value) +{ + if (value) { + BSP_GPIO_WritePin(gpio, true); + } else { + BSP_GPIO_WritePin(gpio, false); + } + return DEVICE_OK; +} +#endif + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/motor.c b/assets/User_code/device/motor.c new file mode 100644 index 0000000..1fb059d --- /dev/null +++ b/assets/User_code/device/motor.c @@ -0,0 +1,52 @@ +/* + 电机通用函数 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "motor.h" + +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/* Exported functions ------------------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.rotor_abs_angle; +} + +float MOTOR_GetRotorSpeed(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.rotor_speed; +} + +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.torque_current; +} + +float MOTOR_GetTemp(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.temp; +} diff --git a/assets/User_code/device/motor.h b/assets/User_code/device/motor.h new file mode 100644 index 0000000..e1f945b --- /dev/null +++ b/assets/User_code/device/motor.h @@ -0,0 +1,68 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + float rotor_abs_angle; /* 转子绝对角度 */ + float rotor_speed; /* 实际转子转速 */ + float torque_current; /* 转矩电流 */ + float temp; /* 温度 */ +} MOTOR_Feedback_t; + +/** + * @brief mit电机输出参数结构体 + */ +typedef struct { + float torque; /* 目标力矩 */ + float velocity; /* 目标速度 */ + float angle; /* 目标位置 */ + float kp; /* 位置环增益 */ + float kd; /* 速度环增益 */ +} MOTOR_MIT_Output_t; + +/** + * @brief 转矩电流控制模式参数结构体 + */ +typedef struct { + float current; /* 目标电流 */ +} MOTOR_Current_Output_t; + +typedef struct { + DEVICE_Header_t header; + bool reverse; /* 是否反装 true表示反装 */ + MOTOR_Feedback_t feedback; +} MOTOR_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor); +float MOTOR_GetRotorSpeed(const MOTOR_t *motor); +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor); +float MOTOR_GetTemp(const MOTOR_t *motor); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/motor_dm.c b/assets/User_code/device/motor_dm.c new file mode 100644 index 0000000..a77656f --- /dev/null +++ b/assets/User_code/device/motor_dm.c @@ -0,0 +1,496 @@ +#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \ + ((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1)))) + +#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \ + (((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits)))) +/* Includes ----------------------------------------------------------------- */ +#include "device/motor_dm.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" +#include "string.h" +#include "math.h" + +/* Private constants -------------------------------------------------------- */ +/* DM电机数据映射范围 */ +#define DM_P_MIN (-12.56637f) +#define DM_P_MAX (12.56637f) +#define DM_V_MIN (-30.0f) +#define DM_V_MAX (30.0f) +#define DM_T_MIN (-12.0f) +#define DM_T_MAX (12.0f) +#define DM_KP_MIN (0.0f) +#define DM_KP_MAX (500.0f) +#define DM_KD_MIN (0.0f) +#define DM_KD_MAX (5.0f) + +/* CAN ID偏移量 */ +#define DM_CAN_ID_OFFSET_POS_VEL 0x100 +#define DM_CAN_ID_OFFSET_VEL 0x200 + +/* Private macro ------------------------------------------------------------ */ +#define FLOAT_TO_UINT(x, x_min, x_max, bits) \ + (uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min)) + +#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \ + ((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min) + + +/* Private variables -------------------------------------------------------- */ +static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +static 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<motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5) + uint16_t v_int=(data[3]<<4)|(data[4]>>4); + motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0) + uint16_t t_int=((data[4]&0xF)<<8)|data[5]; + motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0) + motor->motor.feedback.temp = (float)(data[6]); + + while (motor->motor.feedback.rotor_abs_angle < 0) { + motor->motor.feedback.rotor_abs_angle += M_2PI; + } + while (motor->motor.feedback.rotor_abs_angle >= M_2PI) { + motor->motor.feedback.rotor_abs_angle -= M_2PI; + } + + if (motor->param.reverse) { + motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle; + motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed; + motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current; + } + return DEVICE_OK; +} + +/** + * @brief 发送MIT模式控制命令 + * @param motor 电机实例 + * @param output MIT控制参数 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) { + if (motor == NULL || output == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[8]; + uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp; + uint16_t id = motor->param.can_id; + + pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16); + vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12); + kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12); + kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12); + tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12); + + /* 打包数据 */ + data[0] = (pos_tmp >> 8); + data[1] = pos_tmp; + data[2] = (vel_tmp >> 4); + data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8); + data[4] = kp_tmp; + data[5] = (kd_tmp >> 4); + data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8); + data[7] = tor_tmp; + + /* 发送CAN消息 */ + BSP_CAN_StdDataFrame_t frame; + frame.id = motor->param.can_id; + frame.dlc = 8; + memcpy(frame.data, data, 8); + + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 发送位置速度模式控制命令 + * @param motor 电机实例 + * @param pos 目标位置 + * @param vel 目标速度 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) { + if (motor == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[8] = {0}; + + + /* 直接发送浮点数数据 */ + memcpy(&data[0], &pos, 4); // 位置，低位在前 + memcpy(&data[4], &vel, 4); // 速度，低位在前 + + /* 发送CAN消息，ID为原ID+0x100 */ + uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id; + BSP_CAN_StdDataFrame_t frame; + frame.id = can_id; + frame.dlc = 8; + memcpy(frame.data, data, 8); + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 发送速度模式控制命令 + * @param motor 电机实例 + * @param vel 目标速度 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) { + if (motor == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[4] = {0}; + + /* 直接发送浮点数数据 */ + memcpy(&data[0], &vel, 4); // 速度，低位在前 + + /* 发送CAN消息，ID为原ID+0x200 */ + uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id; + BSP_CAN_StdDataFrame_t frame; + frame.id = can_id; + frame.dlc = 4; + memcpy(frame.data, data, 4); + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 获取指定CAN总线的管理器 + * @param can CAN总线 + * @return CAN管理器指针 + */ +static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) { + return NULL; + } + + return can_managers[can]; +} + +/** + * @brief 创建CAN管理器 + * @param can CAN总线 + * @return 创建结果 + */ +static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 注册一个DM电机 + * @param param 电机参数 + * @return 注册结果 + */ +int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + /* 创建CAN管理器 */ + if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) { + return DEVICE_ERR; + } + + /* 获取CAN管理器 */ + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return DEVICE_ERR; + } + + /* 检查是否已注册 */ + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) { + return DEVICE_ERR_INITED; + } + } + + /* 检查是否已达到最大数量 */ + if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) { + return DEVICE_ERR; + } + + /* 分配内存 */ + MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t)); + if (motor == NULL) { + return DEVICE_ERR; + } + + /* 初始化电机 */ + memset(motor, 0, sizeof(MOTOR_DM_t)); + memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t)); + motor->motor.header.online = false; + motor->motor.reverse = param->reverse; + + /* 注册CAN接收ID - DM电机使用Master ID接收反馈 */ + uint16_t feedback_id = param->master_id; + if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) { + BSP_Free(motor); + return DEVICE_ERR; + } + + /* 添加到管理器 */ + manager->motors[manager->motor_count] = motor; + manager->motor_count++; + + return DEVICE_OK; +} + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return 更新结果 + */ +int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return DEVICE_ERR_NO_DEV; + } + + /* 查找电机 */ + MOTOR_DM_t *motor = NULL; + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) { + motor = manager->motors[i]; + break; + } + } + + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + /* 主动接收CAN消息 */ + uint16_t feedback_id = param->master_id; + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒 + motor->motor.header.online = false; + } + return DEVICE_ERR; + } + + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data); + + return DEVICE_OK; +} + +/** + * @brief 更新所有电机数据 + * @return 更新结果 + */ +int8_t MOTOR_DM_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_DM_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +/** + * @brief MIT模式控制 + * @param param 电机参数 + * @param output MIT控制参数 + * @return 控制结果 + */ +int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) { + if (param == NULL || output == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendMITCmd(motor, output); +} + +/** + * @brief 位置速度模式控制 + * @param param 电机参数 + * @param target_pos 目标位置 + * @param target_vel 目标速度 + * @return 控制结果 + */ +int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel); +} + +/** + * @brief 速度模式控制 + * @param param 电机参数 + * @param target_vel 目标速度 + * @return 控制结果 + */ +int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendVelCmd(motor, target_vel); +} + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return 电机实例指针 + */ +MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return NULL; + } + + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return NULL; + } + + /* 查找对应的电机 */ + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_DM_t *motor = manager->motors[i]; + if (motor && motor->param.can == param->can && + motor->param.master_id == param->master_id) { + return motor; + } + } + + return NULL; +} + + +int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){ + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + BSP_CAN_StdDataFrame_t frame; + frame.id = motor->param.can_id; + frame.dlc = 8; + frame.data[0] = 0XFF; + frame.data[1] = 0xFF; + frame.data[2] = 0xFF; + frame.data[3] = 0xFF; + frame.data[4] = 0xFF; + frame.data[5] = 0xFF; + frame.data[6] = 0xFF; + frame.data[7] = 0xFC; + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 使电机松弛（设置输出为0） + * @param param 电机参数 + * @return 操作结果 + */ +int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_MIT_Output_t output = {0}; + return MOTOR_DM_MITCtrl(param, &output); +} + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param 电机参数 + * @return 操作结果 + */ +int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + motor->motor.header.online = false; + return DEVICE_OK; +} diff --git a/assets/User_code/device/motor_dm.h b/assets/User_code/device/motor_dm.h new file mode 100644 index 0000000..20e91d4 --- /dev/null +++ b/assets/User_code/device/motor_dm.h @@ -0,0 +1,98 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_DM_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_DM_J4310, +} MOTOR_DM_Module_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t master_id; /* 主站ID，用于发送控制命令 */ + uint16_t can_id; /* 反馈ID，用于接收电机反馈 */ + MOTOR_DM_Module_t module; + bool reverse; +} MOTOR_DM_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_DM_Param_t param; + MOTOR_t motor; +} MOTOR_DM_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_DM_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param); + +/** + * @brief 更新所有电机数据 + * @return + */ +int8_t MOTOR_DM_UpdateAll(void); + +int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output); + +int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel); + +int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param); + +int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param); + + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/motor_lk.c b/assets/User_code/device/motor_lk.c new file mode 100644 index 0000000..c26878e --- /dev/null +++ b/assets/User_code/device/motor_lk.c @@ -0,0 +1,329 @@ +/* + LK电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lk.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define LK_CTRL_ID_BASE (0x140) +#define LK_FB_ID_BASE (0x240) + +// LK电机命令字节定义 +#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节 +#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令 +#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令 +#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令 + +// LK电机参数定义 +#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB +#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB +#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000 +#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048 +#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A +#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +// 编码器分辨率定义 +#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值 +#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值 +#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值 + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private functions -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) { + switch (module) { + case MOTOR_LK_MF9025: + case MOTOR_LK_MF9035: + return LK_CURR_LSB_MF; + default: + return LK_CURR_LSB_MG; // 默认使用MG的分辨率 + } +} + +static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) { + // 根据电机型号返回编码器最大值，这里假设都使用16位编码器 + // 实际使用时需要根据具体电机型号配置 + return LK_ENC_16BIT_MAX; +} + +static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) { + + // 检查命令字节是否为反馈命令 + if (msg->data[0] != LK_CMD_FEEDBACK) { + // 如果不是标准反馈命令，可能是其他格式的数据 + // 临时跳过命令字节检查，直接解析数据 + // return; + } + + // 解析温度 (DATA[1]) + motor->motor.feedback.temp = (int8_t)msg->data[1]; + + // 解析转矩电流值或功率值 (DATA[2], DATA[3]) + int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]); + + // 根据电机类型解析电流或功率 + switch (motor->param.module) { + case MOTOR_LK_MF9025: + case MOTOR_LK_MF9035: + motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module); + break; + default: + motor->motor.feedback.torque_current = (float)raw_current_or_power; + break; + } + + // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB + int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]); + motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed; + + // 解析编码器值 (DATA[6], DATA[7]) + uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]); + uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module); + + // 将编码器值转换为弧度 (0 ~ 2π) + float angle = (float)raw_encoder / (float)encoder_max * M_2PI; + motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + + // 检查数量 + if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + + // 对于某些LK电机，反馈数据可能通过命令ID发送 + // 根据实际测试，使用命令ID接收反馈数据 + uint16_t feedback_id = param->id; // 使用命令ID作为反馈ID + + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + // 对于某些LK电机，反馈数据通过命令ID发送 + uint16_t feedback_id = param->id; + + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + MOTOR_LK_Decode(motor, &rx_msg); + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LK_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + // 限制输出值范围 + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 根据反转参数调整输出 + float output = param->reverse ? -value : value; + + // 转矩闭环控制命令 - 将输出值转换为转矩控制值 + int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE); + + // 构建CAN帧 + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + tx_frame.data[0] = LK_CMD_TORQUE_CTRL; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = (uint8_t)(torque_control & 0xFF); + tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) { + // 对于LK电机，每次设置输出时就直接发送控制命令 + // 这个函数可以用于发送其他控制命令，如电机开启/关闭 + return DEVICE_OK; +} + +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机运行命令 + tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机关闭命令 + tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) { + return MOTOR_LK_SetOutput(param, 0.0f); +} + +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) { + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/assets/User_code/device/motor_lk.h b/assets/User_code/device/motor_lk.h new file mode 100644 index 0000000..f17dde0 --- /dev/null +++ b/assets/User_code/device/motor_lk.h @@ -0,0 +1,119 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LK_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_LK_MF9025, + MOTOR_LK_MF9035, +} MOTOR_LK_Module_t; + + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_LK_Module_t module; + bool reverse; +} MOTOR_LK_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_LK_Param_t param; + MOTOR_t motor; +} MOTOR_LK_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LK_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机开启命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机关闭命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_LK_UpdateAll(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/motor_lz.c b/assets/User_code/device/motor_lz.c new file mode 100644 index 0000000..7ed7344 --- /dev/null +++ b/assets/User_code/device/motor_lz.c @@ -0,0 +1,440 @@ +/* + 灵足电机驱动 + + 灵足电机通信协议： + - CAN 2.0通信接口，波特率1Mbps + - 采用扩展帧格式(29位ID) + - ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址) +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lz.h" +#include +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +// 灵足电机协议参数 +#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */ +#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */ +#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */ +#define LZ_KP_MAX (5000.0f) /* Kp最大值 */ +#define LZ_KD_MAX (100.0f) /* Kd最大值 */ + +#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */ +#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */ + +#define LZ_MAX_RECOVER_DIFF_RAD (0.28f) +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +/* Private macro ------------------------------------------------------------ */ + +MOTOR_LZ_MotionParam_t lz_relax_param = { + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 0.0f, + .kd = 0.0f, + .torque = 0.0f, +}; +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private function prototypes ---------------------------------------------- */ +static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can); +static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can); +static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg); +static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id); +static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value); +static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value); +static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc); +static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 获取CAN管理器 + */ +static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +/** + * @brief 创建CAN管理器 + */ +static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +/** + * @brief 构建扩展ID + */ +static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) { + uint32_t ext_id = 0; + ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型 + ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2 + ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址 + return ext_id; +} + +/** + * @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value） + */ +static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) { + // 限制范围 + if (value > max_value) value = max_value; + if (value < -max_value) value = -max_value; + + // 转换为0~65535范围，对应-max_value~max_value + return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX); +} + +/** + * @brief 浮点值转换为原始值（单向范围：0 ~ +max_value） + */ +static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) { + // 限制范围 + if (value > max_value) value = max_value; + if (value < 0.0f) value = 0.0f; + + // 转换为0~65535范围，对应0~max_value + return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX); +} + +/** + * @brief 原始值转换为浮点值 + */ +static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) { + // 将0~65535范围转换为-max_value~max_value + return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value; +} + +/** + * @brief 发送扩展帧 + */ +static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) { + BSP_CAN_ExtDataFrame_t tx_frame; + tx_frame.id = ext_id; + tx_frame.dlc = dlc; + if (data != NULL) { + memcpy(tx_frame.data, data, dlc); + } else { + memset(tx_frame.data, 0, dlc); + } + return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 灵足电机ID解析器 + * @param original_id 原始CAN ID（29位扩展帧） + * @param frame_type 帧类型 + * @return 解析后的ID（用于队列匹配） + * + * 灵足电机扩展ID格式： + * Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位) + * Bit23~8: 数据区2 (根据通信类型而定) + * Bit7~0: 目标地址 (目标电机CAN ID) + */ +static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { + // 只处理扩展数据帧 + if (frame_type != BSP_CAN_FRAME_EXT_DATA) { + return original_id; // 非扩展帧直接返回原始ID + } + + // 解析扩展ID各个字段 + uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型 + uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2 + uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID + + // 对于反馈数据帧，我们使用特殊的解析规则 + if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) { + // 反馈数据的data2字段包含： + // Bit8~15: 当前电机CAN ID + // Bit16~21: 故障信息 + // Bit22~23: 模式状态 + uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID + + // 返回格式化的ID，便于匹配 + // 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID) + return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id; + } + + // 对于其他命令类型，直接返回原始ID + return original_id; +} + +/** + * @brief 解码灵足电机反馈数据 + */ +static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) { + if (motor == NULL || msg == NULL) return; + uint8_t cmd_type = (msg->original_id >> 24) & 0x1F; + if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return; + uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF; + uint8_t motor_can_id = id_data2 & 0xFF; + uint8_t fault_info = (id_data2 >> 8) & 0x3F; + uint8_t mode_state = (id_data2 >> 14) & 0x03; + motor->lz_feedback.motor_can_id = motor_can_id; + motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0; + motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0; + motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0; + motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0; + motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0; + motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0; + motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state; + + // 反馈解码并自动反向 + uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); + float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD); + uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]); + float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S); + uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]); + float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM); + + while (angle <0){ + angle += M_2PI; + } + while (angle > M_2PI){ + angle -= M_2PI; + } + // 自动反向 + if (motor->param.reverse) { + angle = M_2PI - angle; + velocity = -velocity; + torque = -torque; + } + + motor->lz_feedback.current_angle = angle; + motor->lz_feedback.current_velocity = velocity; + motor->lz_feedback.current_torque = torque; + + uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]); + motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE; + + motor->motor.feedback.rotor_abs_angle = angle; + motor->motor.feedback.rotor_speed = velocity; + motor->motor.feedback.torque_current = torque; + motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature; + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Init(void) { + // 注册灵足电机专用的ID解析器 + return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + + +int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) { + return DEVICE_ERR; // 已注册 + } + } + + // 检查数量 + if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t)); + memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t)); + + new_motor->motor.reverse = param->reverse; + + // 注册CAN接收ID - 使用解析后的反馈数据ID + // 构建反馈数据的原始扩展ID + // 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID + uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id); + // 通过ID解析器得到解析后的ID + uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA); + + if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor && motor->param.motor_id == param->motor_id) { + // 获取反馈数据 - 使用解析后的ID + uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id); + uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA); + BSP_CAN_Message_t msg; + + while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) { + MOTOR_LZ_Decode(motor, &msg); + } + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LZ_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor) { + if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) { + if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL; + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 自动反向控制 + MOTOR_LZ_MotionParam_t send_param = *motion_param; + if (param->reverse) { + send_param.target_angle = -send_param.target_angle; + send_param.target_velocity = -send_param.target_velocity; + send_param.torque = -send_param.torque; + } + + memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t)); + + uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM); + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id); + uint8_t data[8]; + uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD); + data[0] = (raw_angle >> 8) & 0xFF; + data[1] = raw_angle & 0xFF; + uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S); + data[2] = (raw_velocity >> 8) & 0xFF; + data[3] = raw_velocity & 0xFF; + uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX); + data[4] = (raw_kp >> 8) & 0xFF; + data[5] = raw_kp & 0xFF; + uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX); + data[6] = (raw_kd >> 8) & 0xFF; + data[7] = raw_kd & 0xFF; + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + + +int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 使能命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id); + + // 数据区清零 + uint8_t data[8] = {0}; + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 停止命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id); + + // 数据区 + uint8_t data[8] = {0}; + if (clear_fault) { + data[0] = 1; // Byte[0]=1时清故障 + } + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 设置零位命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id); + + // 数据区 - Byte[0]=1 + uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0}; + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor && motor->param.motor_id == param->motor_id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) { + return MOTOR_LZ_MotionControl(param, &lz_relax_param); +} + +int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) { + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} + +static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) { + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor && motor->motor.header.online) { + return &motor->lz_feedback; + } + return NULL; +} diff --git a/assets/User_code/device/motor_lz.h b/assets/User_code/device/motor_lz.h new file mode 100644 index 0000000..d2a8002 --- /dev/null +++ b/assets/User_code/device/motor_lz.h @@ -0,0 +1,212 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LZ_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_LZ_RSO0, + MOTOR_LZ_RSO1, + MOTOR_LZ_RSO2, + MOTOR_LZ_RSO3, + MOTOR_LZ_RSO4, + MOTOR_LZ_RSO5, + MOTOR_LZ_RSO6, +} MOTOR_LZ_Module_t; + +/* 灵足电机控制模式 */ +typedef enum { + MOTOR_LZ_MODE_MOTION = 0x1, /* 运控模式 */ + MOTOR_LZ_MODE_CURRENT = 0x2, /* 电流模式 */ + MOTOR_LZ_MODE_VELOCITY = 0x3, /* 速度模式 */ + MOTOR_LZ_MODE_POSITION = 0x4, /* 位置模式 */ +} MOTOR_LZ_ControlMode_t; + +/* 灵足电机通信类型 */ +typedef enum { + MOTOR_LZ_CMD_MOTION = 0x1, /* 运控模式控制 */ + MOTOR_LZ_CMD_FEEDBACK = 0x2, /* 电机反馈数据 */ + MOTOR_LZ_CMD_ENABLE = 0x3, /* 电机使能运行 */ + MOTOR_LZ_CMD_DISABLE = 0x4, /* 电机停止运行 */ + MOTOR_LZ_CMD_SET_ZERO = 0x6, /* 设置电机机械零位 */ +} MOTOR_LZ_CmdType_t; + +/* 灵足电机运行状态 */ +typedef enum { + MOTOR_LZ_STATE_RESET = 0, /* Reset模式[复位] */ + MOTOR_LZ_STATE_CALI = 1, /* Cali模式[标定] */ + MOTOR_LZ_STATE_MOTOR = 2, /* Motor模式[运行] */ +} MOTOR_LZ_State_t; + +/* 灵足电机故障信息 */ +typedef struct { + bool uncalibrated; /* bit21: 未标定 */ + bool stall_overload; /* bit20: 堵转过载故障 */ + bool encoder_fault; /* bit19: 磁编码故障 */ + bool over_temp; /* bit18: 过温 */ + bool driver_fault; /* bit17: 驱动故障 */ + bool under_voltage; /* bit16: 欠压故障 */ +} MOTOR_LZ_Fault_t; + +/* 灵足电机运控参数 */ +typedef struct { + float target_angle; /* 目标角度 (-12.57f~12.57f rad) */ + float target_velocity; /* 目标角速度 (-20~20 rad/s) */ + float kp; /* 位置增益 (0.0~5000.0) */ + float kd; /* 微分增益 (0.0~100.0) */ + float torque; /* 力矩 (-60~60 Nm) */ +} MOTOR_LZ_MotionParam_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; /* CAN总线 */ + uint8_t motor_id; /* 电机CAN ID */ + uint8_t host_id; /* 主机CAN ID */ + MOTOR_LZ_Module_t module; /* 电机型号 */ + bool reverse; /* 是否反向 */ + MOTOR_LZ_ControlMode_t mode; /* 控制模式 */ +} MOTOR_LZ_Param_t; + +/*电机反馈信息扩展*/ +typedef struct { + float current_angle; /* 当前角度 (-12.57f~12.57f rad) */ + float current_velocity; /* 当前角速度 (-20~20 rad/s) */ + float current_torque; /* 当前力矩 (-60~60 Nm) */ + float temperature; /* 当前温度 (摄氏度) */ + MOTOR_LZ_State_t state; /* 运行状态 */ + MOTOR_LZ_Fault_t fault; /* 故障信息 */ + uint8_t motor_can_id; /* 当前电机CAN ID */ +} MOTOR_LZ_Feedback_t; + +/*电机实例*/ +typedef struct { + MOTOR_LZ_Param_t param; + MOTOR_t motor; + MOTOR_LZ_Feedback_t lz_feedback; /* 灵足电机特有反馈信息 */ + MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */ +} MOTOR_LZ_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LZ_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Init(void); + +/** + * @brief 注册一个灵足电机 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param); + +/** + * @brief 更新所有电机数据 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_UpdateAll(void); + +/** + * @brief 运控模式控制电机 + * @param param 电机参数 + * @param motion_param 运控参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param); + +/** + * @brief 电流(力矩)模式控制电机 + * @param param 电机参数 + * @param torque 目标力矩 (-60~60 Nm) + * @return 设备状态码 + */ +int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque); + +/** + * @brief 位置模式控制电机 + * @param param 电机参数 + * @param target_angle 目标角度 (-12.57~12.57 rad) + * @param max_velocity 最大速度 (0~20 rad/s) + * @return 设备状态码 + */ +int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity); + +/** + * @brief 速度模式控制电机 + * @param param 电机参数 + * @param target_velocity 目标速度 (-20~20 rad/s) + * @return 设备状态码 + */ +int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity); + +/** + * @brief 电机使能运行 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param); + +/** + * @brief 电机停止运行 + * @param param 电机参数 + * @param clear_fault 是否清除故障 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault); + +/** + * @brief 设置电机机械零位 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return 电机实例指针，失败返回NULL + */ +MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机松弛（发送停止命令） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/motor_odrive.c b/assets/User_code/device/motor_odrive.c new file mode 100644 index 0000000..74b9319 --- /dev/null +++ b/assets/User_code/device/motor_odrive.c @@ -0,0 +1,327 @@ +/* + Odrive电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_odrive.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ + + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static ODrive_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + + +// 获取指定CAN总线的电机管理器指针 +static ODrive_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +// 为指定CAN总线创建电机管理器 +static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + can_managers[can] = (ODrive_CANManager_t*)BSP_Malloc(sizeof(ODrive_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + memset(can_managers[can], 0, sizeof(ODrive_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +// 解析CAN报文，更新电机反馈信息 +static void Motor_Decode(ODrive_t *motor, BSP_CAN_Message_t *msg) +{ + uint8_t axis_id = (msg->original_id >> 5) & 0x3F; // 提取电机号（0~63） + uint8_t cmd_id = msg->original_id & 0x1F; // 提取命令 ID（低 5 位） + + motor->param.id = axis_id; // 保存电机 ID + + // 解析帧类型（数据帧或远程帧） + if (msg->frame_type == BSP_CAN_FRAME_STD_DATA) { + // 数据帧处理 + switch (cmd_id) + { + case ODRIVE_HEARTBEAT_MESSAGE: // 0x001 ODrive心跳消息 + // motor->motor.feedback.axis_error = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24); + // motor->motor.feedback.axis_state = msg->data[4]; + // motor->motor.feedback.controller_status = msg->data[5]; + break; + + case ENCODER_ESTIMATES: // 0x009 + { + uint32_t raw_pos = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24); + uint32_t raw_vel = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24); + memcpy(&motor->motor.feedback.rotor_abs_angle, &raw_pos, sizeof(float)); + memcpy(&motor->motor.feedback.rotor_speed, &raw_vel, sizeof(float)); + } + break; + + case GET_ENCODER_COUNT: // 0x014 + // motor->motor.feedback.encoder_shadow = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24); + // motor->motor.feedback.encoder_cpr = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24); + break; + + case GET_BUS_VOLTAGE_CURRENT: // 0x017 + { + uint32_t raw_vbus, raw_ibus; + raw_vbus = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24); + raw_ibus = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24); + // memcpy(&motor->motor.feedback.bus_voltage, &raw_vbus, sizeof(float)); + memcpy(&motor->motor.feedback.torque_current, &raw_ibus, sizeof(float)); + } + break; + + case GET_IQ: // 0x018 + { + uint32_t raw_iq_set, raw_iq_meas; + raw_iq_set = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24); + raw_iq_meas = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24); + // memcpy(&motor->motor.feedback.iq_setpoint, &raw_iq_set, sizeof(float)); + // memcpy(&motor->motor.feedback.iq_measured, &raw_iq_meas, sizeof(float)); + } + break; + + default: + + break; + } + } +} + + +/* Exported functions ------------------------------------------------------- */ + +// 注册一个新的电机实例到管理器 +int8_t ODrive_Register(ODrive_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + // 检查数量 + if (manager->motor_count >= ODRIVE_MAX_MOTORS) return DEVICE_ERR; + // 创建新电机实例 + ODrive_t *new_motor = (ODrive_t*)BSP_Malloc(sizeof(ODrive_t)); + if (new_motor == NULL) return DEVICE_ERR; + memcpy(&new_motor->param, param, sizeof(ODrive_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +// 更新指定电机的反馈数据 +int8_t ODrive_Update(ODrive_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + for (int i = 0; i < manager->motor_count; i++) { + ODrive_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + Motor_Decode(motor, &rx_msg); + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +// 更新所有CAN总线下所有电机的反馈数据 +int8_t ODrive_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + ODrive_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + for (int i = 0; i < manager->motor_count; i++) { + ODrive_t *motor = manager->motors[i]; + if (motor != NULL) { + if (ODrive_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +// 获取指定参数对应的电机实例指针 +ODrive_t* ODrive_GetMotor(ODrive_Param_t *param) { + if (param == NULL) return NULL; + ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can); + if (manager == NULL) return NULL; + for (int i = 0; i < manager->motor_count; i++) { + ODrive_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +// 设置指定电机的输出值 +int8_t ODrive_SetOutput(ODrive_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 如果电机反转标志为 true，则反向值 + if (param->reverse) { + value = -value; + } + + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t command_id; + uint8_t *pVal = (uint8_t *)&value; + + // 选择命令 ID 和数据打包方式 + switch (param->mode) { + case POSITION_CONTROL: { + command_id = SET_INPUT_POS; + float pos = value; + int16_t vel_ff = 0; // 可扩展为参数传入 0就行 + int16_t torque_ff = 0; // 可扩展为参数传入 0就行 + uint8_t *pPos = (uint8_t *)&pos; + uint8_t *pVel = (uint8_t *)&vel_ff; + uint8_t *pTor = (uint8_t *)&torque_ff; + memcpy(&tx_frame.data[0], pPos, 4); + memcpy(&tx_frame.data[4], pVel, 2); + memcpy(&tx_frame.data[6], pTor, 2); + tx_frame.dlc = 8; + break; + } + case VELOCITY_CONTROL: { + command_id = SET_INPUT_VEL; + float vel = value; + float torque_ff = 0.0f; // 可扩展为参数传入 + uint8_t *pVel = (uint8_t *)&vel; + uint8_t *pTor = (uint8_t *)&torque_ff; + memcpy(&tx_frame.data[0], pVel, 4); + memcpy(&tx_frame.data[4], pTor, 4); + tx_frame.dlc = 8; + break; + } + case TORQUE_CONTROL: { + command_id = SET_INPUT_TORQUE; + memcpy(&tx_frame.data[0], pVal, 4); + tx_frame.dlc = 4; + break; + } + case VOLTAGE_CONTROL: + default: + return DEVICE_ERR; // 暂不支持电压模式 + } + + // 组装 CAN ID（标准帧） + tx_frame.id = (param->id << 5) | command_id; + + // 标准数据帧 + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +// 设置加速度和减速度 +int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t command_id = SET_TRAJ_ACCEL_LIMITS; + + uint8_t *pAccel = (uint8_t *)&accel; + uint8_t *pDecel = (uint8_t *)&decel; + memcpy(&tx_frame.data[0], pAccel, 4); + memcpy(&tx_frame.data[4], pDecel, 4); + tx_frame.dlc = 8; + + tx_frame.id = (param->id << 5) | command_id; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +// 获取位置和速度反馈 +int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t command_id = ENCODER_ESTIMATES; // 请求编码器估计值命令 + uint8_t zero_data[8] = {0}; // 发送全 0 数据（ODrive 协议要求） + + memcpy(tx_frame.data, zero_data, 8); + tx_frame.dlc = 8; + tx_frame.id = (param->id << 5) | command_id; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +// 设置轴请求状态（一般用来重启 ODrive 的某个轴） +// ODrive_SetAxisRequestedState(odrive_axis[0], CLOSED_LOOP_CONTROL); +int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t command_id = SET_AXIS_REQUESTED_STATE; + + // 将 state 转为 4 字节 + memcpy(tx_frame.data, &state, 4); + memset(&tx_frame.data[4], 0, 4); + tx_frame.dlc = 4; + + // 组装 CAN ID + tx_frame.id = (param->id << 5) | command_id; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +// 清除错误 +int8_t ODrive_ClearErrors(ODrive_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t command_id = CLEAR_ERRORS; + + memset(tx_frame.data, 0, 8); + tx_frame.dlc = 0; + + tx_frame.id = (param->id << 5) | command_id; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +// 重启 ODrive +int8_t ODrive_Reboot(ODrive_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t command_id = REBOOT_ODRIVE; + + memset(tx_frame.data, 0, 8); + tx_frame.dlc = 0; + + tx_frame.id = (param->id << 5) | command_id; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} diff --git a/assets/User_code/device/motor_odrive.h b/assets/User_code/device/motor_odrive.h new file mode 100644 index 0000000..ffc5282 --- /dev/null +++ b/assets/User_code/device/motor_odrive.h @@ -0,0 +1,162 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Private define ----------------------------------------------------------- */ + +//ODrive型号根据实际情况调整 +#define ODRIVE_MAX_MOTORS 2 + +//COMMAND ID +#define ODRIVE_HEARTBEAT_MESSAGE 0x001 // ODrive心跳消息 +#define SET_AXIS_NODE_ID 0x006 // 设置电机节点ID +#define GET_ENCODER_ESTIMATES 0x008 // 获取编码器估计值 +#define GET_ENCODER_COUNT 0x00A // 获取编码器计数 +#define SET_AXIS_REQUESTED_STATE 0x007 // 设置电机请求状态 +#define ENCODER_ESTIMATES 0x009 // 编码器估计值 +#define GET_ENCODER_COUNT 0x00A // 获取编码器计数 +#define SET_CONTROLLER_MODES 0x00B // 设置控制器模式 +#define SET_INPUT_POS 0x00C // 设置输入位置 +#define SET_INPUT_VEL 0x00D // 设置输入速度 +#define SET_INPUT_TORQUE 0x00E // 设置输入转矩 +#define SET_LIMITS 0x00F // 设置限制 +#define GET_IQ 0x014 // 获取电流 +#define REBOOT_ODRIVE 0x016 // 重启ODrive +#define GET_BUS_VOLTAGE_CURRENT 0x017 // 获取总线电压和电流 +#define CLEAR_ERRORS 0x018 // 清除错误 +#define SET_POSITION_GAIN 0x01A // 设置位置增益 +#define SET_VEL_GAINS 0x01B // 设置速度增益 +#define SET_TRAJ_ACCEL_LIMITS 0x012 // 设置轨迹加速度限制 +/* Exported constants ------------------------------------------------------- */ + + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +//Axis States +typedef enum { + UNDEFINED = 0x0, + IDLE = 0x1, + STARTUP_SEQUENCE = 0x2, + FULL_CALIBRATION_SEQUENCE = 0x3, + MOTOR_CALIBRATION = 0x4, + ENCODER_INDEX_SEARCH = 0x6, + ENCODER_OFFSET_CALIBRATION = 0x7, + CLOSED_LOOP_CONTROL = 0x8, + LOCKIN_SPIN = 0x9, + ENCODER_DIR_FIND = 0xA, + HOMING = 0xB, + ENCODER_HALL_POLARITY_CALIBRATION = 0xC, + ENCODER_HALL_PHASE_CALIBRATION = 0xD +} Axis_State; + +//Control Modes +typedef enum{ + VOLTAGE_CONTROL = 0x0, + TORQUE_CONTROL = 0x1, + VELOCITY_CONTROL = 0x2, + POSITION_CONTROL = 0x3 +} Control_Mode; + + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + uint16_t mode; + bool reverse; +} ODrive_Param_t; + +/*电机实例*/ +typedef struct ODrive_t { + ODrive_Param_t param; + MOTOR_t motor; +} ODrive_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + ODrive_t *motors[ODRIVE_MAX_MOTORS]; + uint8_t motor_count; +} ODrive_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个odrive电机 + * @param param 电机参数 + * @return + */ +int8_t ODrive_Register(ODrive_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ + +int8_t ODrive_Update(ODrive_Param_t *param); + +/** * @brief 更新所有ODrive电机状态 + * @return + */ +int8_t ODrive_UpdateAll(void); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值 + * @return + */ +int8_t ODrive_SetOutput(ODrive_Param_t *param, float value); + +/** * @brief 设置电机加速度和减速度限制 + * @param param 电机参数 + * @param accel 加速度 + * @param decel 减速度 + * @return + */ +int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +ODrive_t* ODrive_GetMotor(ODrive_Param_t *param); + +/** * @brief 获取指定电机的编码器估计值 + * @param param 电机参数 + * @return + */ +int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param); + + +/** * @brief 设置轴请求状态（一般用来重启 ODrive 的某个轴） + * @param param 电机参数 + * @return + */ +int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state); + +/** * @brief 清除错误 + * @param param 电机参数 + * @return + */ +int8_t ODrive_ClearErrors(ODrive_Param_t *param); + +/** * @brief 重启 ODrive + * @param param 电机参数 + * @return + */ +int8_t ODrive_Reboot(ODrive_Param_t *param); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/motor_rm.c b/assets/User_code/device/motor_rm.c new file mode 100644 index 0000000..1e78a47 --- /dev/null +++ b/assets/User_code/device/motor_rm.c @@ -0,0 +1,321 @@ +/* + RM电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_rm.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define GM6020_FB_ID_BASE (0x205) +#define GM6020_CTRL_ID_BASE (0x1ff) +#define GM6020_CTRL_ID_EXTAND (0x2ff) + +#define M3508_M2006_FB_ID_BASE (0x201) +#define M3508_M2006_CTRL_ID_BASE (0x200) +#define M3508_M2006_CTRL_ID_EXTAND (0x1ff) +#define M3508_M2006_ID_SETTING_ID (0x700) + +#define GM6020_MAX_ABS_LSB (30000) +#define M3508_MAX_ABS_LSB (16384) +#define M2006_MAX_ABS_LSB (10000) + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */ +#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: + case MOTOR_M3508: + if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) { + return can_id - M3508_M2006_FB_ID_BASE; + } + break; + case MOTOR_GM6020: + if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) { + return can_id - GM6020_FB_ID_BASE + 4; + } + break; + default: + break; + } + return DEVICE_ERR; +} + +static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: return 36.0f; + case MOTOR_M3508: return 3591.0f / 187.0f; + case MOTOR_GM6020: return 1.0f; + default: return 1.0f; + } +} + +static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: return M2006_MAX_ABS_LSB; + case MOTOR_M3508: return M3508_MAX_ABS_LSB; + case MOTOR_GM6020: return GM6020_MAX_ABS_LSB; + default: return DEVICE_ERR; + } +} + +static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { + uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); + int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]); + int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]); + int16_t lsb = MOTOR_RM_GetLSB(motor->param.module); + float ratio = MOTOR_RM_GetRatio(motor->param.module); + + float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI; + float rotor_speed = raw_speed; + float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES; + + if (motor->param.gear) { + // 多圈累加 + int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle; + if (delta > (MOTOR_ENC_RES / 2)) { + motor->gearbox_round_count--; + } else if (delta < -(MOTOR_ENC_RES / 2)) { + motor->gearbox_round_count++; + } + motor->last_raw_angle = raw_angle; + float single_turn = rotor_angle; + motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio; + // 输出轴多圈绝对值 + motor->feedback.rotor_abs_angle = motor->gearbox_total_angle; + motor->feedback.rotor_speed = rotor_speed / ratio; + motor->feedback.torque_current = torque_current * ratio; + } else { + // 非gear模式，直接用转子单圈 + motor->gearbox_round_count = 0; + motor->last_raw_angle = raw_angle; + motor->gearbox_total_angle = 0.0f; + motor->feedback.rotor_abs_angle = rotor_angle; + motor->feedback.rotor_speed = rotor_speed; + motor->feedback.torque_current = torque_current; + } + while (motor->feedback.rotor_abs_angle < 0) { + motor->feedback.rotor_abs_angle += M_2PI; + } + while (motor->feedback.rotor_abs_angle >= M_2PI) { + motor->feedback.rotor_abs_angle -= M_2PI; + } + if (motor->motor.reverse) { + motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; + motor->feedback.rotor_speed = -motor->feedback.rotor_speed; + motor->feedback.torque_current = -motor->feedback.torque_current; + } + motor->feedback.temp = msg->data[6]; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + // 检查数量 + if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR; + // 创建新电机实例 + MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t)); + if (new_motor == NULL) return DEVICE_ERR; + memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + Motor_RM_Decode(motor, &rx_msg); + motor->motor.feedback = motor->feedback; + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_RM_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + if (param->reverse){ + value = -value; + } + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module); + if (logical_index < 0) return DEVICE_ERR; + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module)); + output_msg->output[logical_index] = output_value; + return DEVICE_OK; +} + +int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t id = param->id; + switch (id) { + case M3508_M2006_FB_ID_BASE: + case M3508_M2006_FB_ID_BASE+1: + case M3508_M2006_FB_ID_BASE+2: + case M3508_M2006_FB_ID_BASE+3: + tx_frame.id = M3508_M2006_CTRL_ID_BASE; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 0; i < 4; i++) { + tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + break; + case M3508_M2006_FB_ID_BASE+4: + case M3508_M2006_FB_ID_BASE+5: + case M3508_M2006_FB_ID_BASE+6: + case M3508_M2006_FB_ID_BASE+7: + tx_frame.id = M3508_M2006_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 4; i < 8; i++) { + tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + break; + case GM6020_FB_ID_BASE+4: + case GM6020_FB_ID_BASE+5: + case GM6020_FB_ID_BASE+6: + tx_frame.id = GM6020_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 8; i < 11; i++) { + tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + tx_frame.data[6] = 0; + tx_frame.data[7] = 0; + break; + default: + return DEVICE_ERR; + } + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) { + if (param == NULL) return NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return NULL; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) { + return MOTOR_RM_SetOutput(param, 0.0f); +} + +int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) { + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/assets/User_code/device/motor_rm.h b/assets/User_code/device/motor_rm.h new file mode 100644 index 0000000..670b427 --- /dev/null +++ b/assets/User_code/device/motor_rm.h @@ -0,0 +1,132 @@ +#pragma once + +#include "motor.h" +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_RM_MAX_MOTORS 11 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_M2006, + MOTOR_M3508, + MOTOR_GM6020, +} MOTOR_RM_Module_t; + +/*一个can最多控制11个电机*/ +typedef union { + int16_t output[MOTOR_RM_MAX_MOTORS]; + struct { + int16_t m3508_m2006_id201; + int16_t m3508_m2006_id202; + int16_t m3508_m2006_id203; + int16_t m3508_m2006_id204; + int16_t m3508_m2006_gm6020_id205; + int16_t m3508_m2006_gm6020_id206; + int16_t m3508_m2006_gm6020_id207; + int16_t m3508_m2006_gm6020_id208; + int16_t gm6020_id209; + int16_t gm6020_id20A; + int16_t gm6020_id20B; + } named; +} MOTOR_RM_MsgOutput_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_RM_Module_t module; + bool reverse; + bool gear; +} MOTOR_RM_Param_t; + +typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t; + +typedef struct { + MOTOR_RM_Param_t param; + MOTOR_RM_Feedback_t feedback; + MOTOR_t motor; + // 多圈相关变量，仅gear模式下有效 + uint16_t last_raw_angle; + int32_t gearbox_round_count; + float gearbox_total_angle; +} MOTOR_RM_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_RM_MsgOutput_t output_msg; + MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_RM_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个RM电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_RM_UpdateAll(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/motor_vesc.c b/assets/User_code/device/motor_vesc.c new file mode 100644 index 0000000..68333f7 --- /dev/null +++ b/assets/User_code/device/motor_vesc.c @@ -0,0 +1,249 @@ +/* + VESC电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_vesc.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ + + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +/************************************** + * 限幅函数 + **************************************/ +void assert_param_duty(float *duty){ + // 如果 duty 是 -1.0 ~ 1.0，则最大值用 wtrcfg_VESC_COMMAND_DUTY_MAX / 100 + float max_duty = wtrcfg_VESC_COMMAND_DUTY_MAX / 100.0f; + if (fabsf(*duty) > max_duty) { + *duty = (*duty > 0) ? max_duty : -max_duty; + } +} + +void assert_param_current(float *current){ + if( fabsf(*current) > wtrcfg_VESC_COMMAND_CURRENT_MAX ) + *current = *current > 0 ? wtrcfg_VESC_COMMAND_CURRENT_MAX : - wtrcfg_VESC_COMMAND_CURRENT_MAX ; +} +void assert_param_rpm(float *rpm){ + if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX ) + *rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ; +} +void assert_param_pos(float *pos){ + if( fabsf(*pos) > wtrcfg_VESC_COMMAND_POS_MAX ) + *pos = *pos > 0 ? wtrcfg_VESC_COMMAND_POS_MAX : - wtrcfg_VESC_COMMAND_POS_MAX ; +} + +static VESC_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + + +// 获取指定CAN总线的电机管理器指针 +static VESC_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +// 为指定CAN总线创建电机管理器 +static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + can_managers[can] = (VESC_CANManager_t*)BSP_Malloc(sizeof(VESC_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + memset(can_managers[can], 0, sizeof(VESC_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +// 解析CAN报文，更新电机反馈信息 +static void Motor_VESC_Decode(VESC_t *motor, BSP_CAN_Message_t *msg) +{ + if (motor == NULL || msg == NULL) return; + motor->motor.feedback.rotor_speed = + ((int32_t)msg->data[0] << 24) | + ((int32_t)msg->data[1] << 16) | + ((int32_t)msg->data[2] << 8) | + ((int32_t)msg->data[3]); + + // torque_current: 低 2 字节 (data[4], data[5]) + int16_t raw_current = (int16_t)((msg->data[5] << 8) | msg->data[4]); + motor->motor.feedback.torque_current = raw_current / 1000.0f; // 从 0.1A -> A + + // duty_cycle: 低 2 字节 (data[6], data[7]) + int16_t raw_duty = (int16_t)((msg->data[7] << 8) | msg->data[6]); + //motor->motor.feedback.duty_cycle = raw_duty / 1000.0f; // 从千分之一 -> (-1.0 ~ 1.0) + +} + + +/* Exported functions ------------------------------------------------------- */ + +// 注册一个新的电机实例到管理器 +int8_t VESC_Register(VESC_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + // 检查数量 + if (manager->motor_count >= VESC_MAX_MOTORS) return DEVICE_ERR; + // 创建新电机实例 + VESC_t *new_motor = (VESC_t*)BSP_Malloc(sizeof(VESC_t)); + if (new_motor == NULL) return DEVICE_ERR; + memcpy(&new_motor->param, param, sizeof(VESC_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +// 更新指定电机的反馈数据（扩展帧方式） +int8_t VESC_Update(VESC_Param_t *param) +{ + if (param == NULL) return DEVICE_ERR_NULL; + VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + VESC_t *motor = NULL; + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + motor = manager->motors[i]; + break; + } + } + if (motor == NULL) return DEVICE_ERR_NO_DEV; + // 根据电机 ID 获取对应扩展帧 ID + uint32_t ext_id = 0; + switch (param->id) { + case VESC_1: ext_id = CAN_VESC5065_M1_MSG1; break; + case VESC_2: ext_id = CAN_VESC5065_M2_MSG1; break; + case VESC_4: ext_id = CAN_VESC5065_M3_MSG1; break; + default: return DEVICE_ERR_NO_DEV; + } + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, ext_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + Motor_VESC_Decode(motor, &rx_msg); + return DEVICE_OK; +} + +// 更新所有CAN总线下所有电机的反馈数据 +int8_t VESC_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + VESC_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + for (int i = 0; i < manager->motor_count; i++) { + VESC_t *motor = manager->motors[i]; + if (motor != NULL) { + if (VESC_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +// 获取指定参数对应的电机实例指针 +VESC_t* VESC_GetMotor(VESC_Param_t *param) { + if (param == NULL) return NULL; + VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can); + if (manager == NULL) return NULL; + for (int i = 0; i < manager->motor_count; i++) { + VESC_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +// 设置指定电机的输出值 +int8_t VESC_SetOutput(VESC_Param_t *param, float value) +{ + if (param == NULL) return DEVICE_ERR_NULL; + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t command_id; + + if (param->reverse) { + value = -value; + } + + switch (param->mode) + { + case DUTY_CONTROL: { + assert_param_duty(&value); // 调用你现有的限幅函数 + command_id = CAN_PACKET_SET_DUTY; + int32_t duty_val = (int32_t)(value * 1e5f); // duty 放大 1e5 + memcpy(&tx_frame.data[0], &duty_val, 4); + tx_frame.dlc = 4; + break; + } + case RPM_CONTROL: { + assert_param_rpm(&value); + command_id = CAN_PACKET_SET_RPM; + int32_t rpm_val = (int32_t)value; + memcpy(&tx_frame.data[0], &rpm_val, 4); + tx_frame.dlc = 4; + break; + } + case CURRENT_CONTROL: { + assert_param_current(&value); + command_id = CAN_PACKET_SET_CURRENT; + int32_t cur_val = (int32_t)(value * 1e3f); // A -> mA （0-50A） + memcpy(&tx_frame.data[0], &cur_val, 4); + tx_frame.dlc = 4; + break; + } + case POSITION_CONTROL: { + assert_param_pos(&value); + command_id = CAN_PACKET_SET_POS; + memcpy(&tx_frame.data[0], &value, 4); + tx_frame.dlc = 4; + break; + } + default: + return DEVICE_ERR; + } + tx_frame.id = (param->id << 5) | command_id; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; + } + +int8_t VESC_Relax(VESC_Param_t *param) { + return VESC_SetOutput(param, 0.0f); +} + + +int8_t VESC_Offine(VESC_Param_t *param) { + VESC_t *motor = VESC_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/assets/User_code/device/motor_vesc.h b/assets/User_code/device/motor_vesc.h new file mode 100644 index 0000000..7209218 --- /dev/null +++ b/assets/User_code/device/motor_vesc.h @@ -0,0 +1,146 @@ +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Private define ----------------------------------------------------------- */ +#define wtrcfg_VESC_COMMAND_DUTY_MAX 100 +#define wtrcfg_VESC_COMMAND_CURRENT_MAX 10 +#define wtrcfg_VESC_COMMAND_POS_MAX 360 +#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000 +#define wtrcfg_VESC_UART_TIMEOUT 0xff + +// VESC数量根据实际情况调整 +#define VESC_MAX_MOTORS 4 + +/* Exported constants ------------------------------------------------------- */ + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +typedef enum +{ + VESC_1 = 1, + VESC_2 = 2, + VESC_3 = 3, + VESC_4 = 4, + CAN_VESC5065_M1_MSG1 = 0x901, // vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型 + CAN_VESC5065_M2_MSG1 = 0x902, + CAN_VESC5065_M3_MSG1 = 0x903, + CAN_VESC5065_M4_MSG1 = 0x904, +}VESC_ID; + +typedef enum +{ + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT = 1, + CAN_PACKET_SET_CURRENT_BRAKE = 2, + CAN_PACKET_SET_RPM = 3, + CAN_PACKET_SET_POS = 4, + CAN_PACKET_FILL_RX_BUFFER = 5, + CAN_PACKET_FILL_RX_BUFFER_LONG = 6, + CAN_PACKET_PROCESS_RX_BUFFER = 7, + CAN_PACKET_PROCESS_SHORT_BUFFER = 8, + CAN_PACKET_STATUS = 9, + CAN_PACKET_SET_CURRENT_REL = 10, + CAN_PACKET_SET_CURRENT_BRAKE_REL = 11, + CAN_PACKET_SET_CURRENT_HANDBRAKE = 12, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13 +} CAN_PACKET_ID; + +// Control Modes +typedef enum +{ + DUTY_CONTROL = 0x0, + RPM_CONTROL = 0x1, + CURRENT_CONTROL = 0x2, + POSITION_CONTROL = 0x3 +} Control_Mode; + +/*每个电机需要的参数*/ +typedef struct +{ + BSP_CAN_t can; + uint16_t id; + uint16_t mode; + bool reverse; +} VESC_Param_t; + +/*电机实例*/ +typedef struct ODrive_t +{ + VESC_Param_t param; + MOTOR_t motor; +} VESC_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct +{ + BSP_CAN_t can; + VESC_t *motors[VESC_MAX_MOTORS]; + uint8_t motor_count; +} VESC_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个vesc电机 + * @param param 电机参数 + * @return + */ +int8_t VESC_Register(VESC_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t VESC_Update(VESC_Param_t *param); + +/** + * @brief 更新所有电机数据 + * @return + */ +int8_t VESC_UpdateAll(void); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值 + * @return + */ + +int8_t VESC_SetOutput(VESC_Param_t *param, float value); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ + +VESC_t* VESC_GetMotor(VESC_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t VESC_Relax(VESC_Param_t *param); +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t VESC_Offine(VESC_Param_t *param); + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/ops9.c b/assets/User_code/device/ops9.c new file mode 100644 index 0000000..70cc0aa --- /dev/null +++ b/assets/User_code/device/ops9.c @@ -0,0 +1,58 @@ +/* + ACTION全场定位码盘ops9 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "device/ops9.h" + +#include + +#include "bsp/uart.h" +#include "bsp/time.h" +/* Private define ----------------------------------------------------------- */ + + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +static void OPS9_RxCpltCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_OPS9_RAW_REDY); + +} + +/* Exported functions ------------------------------------------------------- */ +int8_t OPS9_init(OPS9_t *ops9) { + if (ops9 == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + BSP_UART_RegisterCallback(BSP_UART_OPS9, BSP_UART_RX_CPLT_CB, + OPS9_RxCpltCallback); + + inited = true; + return DEVICE_OK; +} + +int8_t OPS9_Restart(void) { + __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_OPS9)); + __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_OPS9)); + return DEVICE_OK; +} + +int8_t OPS9_StartDmaRecv(OPS9_t *ops9) { + if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_OPS9), + (uint8_t *)&(ops9->data), + sizeof(ops9->data)) == HAL_OK) + return DEVICE_OK; + return DEVICE_ERR; +} + +bool OPS9_WaitDmaCplt(uint32_t timeout) { + return (osThreadFlagsWait(SIGNAL_OPS9_RAW_REDY, osFlagsWaitAll, timeout) == + SIGNAL_OPS9_RAW_REDY); +} + diff --git a/assets/User_code/device/ops9.h b/assets/User_code/device/ops9.h new file mode 100644 index 0000000..683142f --- /dev/null +++ b/assets/User_code/device/ops9.h @@ -0,0 +1,49 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "component/user_math.h" +#include "device/device.h" +/* Exported constants ------------------------------------------------------- */ + +#define OPS9_HEADER 0x0D0A +#define OPS9_TAIL 0x0A0D + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +// 数据包结构体 +typedef struct __packed { + uint16_t header; // 2字节 + float yaw; // 4字节 + float pitch; // 4字节 + float roll; // 4字节 + float x; // 4字节 + float y; // 4字节 + float angular_velocity; // 4字节 + uint16_t tail; // 2字节 +} OPS9_Data_t; // 共28字节 + +typedef struct { + DEVICE_Header_t header; // 设备头 + OPS9_Data_t data; // 存储接收到的数据 +} OPS9_t; + +/* Exported functions prototypes -------------------------------------------- */ + +int8_t OPS9_init(OPS9_t *ops9); +int8_t OPS9_Restart(void); +int8_t OPS9_StartDmaRecv(OPS9_t *ops9); +bool OPS9_WaitDmaCplt(uint32_t timeout); + + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/rc_can.c b/assets/User_code/device/rc_can.c new file mode 100644 index 0000000..77d9733 --- /dev/null +++ b/assets/User_code/device/rc_can.c @@ -0,0 +1,328 @@ +/* Includes ----------------------------------------------------------------- */ +#include "device/rc_can.h" +#include "bsp/time.h" +#include "device/device.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private constants -------------------------------------------------------- */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private types ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ + +/* USER VARIABLE BEGIN */ + +/* USER VARIABLE END */ + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/* Private function prototypes ---------------------------------------------- */ +static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param); +static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can); + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化RC CAN发送模块 + * @param rc_can RC_CAN结构体指针 + * @param param 初始化参数 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param) { + if (rc_can == NULL || param == NULL) { + return DEVICE_ERR_NULL; + } + + // 参数验证 + if (RC_CAN_ValidateParams(param) != DEVICE_OK) { + return DEVICE_ERR; + } + + rc_can->param = *param; + + // 初始化header + rc_can->header.online = false; + rc_can->header.last_online_time = 0; + + // 手动初始化数据结构 + rc_can->data.joy.ch_l_x = 0.0f; + rc_can->data.joy.ch_l_y = 0.0f; + rc_can->data.joy.ch_r_x = 0.0f; + rc_can->data.joy.ch_r_y = 0.0f; + rc_can->data.sw.sw_l = RC_CAN_SW_ERR; + rc_can->data.sw.sw_r = RC_CAN_SW_ERR; + rc_can->data.sw.ch_res = 0.0f; + rc_can->data.mouse.x = 0.0f; + rc_can->data.mouse.y = 0.0f; + rc_can->data.mouse.z = 0.0f; + rc_can->data.mouse.mouse_l = false; + rc_can->data.mouse.mouse_r = false; + rc_can->data.keyboard.key_value = 0; + for (int i = 0; i < 6; i++) { + rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE; + } + + // 注册CAN ID队列（从机模式需要接收数据） + if (rc_can->param.mode == RC_CAN_MODE_SLAVE) { + return RC_CAN_RegisterIds(rc_can); + } + + return DEVICE_OK; +} + +/** + * @brief 更新并发送数据到CAN总线 + * @param rc_can RC_CAN结构体指针 + * @param data_type 数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) { + if (rc_can == NULL) { + return DEVICE_ERR_NULL; + } + if (rc_can->param.mode != RC_CAN_MODE_MASTER) { + return DEVICE_ERR; + } + BSP_CAN_StdDataFrame_t frame; + frame.dlc = 8; + // 边界裁剪宏 + #define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x))) + switch (data_type) { + case RC_CAN_DATA_JOYSTICK: { + frame.id = rc_can->param.joy_id; + float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f); + float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f); + float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f); + float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f); + int16_t l_x_i = (int16_t)(l_x * 32768.0f); + int16_t l_y_i = (int16_t)(l_y * 32768.0f); + int16_t r_x_i = (int16_t)(r_x * 32768.0f); + int16_t r_y_i = (int16_t)(r_y * 32768.0f); + frame.data[0] = (uint8_t)(l_x_i & 0xFF); + frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF); + frame.data[2] = (uint8_t)(l_y_i & 0xFF); + frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF); + frame.data[4] = (uint8_t)(r_x_i & 0xFF); + frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF); + frame.data[6] = (uint8_t)(r_y_i & 0xFF); + frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF); + break; + } + case RC_CAN_DATA_SWITCH: { + frame.id = rc_can->param.sw_id; + frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l); + frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r); + float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f); + int16_t ch_res_i = (int16_t)(ch_res * 32768.0f); + frame.data[2] = (uint8_t)(ch_res_i & 0xFF); + frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF); + frame.data[4] = 0; // 保留字节 + frame.data[5] = 0; // 保留字节 + frame.data[6] = 0; // 保留字节 + frame.data[7] = 0; // 保留字节 + break; + } + case RC_CAN_DATA_MOUSE: { + frame.id = rc_can->param.mouse_id; + // 鼠标x/y/z一般为增量，若有极限也可加clamp + int16_t x = (int16_t)(rc_can->data.mouse.x); + int16_t y = (int16_t)(rc_can->data.mouse.y); + int16_t z = (int16_t)(rc_can->data.mouse.z); + frame.data[0] = (uint8_t)(x & 0xFF); + frame.data[1] = (uint8_t)((x >> 8) & 0xFF); + frame.data[2] = (uint8_t)(y & 0xFF); + frame.data[3] = (uint8_t)((y >> 8) & 0xFF); + frame.data[4] = (uint8_t)(z & 0xFF); + frame.data[5] = (uint8_t)((z >> 8) & 0xFF); + frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0); + frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0); + break; + } + case RC_CAN_DATA_KEYBOARD: { + frame.id = rc_can->param.keyboard_id; + frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF); + frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF); + for (int i = 0; i < 6; i++) { + frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0; + } + break; + } + default: + return DEVICE_ERR; + } + #undef RC_CAN_CLAMP + if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id, + frame.data, frame.dlc) != BSP_OK) { + return DEVICE_ERR; + } + return DEVICE_OK; +} + +/** + * @brief 接收并更新CAN数据 + * @param rc_can RC_CAN结构体指针 + * @param data_type 数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t RC_CAN_Update(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) { + if (rc_can == NULL) { + return DEVICE_ERR_NULL; + } + + // 只有从机模式才能接收数据 + if (rc_can->param.mode != RC_CAN_MODE_SLAVE) { + return DEVICE_ERR; + } + BSP_CAN_Message_t msg; + + switch (data_type) { + case RC_CAN_DATA_JOYSTICK: + if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.joy_id, &msg, + BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + return DEVICE_ERR; + } + // 解包数据 + int16_t ch_l_x = (int16_t)((msg.data[1] << 8) | msg.data[0]); + int16_t ch_l_y = (int16_t)((msg.data[3] << 8) | msg.data[2]); + int16_t ch_r_x = (int16_t)((msg.data[5] << 8) | msg.data[4]); + int16_t ch_r_y = (int16_t)((msg.data[7] << 8) | msg.data[6]); + + // 转换为浮点数（范围：-1.0到1.0） + rc_can->data.joy.ch_l_x = (float)ch_l_x / 32768.0f; + rc_can->data.joy.ch_l_y = (float)ch_l_y / 32768.0f; + rc_can->data.joy.ch_r_x = (float)ch_r_x / 32768.0f; + rc_can->data.joy.ch_r_y = (float)ch_r_y / 32768.0f; + break; + case RC_CAN_DATA_SWITCH: + if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.sw_id, &msg, + BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + return DEVICE_ERR; + } + // 解包数据 + rc_can->data.sw.sw_l = (RC_CAN_SW_t)msg.data[0]; + rc_can->data.sw.sw_r = (RC_CAN_SW_t)msg.data[1]; + + int16_t ch_res = (int16_t)((msg.data[3] << 8) | msg.data[2]); + rc_can->data.sw.ch_res = (float)ch_res / 32768.0f; + break; + case RC_CAN_DATA_MOUSE: + if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.mouse_id, &msg, + BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + return DEVICE_ERR; + } + // 解包数据 + int16_t x = (int16_t)((msg.data[1] << 8) | msg.data[0]); + int16_t y = (int16_t)((msg.data[3] << 8) | msg.data[2]); + int16_t z = (int16_t)((msg.data[5] << 8) | msg.data[4]); + rc_can->data.mouse.x = (float)x; + rc_can->data.mouse.y = (float)y; + rc_can->data.mouse.z = (float)z; + rc_can->data.mouse.mouse_l = (msg.data[6] & 0x01) ? true : false; + rc_can->data.mouse.mouse_r = (msg.data[7] & 0x01) ? true : false; + break; + case RC_CAN_DATA_KEYBOARD: + if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.keyboard_id, &msg, + BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + return DEVICE_ERR; + } + if (msg.dlc < 2) { + return DEVICE_ERR; + } + // 解包数据 + rc_can->data.keyboard.key_value = + (uint16_t)((msg.data[1] << 8) | msg.data[0]); + for (int i = 0; i < 6 && (i + 2) < msg.dlc; i++) { + rc_can->data.keyboard.keys[i] = (RC_CAN_Key_t)(msg.data[2 + i]); + } + // 清空未使用的按键位置 + for (int i = (msg.dlc > 2 ? msg.dlc - 2 : 0); i < 6; i++) { + rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE; + } + break; + default: + return DEVICE_ERR; + } + + // 更新header状态 + rc_can->header.online = true; + rc_can->header.last_online_time = BSP_TIME_Get_us(); + + return DEVICE_OK; +} + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 验证RC_CAN参数 + * @param param 参数指针 + * @return DEVICE_OK 成功，其他值失败 + */ +static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + // 检查CAN总线有效性 + if (param->can >= BSP_CAN_NUM) { + return DEVICE_ERR; + } + + // 检查工作模式有效性 + if (param->mode != RC_CAN_MODE_MASTER && param->mode != RC_CAN_MODE_SLAVE) { + return DEVICE_ERR; + } + + // 检查CAN ID是否重复 + if (param->joy_id == param->sw_id || param->joy_id == param->mouse_id || + param->joy_id == param->keyboard_id || param->sw_id == param->mouse_id || + param->sw_id == param->keyboard_id || + param->mouse_id == param->keyboard_id) { + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +/** + * @brief 注册CAN ID + * @param rc_can RC_CAN结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can) { + if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.joy_id, 0) != + BSP_OK) { + return DEVICE_ERR; + } + if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.sw_id, 0) != BSP_OK) { + return DEVICE_ERR; + } + if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.mouse_id, 0) != + BSP_OK) { + return DEVICE_ERR; + } + if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.keyboard_id, 0) != + BSP_OK) { + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can){ + if (rc_can == NULL) { + return DEVICE_ERR_NULL; + } + rc_can->header.online = false; + return DEVICE_OK; +} +/* USER CODE BEGIN */ + +/* USER CODE END */ diff --git a/assets/User_code/device/rc_can.h b/assets/User_code/device/rc_can.h new file mode 100644 index 0000000..e5c6d90 --- /dev/null +++ b/assets/User_code/device/rc_can.h @@ -0,0 +1,157 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "bsp/can.h" +#include "device/device.h" +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + RC_CAN_SW_ERR = 0, + RC_CAN_SW_UP = 1, + RC_CAN_SW_MID = 3, + RC_CAN_SW_DOWN = 2, +} RC_CAN_SW_t; + +typedef enum { + RC_CAN_MODE_MASTER = 0, // 主机模式 + RC_CAN_MODE_SLAVE = 1, // 从机模式 +} RC_CAN_Mode_t; + +typedef enum { + RC_CAN_DATA_JOYSTICK = 0, + RC_CAN_DATA_SWITCH, + RC_CAN_DATA_MOUSE, + RC_CAN_DATA_KEYBOARD +} RC_CAN_DataType_t; + +typedef enum { + RC_CAN_KEY_NONE = 0xFF, // 无按键 + RC_CAN_KEY_W = 0, + RC_CAN_KEY_S, + RC_CAN_KEY_A, + RC_CAN_KEY_D, + RC_CAN_KEY_SHIFT, + RC_CAN_KEY_CTRL, + RC_CAN_KEY_Q, + RC_CAN_KEY_E, + RC_CAN_KEY_R, + RC_CAN_KEY_F, + RC_CAN_KEY_G, + RC_CAN_KEY_Z, + RC_CAN_KEY_X, + RC_CAN_KEY_C, + RC_CAN_KEY_V, + RC_CAN_KEY_B, + RC_CAN_KEY_NUM, +} RC_CAN_Key_t; + +// 遥杆数据包 +typedef struct { + float ch_l_x; + float ch_l_y; + float ch_r_x; + float ch_r_y; +} RC_CAN_JoyData_t; + +// 拨杆数据包 +typedef struct { + RC_CAN_SW_t sw_l; // 左拨杆状态 + RC_CAN_SW_t sw_r; // 右拨杆状态 + float ch_res; // 第五通道 +} RC_CAN_SwitchData_t; + +// 鼠标数据包 +typedef struct { + float x; // 鼠标X轴移动 + float y; // 鼠标Y轴移动 + float z; // 鼠标Z轴(滚轮) + bool mouse_l; // 鼠标左键 + bool mouse_r; // 鼠标右键 +} RC_CAN_MouseData_t; + +// 键盘数据包 +typedef struct { + uint16_t key_value; // 键盘按键位映射 + RC_CAN_Key_t keys[16]; +} RC_CAN_KeyboardData_t; + + +typedef struct { + RC_CAN_JoyData_t joy; + RC_CAN_SwitchData_t sw; + RC_CAN_MouseData_t mouse; + RC_CAN_KeyboardData_t keyboard; +} RC_CAN_Data_t; + +// RC_CAN 参数结构 +typedef struct { + BSP_CAN_t can; // 使用的CAN总线 + RC_CAN_Mode_t mode; // 工作模式 + uint16_t joy_id; // 遥杆CAN ID + uint16_t sw_id; // 拨杆CAN ID + uint16_t mouse_id; // 鼠标CAN ID + uint16_t keyboard_id; // 键盘CAN ID +} RC_CAN_Param_t; + +// RC_CAN 主结构 +typedef struct { + DEVICE_Header_t header; + RC_CAN_Param_t param; + RC_CAN_Data_t data; +} RC_CAN_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化RC CAN发送模块 + * @param rc_can RC_CAN结构体指针 + * @param param 初始化参数 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param); + +/** + * @brief 更新并发送数据到CAN总线 + * @param rc_can RC_CAN结构体指针 + * @param data_type 数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type); + +/** + * @brief 接收并更新CAN数据 + * @param rc_can RC_CAN结构体指针 + * @param data_type 数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t RC_CAN_Update(RC_CAN_t *rc_can , RC_CAN_DataType_t data_type); + +int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can); +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/device/servo.c b/assets/User_code/device/servo.c new file mode 100644 index 0000000..7981bf7 --- /dev/null +++ b/assets/User_code/device/servo.c @@ -0,0 +1,47 @@ +/* + pwm���ƶ�� +*/ + +/*Includes -----------------------------------------*/ + +#include "bsp/pwm.h" +#include "servo.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define SERVO_MIN_DUTY 0.025f +#define SERVO_MAX_DUTY 0.125f + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief + * @param + * @retval BSP_OK / BSP_ERR + */ + +int8_t SERVO_Init(SERVO_t *servo) { + if (servo == NULL) return BSP_ERR; + return BSP_PWM_Start(servo->pwm_ch); +} + +int8_t SERVO_SetAngle(SERVO_t *servo, float angle) { + if (servo == NULL) return BSP_ERR; + + /*���ƽǶȷ�Χ*/ + if (angle < 0.0f) angle = 0.0f; + if (angle > 180.0f) angle = 180.0f; + /*�Ƕ�ӳ�䵽ռ�ձ�*/ + float duty = servo->min_duty + (angle / 180.0f) * (servo->max_duty - servo->min_duty); + + return BSP_PWM_Set(servo->pwm_ch, duty); +} + +int8_t SERVO_Stop(SERVO_t *servo) { + if (servo == NULL) return BSP_ERR; + return BSP_PWM_Stop(servo->pwm_ch); +} \ No newline at end of file diff --git a/assets/User_code/device/servo.h b/assets/User_code/device/servo.h new file mode 100644 index 0000000..ba8562d --- /dev/null +++ b/assets/User_code/device/servo.h @@ -0,0 +1,68 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/pwm.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/** + * @brief + */ +typedef struct { + BSP_PWM_Channel_t pwm_ch; + float min_duty; + float max_duty; +} SERVO_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief + * @param servo + * @retval BSP_OK / BSP_ERR + */ + +int8_t SERVO_Init(SERVO_t *servo); + +/** + * @brief + * @param servo + * @param angle + * @retval BSP_OK / BSP_ERR + */ +int8_t SERVO_SetAngle(SERVO_t *servo, float angle); + +/** + * @brief + * @param servo + * @retval BSP_OK / BSP_ERR + */ + +int8_t SERVO_Stop(SERVO_t *servo); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/vofa.c b/assets/User_code/device/vofa.c new file mode 100644 index 0000000..8561042 --- /dev/null +++ b/assets/User_code/device/vofa.c @@ -0,0 +1,106 @@ +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include "device/vofa.h" +#include "bsp/uart.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ + +#define MAX_CHANNEL 64u // 根据实际最大通道数调整 + +#define JUSTFLOAT_TAIL 0x7F800000 + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static uint8_t vofa_tx_buf[sizeof(float) * MAX_CHANNEL + sizeof(uint32_t)]; +static VOFA_Protocol_t current_protocol = VOFA_PROTOCOL_FIREWATER; // 默认协议 + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/************************ RawData *************************/ +void VOFA_RawData_Send(const char* data, bool dma) { + BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t*)data, strlen(data), dma); +} + +/************************ FireWater *************************/ +void VOFA_FireWater_Send(float *channels, uint8_t channel_count, bool dma) +{ + if (channel_count == 0 || channel_count > MAX_CHANNEL) + return; + + char *buf = (char *)vofa_tx_buf; + size_t len = 0; + + for (uint8_t i = 0; i < channel_count; ++i) { + len += snprintf(buf + len, + sizeof(vofa_tx_buf) - len, + "%s%.2f", + (i ? "," : ""), + channels[i]); + } + snprintf(buf + len, sizeof(vofa_tx_buf) - len, "\n"); + + BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, strlen(buf), dma); +} + +/************************ JustFloat *************************/ +void VOFA_JustFloat_Send(float *channels, uint8_t channel_count, bool dma) +{ + if (channel_count == 0 || channel_count > MAX_CHANNEL) + return; + memcpy(vofa_tx_buf, channels, channel_count * sizeof(float)); + + uint32_t tail = JUSTFLOAT_TAIL; // 0x7F800000 + memcpy(vofa_tx_buf + channel_count * sizeof(float), &tail, sizeof(tail)); + + BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, channel_count * sizeof(float) + sizeof(tail), dma); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t VOFA_init(VOFA_Protocol_t protocol) { + current_protocol = protocol; + return DEVICE_OK; +} + +int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma) { + switch (current_protocol) { + case VOFA_PROTOCOL_RAWDATA: + { + char data[256]; + if (channel_count >= 1) { + sprintf(data, "Channel1: %.2f", channels[0]); + if (channel_count >= 2) { + sprintf(data + strlen(data), ", Channel2: %.2f", channels[1]); + } + strcat(data, "\n"); + VOFA_RawData_Send(data, dma); + } + } + break; + case VOFA_PROTOCOL_FIREWATER: + VOFA_FireWater_Send(channels, channel_count, dma); + break; + case VOFA_PROTOCOL_JUSTFLOAT: + VOFA_JustFloat_Send(channels, channel_count, dma); + break; + default: + return DEVICE_ERR; + } + return DEVICE_OK; +} diff --git a/assets/User_code/device/vofa.h b/assets/User_code/device/vofa.h new file mode 100644 index 0000000..b8f07d9 --- /dev/null +++ b/assets/User_code/device/vofa.h @@ -0,0 +1,39 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "bsp/uart.h" +#include "device/device.h" +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ + +typedef enum { + VOFA_PROTOCOL_RAWDATA, + VOFA_PROTOCOL_FIREWATER, + VOFA_PROTOCOL_JUSTFLOAT, +} VOFA_Protocol_t; + +/** + * @brief 初始化VOFA设备 + * @param protocol 设置通信协议 + * @return + */ +int8_t VOFA_init(VOFA_Protocol_t protocol); + +/** + * @brief 发送数据到VOFA + * @param channels 要发送的通道数据 + * @param channel_count 通道数量 + * @param dma 是否使用DMA发送 + * @return + */ +int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/device/ws2812.c b/assets/User_code/device/ws2812.c new file mode 100644 index 0000000..101edcb --- /dev/null +++ b/assets/User_code/device/ws2812.c @@ -0,0 +1,114 @@ +/* Includes ----------------------------------------------------------------- */ +#include "ws2812.h" +#include "device.h" + +#include "bsp/pwm.h" +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define DEVICE_WS2812_T1H (uint16_t)(BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.56) // High-level width of logic-1 pulse +#define DEVICE_WS2812_T0H (BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.29) // High-level width of logic-0 pulse +#define DEVICE_WS2812_WS_REST 40 // Number of reset pulses (low level) after data stream +#define DEVICE_WS2812_DATA_LEN 24 // WS2812 data length: 24 bits (GRB) per LED +#define DEVICE_WS2812_RST_NUM 50 // Extra reset pulses reserved at the end of the buffer + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static uint16_t DEVICE_WS2812_LED_NUM; // Total number of LEDs +static uint16_t *DEVICE_WS2812_RGB_Buff = NULL;// PWM duty buffer for DMA +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/* Exported functions ------------------------------------------------------- */ +/** + * Set color of a single WS2812 LED + * @param num LED index (1-based) + * @param R Red value (0-255) + * @param G Green value (0-255) + * @param B Blue value (0-255) + * @return DEVICE_OK on success, DEVICE_ERR if num is invalid + */ +uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B) +{ + if(num<1 || num>DEVICE_WS2812_LED_NUM) return DEVICE_ERR; + uint32_t indexx = (num-1) * DEVICE_WS2812_DATA_LEN; + + /* WS2812 uses GRB order, MSB first */ + for (uint8_t i = 0; i < 8; i++) { + // G + DEVICE_WS2812_RGB_Buff[indexx + i] = (G & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H; + // R + DEVICE_WS2812_RGB_Buff[indexx + i + 8] = (R & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H; + // B + DEVICE_WS2812_RGB_Buff[indexx + i + 16] = (B & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H; + } + return DEVICE_OK; +} + +/** + * Initialize WS2812 driver + * @param ledNum Number of LEDs in the strip + * @return DEVICE_OK on success, DEVICE_ERR if memory allocation or PWM setup fails + */ +uint8_t DEVICE_WS2812_Init(uint16_t ledNum) +{ + DEVICE_WS2812_LED_NUM = ledNum; + + if (DEVICE_WS2812_RGB_Buff != NULL) + { + free(DEVICE_WS2812_RGB_Buff); + DEVICE_WS2812_RGB_Buff = NULL; + } + + /* Allocate new buffer: 24 PWM samples per LED + reset pulses */ + size_t bufLen = ledNum * DEVICE_WS2812_DATA_LEN + DEVICE_WS2812_RST_NUM; + DEVICE_WS2812_RGB_Buff = (uint16_t *)malloc(bufLen * sizeof(uint16_t)); + if (DEVICE_WS2812_RGB_Buff == NULL) + return DEVICE_ERR; + + /* Initialize all LEDs to dim green */ + for (int i = 1; i <= ledNum; i++) + DEVICE_WS2812_Set(i, 0, 20, 0); + + /* Configure PWM frequency to 800 kHz and start DMA */ + if (BSP_PWM_SetFreq(BSP_PWM_WS2812, 800000) == DEVICE_OK) + BSP_PWM_Start_DMA( + BSP_PWM_WS2812, + (uint32_t *)DEVICE_WS2812_RGB_Buff, + bufLen); + else + return DEVICE_ERR; + + return DEVICE_OK; +} + +/** + * De-initialize WS2812 driver + * Frees the DMA buffer and stops PWM + */ +void DEVICE_WS2812_DeInit() +{ + for (int i = 1; i <= DEVICE_WS2812_LED_NUM; i++) + DEVICE_WS2812_Set(i, 0, 0, 0); + if (DEVICE_WS2812_RGB_Buff != NULL) + { + free(DEVICE_WS2812_RGB_Buff); + DEVICE_WS2812_RGB_Buff = NULL; + } + BSP_PWM_Stop_DMA(BSP_PWM_WS2812); +} \ No newline at end of file diff --git a/assets/User_code/device/ws2812.h b/assets/User_code/device/ws2812.h new file mode 100644 index 0000000..9a26cfa --- /dev/null +++ b/assets/User_code/device/ws2812.h @@ -0,0 +1,19 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +uint8_t DEVICE_WS2812_Init(uint16_t led_num); +uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B); +void DEVICE_WS2812_DeInit(); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/assets/User_code/module/.gitkeep b/assets/User_code/module/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/assets/User_code/module/config.c b/assets/User_code/module/config.c new file mode 100644 index 0000000..cb4ed7c --- /dev/null +++ b/assets/User_code/module/config.c @@ -0,0 +1,31 @@ +/* + * 配置相关 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "module/config.h" + + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ + +/* Exported variables ------------------------------------------------------- */ + +// 机器人参数配置 +Config_RobotParam_t robot_config = { + + +}; + +/* Private function prototypes ---------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void) { + return &robot_config; +} \ No newline at end of file diff --git a/assets/User_code/module/config.h b/assets/User_code/module/config.h new file mode 100644 index 0000000..446fb6c --- /dev/null +++ b/assets/User_code/module/config.h @@ -0,0 +1,27 @@ +/* + * 配置相关 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef struct { + +} Config_RobotParam_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void); + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/module/gimbal/2_axis_gimbal/2_axis_gimbal.c b/assets/User_code/module/gimbal/2_axis_gimbal/2_axis_gimbal.c new file mode 100644 index 0000000..53ff302 --- /dev/null +++ b/assets/User_code/module/gimbal/2_axis_gimbal/2_axis_gimbal.c @@ -0,0 +1,259 @@ +/* + * 云台模组 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "bsp/can.h" +#include "bsp/time.h" +#include "component/filter.h" +#include "component/pid.h" +#include "device/motor_dm.h" +#include "device/motor_rm.h" +#include "gimbal.h" +#include + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ + +/** + * \brief 设置云台模式 + * + * \param c 包含云台数据的结构体 + * \param mode 要设置的模式 + * + * \return 函数运行结果 + */ +static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { + if (g == NULL) + return -1; + if (mode == g->mode) + return GIMBAL_OK; + + PID_Reset(&g->pid.yaw_angle); + PID_Reset(&g->pid.yaw_omega); + PID_Reset(&g->pid.pit_angle); + PID_Reset(&g->pid.pit_omega); + LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f); + LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); + + MOTOR_DM_Enable(&(g->param->yaw_motor)); + + AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ + // if (g->mode == GIMBAL_MODE_RELAX) { + // if (mode == GIMBAL_MODE_ABSOLUTE) { + // g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + // } else if (mode == GIMBAL_MODE_RELATIVE) { + // g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + // } + // } + g->setpoint.eulr.pit = g->feedback.imu.eulr.rol; + g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + + g->mode = mode; + return 0; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * \brief 初始化云台 + * + * \param g 包含云台数据的结构体 + * \param param 包含云台参数的结构体指针 + * \param target_freq 任务预期的运行频率 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, + float target_freq) { + if (g == NULL) + return -1; + + g->param = param; + g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */ + + /* 初始化云台电机控制PID和LPF */ + PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq, + &(g->param->pid.yaw_angle)); + PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq, + &(g->param->pid.yaw_omega)); + PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq, + &(g->param->pid.pit_angle)); + PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq, + &(g->param->pid.pit_omega)); + + LowPassFilter2p_Init(&g->filter_out.yaw, target_freq, + g->param->low_pass_cutoff_freq.out); + LowPassFilter2p_Init(&g->filter_out.pit, target_freq, + g->param->low_pass_cutoff_freq.out); + g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw; + g->limit.yaw.min = g->param->mech_zero.yaw; + g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit; + g->limit.pit.min = g->param->mech_zero.pit; + BSP_CAN_Init(); + + MOTOR_RM_Register(&(g->param->pit_motor)); + MOTOR_DM_Register(&(g->param->yaw_motor)); + + MOTOR_DM_Enable(&(g->param->yaw_motor)); + return 0; +} + +/** + * \brief 通过CAN设备更新云台反馈信息 + * + * \param gimbal 云台 + * \param can CAN设备 + * + * \return 函数运行结果 + */ +int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) { + if (gimbal == NULL) + return -1; + + /* 更新RM电机反馈数据（pitch轴） */ + MOTOR_RM_Update(&(gimbal->param->pit_motor)); + MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor)); + if (rm_motor != NULL) { + gimbal->feedback.motor.pit = rm_motor->feedback; + } + + /* 更新DM电机反馈数据（yaw轴） */ + MOTOR_DM_Update(&(gimbal->param->yaw_motor)); + MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor)); + if (dm_motor != NULL) { + gimbal->feedback.motor.yaw = dm_motor->motor.feedback; + } + + return 0; +} + +int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu) { + + if (gimbal == NULL) { + return -1; + } + gimbal->feedback.imu.gyro = imu->gyro; + gimbal->feedback.imu.eulr = imu->eulr; +} + +/** + * \brief 运行云台控制逻辑 + * + * \param g 包含云台数据的结构体 + * \param g_cmd 云台控制指令 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) { + if (g == NULL || g_cmd == NULL) { + return -1; + } + + g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f; + g->lask_wakeup = BSP_TIME_Get_us(); + + Gimbal_SetMode(g, g_cmd->mode); + + /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */ + float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f; + if (g->param->travel.yaw > 0) { + /* 计算当前电机角度与IMU角度的偏差 */ + float motor_imu_offset = + g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw; + /* 处理跨越±π的情况 */ + if (motor_imu_offset > M_PI) + motor_imu_offset -= M_2PI; + if (motor_imu_offset < -M_PI) + motor_imu_offset += M_2PI; + + /* 计算到限位边界的距离 */ + const float delta_max = CircleError( + g->limit.yaw.max, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), + M_2PI); + const float delta_min = CircleError( + g->limit.yaw.min, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), + M_2PI); + + /* 限制控制命令 */ + if (delta_yaw > delta_max) + delta_yaw = delta_max; + if (delta_yaw < delta_min) + delta_yaw = delta_min; + } + CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI); + + /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */ + float delta_pit = g_cmd->delta_pit * g->dt; + if (g->param->travel.pit > 0) { + /* 计算当前电机角度与IMU角度的偏差 */ + float motor_imu_offset = + g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol; + /* 处理跨越±π的情况 */ + if (motor_imu_offset > M_PI) + motor_imu_offset -= M_2PI; + if (motor_imu_offset < -M_PI) + motor_imu_offset += M_2PI; + + /* 计算到限位边界的距离 */ + const float delta_max = CircleError( + g->limit.pit.max, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), + M_2PI); + const float delta_min = CircleError( + g->limit.pit.min, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), + M_2PI); + + /* 限制控制命令 */ + if (delta_pit > delta_max) + delta_pit = delta_max; + if (delta_pit < delta_min) + delta_pit = delta_min; + } + CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI); + + /* 控制相关逻辑 */ + float yaw_omega_set_point, pit_omega_set_point; + switch (g->mode) { + case GIMBAL_MODE_RELAX: + g->out.yaw = 0.0f; + g->out.pit = 0.0f; + break; + + case GIMBAL_MODE_ABSOLUTE: + yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw, + g->feedback.imu.eulr.yaw, 0.0f, g->dt); + g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point, + g->feedback.imu.gyro.z, 0.f, g->dt); + + pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit, + g->feedback.imu.eulr.rol, 0.0f, g->dt); + g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, + g->feedback.imu.gyro.y, 0.f, g->dt); + + break; + + /* 输出滤波 */ + g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw); + g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit); + + return 0; + } +} + +/** + * \brief 云台输出 + * + * \param s 包含云台数据的结构体 + * \param out CAN设备云台输出结构体 + */ +void Gimbal_Output(Gimbal_t *g) { + MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit); + MOTOR_MIT_Output_t output = {0}; + output.torque = g->out.yaw * 5.0f; // 乘以减速比 + output.kd = 0.3f; + MOTOR_RM_Ctrl(&g->param->pit_motor); + MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output); +} diff --git a/assets/User_code/module/gimbal/2_axis_gimbal/2_axis_gimbal.h b/assets/User_code/module/gimbal/2_axis_gimbal/2_axis_gimbal.h new file mode 100644 index 0000000..e45faa7 --- /dev/null +++ b/assets/User_code/module/gimbal/2_axis_gimbal/2_axis_gimbal.h @@ -0,0 +1,179 @@ +/* + * 云台模组 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "component/ahrs.h" +#include "component/filter.h" +#include "component/pid.h" +#include "device/motor.h" +#include "device/motor_dm.h" +#include "device/motor_rm.h" + +/* Exported constants ------------------------------------------------------- */ +#define GIMBAL_OK (0) /* 运行正常 */ +#define GIMBAL_ERR (-1) /* 运行时发现了其他错误 */ +#define GIMBAL_ERR_NULL (-2) /* 运行时发现NULL指针 */ +#define GIMBAL_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */ + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +typedef enum { + GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ + GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ +} Gimbal_Mode_t; + +typedef struct { + Gimbal_Mode_t mode; + float delta_yaw; + float delta_pit; +} Gimbal_CMD_t; + +/* 软件限位 */ +typedef struct { + float max; + float min; +} Gimbal_Limit_t; + +/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/ +typedef struct { + MOTOR_RM_Param_t pit_motor; /* pitch轴电机参数 */ + MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */ + struct { + KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */ + KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */ + KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ + KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ + } pid; + + /* 低通滤波器截止频率 */ + struct { + float out; /* 电机输出 */ + float gyro; /* 陀螺仪数据 */ + } low_pass_cutoff_freq; + + struct { + float yaw; /* yaw轴机械限位 */ + float pit; /* pitch轴机械限位 */ + } mech_zero; + + struct { + float yaw; /* yaw轴机械限位行程 -1表示无限位 */ + float pit; /* pitch轴机械限位行程 -1表示无限位*/ + } travel; + +} Gimbal_Params_t; + +typedef struct { + AHRS_Gyro_t gyro; + AHRS_Eulr_t eulr; +} Gimbal_IMU_t; +/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */ +typedef struct { + Gimbal_IMU_t imu; + struct { + MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */ + MOTOR_Feedback_t pit; /* pitch轴电机反馈 */ + } motor; +} Gimbal_Feedback_t; + +/* 云台输出数据的结构体*/ +typedef struct { + float yaw; /* yaw轴电机输出 */ + float pit; /* pitch轴电机输出 */ +} Gimbal_Output_t; +/* + * 运行的主结构体，所有这个文件里的函数都在操作这个结构体。 + * 包含了初始化参数，中间变量，输出变量。 + */ +typedef struct { + uint64_t lask_wakeup; + float dt; + + Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */ + /* 模块通用 */ + Gimbal_Mode_t mode; /* 云台模式 */ + + /* PID计算的目标值 */ + struct { + AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */ + } setpoint; + + struct { + KPID_t yaw_angle; /* yaw轴角位置环PID */ + KPID_t yaw_omega; /* yaw轴角速度环PID */ + KPID_t pit_angle; /* pitch轴角位置环PID */ + KPID_t pit_omega; /* pitch轴角速度环PID */ + } pid; + + struct { + Gimbal_Limit_t yaw; + Gimbal_Limit_t pit; + } limit; + + struct { + LowPassFilter2p_t yaw; + LowPassFilter2p_t pit; + } filter_out; + + Gimbal_Output_t out; /* 云台输出 */ + Gimbal_Feedback_t feedback; /* 反馈 */ + +} Gimbal_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * \brief 初始化云台 + * + * \param g 包含云台数据的结构体 + * \param param 包含云台参数的结构体指针 + * \param target_freq 任务预期的运行频率 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, + float target_freq); + +/** + * \brief 通过CAN设备更新云台反馈信息 + * + * \param gimbal 云台 + * \param can CAN设备 + * + * \return 函数运行结果 + */ +int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal); + +int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu); +/** + * \brief 运行云台控制逻辑 + * + * \param g 包含云台数据的结构体 + * \param fb 云台反馈信息 + * \param g_cmd 云台控制指令 + * \param dt_sec 两次调用的时间间隔 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd); + +/** + * \brief 云台输出 + * + * \param s 包含云台数据的结构体 + * \param out CAN设备云台输出结构体 + */ +void Gimbal_Output(Gimbal_t *g); + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/module/gimbal/2_axis_gimbal/config.yaml b/assets/User_code/module/gimbal/2_axis_gimbal/config.yaml new file mode 100644 index 0000000..e69de29 diff --git a/assets/User_code/task/.gitkeep b/assets/User_code/task/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/assets/User_code/task/init.c.template b/assets/User_code/task/init.c.template new file mode 100644 index 0000000..5dc631a --- /dev/null +++ b/assets/User_code/task/init.c.template @@ -0,0 +1,42 @@ +/* + Init Task + 任务初始化，创建各个线程任务和消息队列 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * \brief 初始化 + * + * \param argument 未使用 + */ +void Task_Init(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + osKernelLock(); /* 锁定内核，防止任务切换 */ + + /* 创建任务线程 */ +{{thread_creation_code}} + + // 创建消息队列 + /* USER MESSAGE BEGIN */ + task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); + /* USER MESSAGE END */ + + osKernelUnlock(); // 解锁内核 + osThreadTerminate(osThreadGetId()); // 任务完成后结束自身 +} diff --git a/assets/User_code/task/task.c.template b/assets/User_code/task/task.c.template new file mode 100644 index 0000000..e238fa2 --- /dev/null +++ b/assets/User_code/task/task.c.template @@ -0,0 +1,56 @@ +/* + {{task_name}} Task + {{task_description}} +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void {{task_function}}(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + {% if freq_control %} + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / {{task_frequency}}; + + osDelay({{task_delay}}); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + {% else %} + osDelay({{task_delay}}); /* 延时一段时间再开启任务 */ + + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + /* USER CODE BEGIN */ + + /* USER CODE END */ + } + {% endif %} +} \ No newline at end of file diff --git a/assets/User_code/task/user_task.c.template b/assets/User_code/task/user_task.c.template new file mode 100644 index 0000000..69f49f0 --- /dev/null +++ b/assets/User_code/task/user_task.c.template @@ -0,0 +1,12 @@ +#include "task/user_task.h" + +Task_Runtime_t task_runtime; + +const osThreadAttr_t attr_init = { + .name = "Task_Init", + .priority = osPriorityRealtime, + .stack_size = 256 * 4, +}; + +/* User_task */ +{{task_attr_definitions}} diff --git a/assets/User_code/task/user_task.h.template b/assets/User_code/task/user_task.h.template new file mode 100644 index 0000000..79b7476 --- /dev/null +++ b/assets/User_code/task/user_task.h.template @@ -0,0 +1,80 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif +/* Includes ----------------------------------------------------------------- */ +#include +#include "FreeRTOS.h" +#include "task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Exported constants ------------------------------------------------------- */ +/* 任务运行频率 */ +{{task_frequency_definitions}} + +/* 任务初始化延时ms */ +#define TASK_INIT_DELAY (100u) +{{task_init_delay_definitions}} + +/* Exported defines --------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 任务运行时结构体 */ +typedef struct { + /* 各任务，也可以叫做线程 */ + struct { +{{thread_definitions}} + } thread; + + /* USER MESSAGE BEGIN */ + struct { + osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ + } msgq; + /* USER MESSAGE END */ + + /* 机器人状态 */ + struct { + float battery; /* 电池电量百分比 */ + float vbat; /* 电池电压 */ + float cpu_temp; /* CPU温度 */ + } status; + + /* USER CONFIG BEGIN */ + + /* USER CONFIG END */ + + /* 各任务的stack使用 */ + struct { +{{stack_definitions}} + } stack_water_mark; + + /* 各任务运行频率 */ + struct { +{{freq_definitions}} + } freq; + + /* 任务最近运行时间 */ + struct { +{{last_up_time_definitions}} + } last_up_time; + +} Task_Runtime_t; + +/* 任务运行时结构体 */ +extern Task_Runtime_t task_runtime; + +/* 初始化任务句柄 */ +extern const osThreadAttr_t attr_init; +{{task_attr_declarations}} + +/* 任务函数声明 */ +void Task_Init(void *argument); +{{task_function_declarations}} + +#ifdef __cplusplus +} +#endif diff --git a/assets/logo/M.ico b/assets/logo/M.ico new file mode 100644 index 0000000..b7df782 Binary files /dev/null and b/assets/logo/M.ico differ diff --git a/assets/logo/M.png b/assets/logo/M.png new file mode 100644 index 0000000..300d879 Binary files /dev/null and b/assets/logo/M.png differ diff --git a/assets/logo/M2.ico b/assets/logo/M2.ico new file mode 100644 index 0000000..400b26d Binary files /dev/null and b/assets/logo/M2.ico differ diff --git a/assets/logo/MR.ico b/assets/logo/MR.ico new file mode 100644 index 0000000..ebad3d5 Binary files /dev/null and b/assets/logo/MR.ico differ diff --git a/assets/logo/MR.png b/assets/logo/MR.png new file mode 100644 index 0000000..c546f6f Binary files /dev/null and b/assets/logo/MR.png differ diff --git a/assets/logo/MRobot.ico b/assets/logo/MRobot.ico new file mode 100644 index 0000000..004d771 Binary files /dev/null and b/assets/logo/MRobot.ico differ diff --git a/assets/logo/MRobot.png b/assets/logo/MRobot.png new file mode 100644 index 0000000..4524089 Binary files /dev/null and b/assets/logo/MRobot.png differ diff --git a/assets/logo/m1.png b/assets/logo/m1.png new file mode 100644 index 0000000..4c8bccb Binary files /dev/null and b/assets/logo/m1.png differ diff --git a/assets/mech_lib/.DS_Store b/assets/mech_lib/.DS_Store new file mode 100644 index 0000000..68586da Binary files /dev/null and b/assets/mech_lib/.DS_Store differ diff --git a/assets/mech_lib/README.md b/assets/mech_lib/README.md new file mode 100644 index 0000000..e476d43 --- /dev/null +++ b/assets/mech_lib/README.md @@ -0,0 +1 @@ +# 机械常用零件库 \ No newline at end of file