diff --git a/.DS_Store b/.DS_Store index 3e04fe2..4410221 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/bsp/.DS_Store b/bsp/.DS_Store index dc0ff54..ec9a612 100644 Binary files a/bsp/.DS_Store and b/bsp/.DS_Store differ diff --git a/bsp/bsp.h b/bsp/bsp.h index d1beac0..6a3171b 100644 --- a/bsp/bsp.h +++ b/bsp/bsp.h @@ -9,6 +9,7 @@ extern "C" { #define BSP_ERR_NULL (-2) #define BSP_ERR_INITED (-3) #define BSP_ERR_NO_DEV (-4) +#define BSP_ERR_TIMEOUT (-5) #ifdef __cplusplus } diff --git a/bsp/buzzer_gpio.c b/bsp/buzzer_gpio.c deleted file mode 100644 index bd8406a..0000000 --- a/bsp/buzzer_gpio.c +++ /dev/null @@ -1,32 +0,0 @@ - - -/* Includes ----------------------------------------------------------------- */ -#include -#include "bsp/bsp.h" -#include "bsp/buzzer_gpio.h" -/* Private define ----------------------------------------------------------- */ -#define BSP_BUZZER_GPIO GPIOA -#define BSP_BUZZER_PIN GPIO_PIN_1 -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -/* Private function --------------------------------------------------------- */ -/* Exported functions ------------------------------------------------------- */ -int8_t BSP_Buzzer_Set(BSP_Buzzer_Status_t s) -{ - switch (s) - { - case BSP_BUZZER_ON: - HAL_GPIO_WritePin(BSP_BUZZER_GPIO, BSP_BUZZER_PIN, GPIO_PIN_SET); // 打开蜂鸣器 - break; - case BSP_BUZZER_OFF: - HAL_GPIO_WritePin(BSP_BUZZER_GPIO, BSP_BUZZER_PIN, GPIO_PIN_RESET); // 关闭蜂鸣器 - break; - case BSP_BUZZER_TAGGLE: - HAL_GPIO_TogglePin(BSP_BUZZER_GPIO, BSP_BUZZER_PIN); // 切换蜂鸣器状态 - break; - default: - return BSP_ERR; - } - return BSP_OK; -} \ No newline at end of file diff --git a/bsp/can.c b/bsp/can.c index 20a9c66..5083893 100644 --- a/bsp/can.c +++ b/bsp/can.c @@ -1,68 +1,291 @@ /* Includes ----------------------------------------------------------------- */ -#include "bsp\can.h" +#include "bsp/can.h" +#include "bsp/bsp.h" +#include +#include +#include /* Private define ----------------------------------------------------------- */ +#define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */ +#define CAN_TX_SEMAPHORE_TIMEOUT 1000 /* 发送信号量超时时间(ms) */ +#define CAN_TX_MAILBOX_NUM 3 /* CAN发送邮箱数量 */ + /* Private macro ------------------------------------------------------------ */ /* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void); +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; -/* Private function -------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static BSP_CAN_QueueNode_t *queue_list = NULL; +static osMutexId_t queue_mutex = NULL; +static osSemaphoreId_t tx_semaphore[BSP_CAN_NUM] = {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解析器 */ + +/* 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 int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id); +static void BSP_CAN_RxFifo0Callback(void); +static void BSP_CAN_RxFifo1Callback(void); +static void BSP_CAN_TxCompleteCallback(CAN_HandleTypeDef *hcan); +static void BSP_CAN_TxAbortCallback(CAN_HandleTypeDef *hcan); +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); + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 根据CAN句柄获取BSP_CAN实例 + */ static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) { - if (hcan->Instance == CAN2) - return BSP_CAN_2; - else if (hcan->Instance == CAN1) - return BSP_CAN_1; - else - return BSP_CAN_ERR; + 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 删除指定CAN ID的消息队列 + * @note 内部函数，已包含互斥锁保护 + */ +static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) { + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + BSP_CAN_QueueNode_t **current = &queue_list; + while (*current != NULL) { + if ((*current)->can == can && (*current)->can_id == can_id) { + BSP_CAN_QueueNode_t *to_delete = *current; + *current = (*current)->next; + osMessageQueueDelete(to_delete->queue); + BSP_Free(to_delete); + osMutexRelease(queue_mutex); + return BSP_OK; + } + current = &(*current)->next; + } + osMutexRelease(queue_mutex); + return BSP_ERR; // 未找到 +} +/** + * @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 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); + } + } + } + } +} + +/** + * @brief 发送完成回调统一处理函数 + */ +static void BSP_CAN_TxCompleteCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 释放发送信号量 + if (tx_semaphore[bsp_can] != NULL) { + osSemaphoreRelease(tx_semaphore[bsp_can]); + } + } +} + +/** + * @brief 发送中止回调统一处理函数 + */ +static void BSP_CAN_TxAbortCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 释放发送信号量（发送中止也要释放） + if (tx_semaphore[bsp_can] != NULL) { + osSemaphoreRelease(tx_semaphore[bsp_can]); + } + } +} + +/* 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](); - } + 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](); - } + 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](); - } + 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](); - } + 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](); - } + 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](); - } + 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) { @@ -122,20 +345,312 @@ void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { } /* Exported functions ------------------------------------------------------- */ + +int8_t BSP_CAN_Init(void) { + if (inited) { + return BSP_ERR_INITED; + } + + // 清零回调函数数组 + memset(CAN_Callback, 0, sizeof(CAN_Callback)); + + // 初始化ID解析器为默认解析器 + id_parser = BSP_CAN_DefaultIdParser; + + // 创建互斥锁 + queue_mutex = osMutexNew(NULL); + if (queue_mutex == NULL) { + return BSP_ERR; + } + + // 创建发送信号量，每个CAN通道有3个发送邮箱 + for (int i = 0; i < BSP_CAN_NUM; i++) { + tx_semaphore[i] = osSemaphoreNew(CAN_TX_MAILBOX_NUM, CAN_TX_MAILBOX_NUM, NULL); + if (tx_semaphore[i] == NULL) { + // 清理已创建的信号量 + for (int j = 0; j < i; j++) { + if (tx_semaphore[j] != NULL) { + osSemaphoreDelete(tx_semaphore[j]); + tx_semaphore[j] = NULL; + } + } + if (queue_mutex != NULL) { + osMutexDelete(queue_mutex); + queue_mutex = NULL; + } + return BSP_ERR; + } + } + +/* AUTO GENERATED CAN_INIT */ + + inited = true; + return BSP_OK; +} + +int8_t BSP_CAN_DeInit(void) { + if (!inited) { + return BSP_ERR; + } + + // 删除所有队列 + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) == osOK) { + BSP_CAN_QueueNode_t *current = queue_list; + while (current != NULL) { + BSP_CAN_QueueNode_t *next = current->next; + osMessageQueueDelete(current->queue); + BSP_Free(current); + current = next; + } + queue_list = NULL; + osMutexRelease(queue_mutex); + } + + // 删除发送信号量 + for (int i = 0; i < BSP_CAN_NUM; i++) { + if (tx_semaphore[i] != NULL) { + osSemaphoreDelete(tx_semaphore[i]); + tx_semaphore[i] = NULL; + } + } + + // 删除互斥锁 + if (queue_mutex != NULL) { + osMutexDelete(queue_mutex); + queue_mutex = NULL; + } + + // 清零回调函数数组 + memset(CAN_Callback, 0, sizeof(CAN_Callback)); + + // 重置ID解析器 + id_parser = NULL; + + inited = false; + return BSP_OK; +} + CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) { - switch (can) { - case BSP_CAN_2: - return &hcan2; - case BSP_CAN_1: - return &hcan1; - default: - return NULL; - } + 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 (callback == NULL) return BSP_ERR_NULL; - CAN_Callback[can][type] = callback; - return BSP_OK; + 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; + } + + // 获取发送信号量，确保有可用的发送邮箱 + if (tx_semaphore[can] == NULL) { + return BSP_ERR; + } + + osStatus_t sem_status = osSemaphoreAcquire(tx_semaphore[can], CAN_TX_SEMAPHORE_TIMEOUT); + if (sem_status != osOK) { + return BSP_ERR_TIMEOUT; // 获取信号量超时，表示发送邮箱已满 + } + + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); + if (hcan == NULL) { + // 如果获取句柄失败，需要释放信号量 + osSemaphoreRelease(tx_semaphore[can]); + return BSP_ERR_NULL; + } + + CAN_TxHeaderTypeDef header = {0}; + uint32_t mailbox; + + switch (format) { + case BSP_CAN_FORMAT_STD_DATA: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_EXT_DATA: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_STD_REMOTE: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_REMOTE; + break; + case BSP_CAN_FORMAT_EXT_REMOTE: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_REMOTE; + break; + default: + // 如果格式错误，需要释放信号量 + osSemaphoreRelease(tx_semaphore[can]); + return BSP_ERR; + } + + header.DLC = dlc; + header.TransmitGlobalTime = DISABLE; + + HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox); + + if (result != HAL_OK) { + // 如果发送失败，需要释放信号量 + osSemaphoreRelease(tx_semaphore[can]); + return BSP_ERR; + } + + // 发送成功，信号量将在发送完成回调中释放 + return BSP_OK; +} + +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_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) { + if (!inited) { + return BSP_ERR_INITED; + } + return BSP_CAN_DeleteIdQueue(can, can_id); +} + +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; +} + +int8_t BSP_CAN_UnregisterIdParser(void) { + if (!inited) { + return BSP_ERR_INITED; + } + + id_parser = BSP_CAN_DefaultIdParser; + 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/bsp/can.h b/bsp/can.h index 47bb9a2..c1bc147 100644 --- a/bsp/can.h +++ b/bsp/can.h @@ -6,15 +6,23 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include - #include "bsp/bsp.h" +#include "bsp/mm.h" +#include +#include +#include /* 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 CAN_TX_SEMAPHORE_TIMEOUT 1000 /* 发送信号量超时时间(ms) */ + /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ typedef enum { - BSP_CAN_1, - BSP_CAN_2, +/* AUTO GENERATED BSP_CAN_NAME */ BSP_CAN_NUM, BSP_CAN_ERR, } BSP_CAN_t; @@ -33,14 +41,190 @@ typedef enum { HAL_CAN_SLEEP_CB, HAL_CAN_WAKEUP_FROM_RX_MSG_CB, HAL_CAN_ERROR_CB, - BSP_CAN_CB_NUM + 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); + /* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化 CAN 模块 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_Init(void); + +/** + * @brief 反初始化 CAN 模块 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_DeInit(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 注册 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 ID 接收队列 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id); + +/** + * @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 注销ID解析器 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_UnregisterIdParser(void); + +/** + * @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); + #ifdef __cplusplus } -#endif +#endif \ No newline at end of file diff --git a/bsp/delay.c b/bsp/delay.c deleted file mode 100644 index d906c4a..0000000 --- a/bsp/delay.c +++ /dev/null @@ -1,118 +0,0 @@ -#include "bsp_delay.h" - -#include "cmsis_os.h" -#include "main.h" - -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -static uint8_t fac_us = 0; -static uint32_t fac_ms = 0; -/* Private function -------------------------------------------------------- */ -static void delay_ticks(uint32_t ticks) -{ - uint32_t told = SysTick->VAL; - uint32_t tnow = 0; - uint32_t tcnt = 0; - uint32_t reload = SysTick->LOAD; - while (1) - { - tnow = SysTick->VAL; - if (tnow != told) - { - if (tnow < told) - { - tcnt += told - tnow; - } - else - { - tcnt += reload - tnow + told; - } - told = tnow; - if (tcnt >= ticks) - { - break; - } - } - } -} -/* Exported functions ------------------------------------------------------- */ - -/** - * @brief 毫秒延时函数 - * @param ms - * @return - */ -int8_t BSP_Delay(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; -} - -/** - * @brief 延时初始化 - * @param - * @return - */ -int8_t BSP_Delay_Init(void) -{ - if (SystemCoreClock == 0) - { - return BSP_ERR; - } - fac_us = SystemCoreClock / 1000000; - fac_ms = SystemCoreClock / 1000; - return BSP_OK; -} - -/** - * @brief 微秒延时，注意：此函数会阻塞当前线程，直到延时结束，并且会被中断打断 - * @param us - * @return - */ -int8_t BSP_Delay_us(uint32_t us) -{ - if (fac_us == 0) - { - return BSP_ERR; - } - uint32_t ticks = us * fac_us; - delay_ticks(ticks); - return BSP_OK; -} - -/** - * @brief 毫秒延时，注意：此函数会阻塞当前线程，直到延时结束，并且会被中断打断 - * @param ms - * @return - */ -int8_t BSP_Delay_ms(uint32_t ms) -{ - if (fac_ms == 0) - { - return BSP_ERR; - } - uint32_t ticks = ms * fac_ms; - delay_ticks(ticks); - return BSP_OK; -} diff --git a/bsp/dependencies.csv b/bsp/dependencies.csv deleted file mode 100644 index 2f81068..0000000 --- a/bsp/dependencies.csv +++ /dev/null @@ -1,2 +0,0 @@ -i2c,I2C -uart,USART,UART \ No newline at end of file diff --git a/bsp/describe.csv b/bsp/describe.csv index 2788534..fd64d14 100644 --- a/bsp/describe.csv +++ b/bsp/describe.csv @@ -1,3 +1,11 @@ -uart,要求开启dma和中断 -can,要求开启can的中断 -delay,暂时只有delay_ms函数 \ No newline at end of file +uart,请开启uart的dma和中断 +can,请开启can中断，使用函数前请确保can已经初始化。 +gpio,会自动读取cubemx中配置为gpio的引脚，并自动区分输入输出和中断。 +spi,请开启spi的dma和中断 +i2c,要求开始spi中断 +mm,这是套了一层的动态内存分配 +time,获取时间戳函数，需要开启freerots +dwt,需要开启dwt，获取时间 +i2c,请开启i2c的dma和中断 +pwm,用于选择那些勇于输出pwm + diff --git a/bsp/gpio.c b/bsp/gpio.c new file mode 100644 index 0000000..5001f39 --- /dev/null +++ b/bsp/gpio.c @@ -0,0 +1,82 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/gpio.h" + +#include +#include + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + uint16_t pin; + GPIO_TypeDef *gpio; +} BSP_GPIO_MAP_t; + +/* 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 -------------------------------------------------------- */ +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/bsp/gpio_exti.h b/bsp/gpio.h similarity index 69% rename from bsp/gpio_exti.h rename to bsp/gpio.h index 03f431b..75027f1 100644 --- a/bsp/gpio_exti.h +++ b/bsp/gpio.h @@ -6,32 +6,30 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include +#include #include "bsp/bsp.h" /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ - -/* GPIO设备枚举，与设备对应 */ typedef enum { - BSP_GPIO_USER_KEY, - /* BSP_GPIO_XXX, */ +/* AUTO GENERATED BSP_GPIO_ENUM */ BSP_GPIO_NUM, BSP_GPIO_ERR, } BSP_GPIO_t; -/* GPIO支持的中断回调函数类型 */ -typedef enum { - BSP_GPIO_EXTI_CB, - BSP_GPIO_CB_NUM, -} BSP_GPIO_Callback_t; - /* Exported functions prototypes -------------------------------------------- */ -int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, BSP_GPIO_Callback_t type, void (*callback)(void)); +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); + #ifdef __cplusplus } #endif \ No newline at end of file diff --git a/bsp/gpio_exti.c b/bsp/gpio_exti.c deleted file mode 100644 index d260fe1..0000000 --- a/bsp/gpio_exti.c +++ /dev/null @@ -1,72 +0,0 @@ -/* Includes ----------------------------------------------------------------- */ -#include "bsp\gpio.h" - -#include -#include - -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -static void (*GPIO_Callback[BSP_GPIO_NUM][BSP_GPIO_CB_NUM])(void); - -/* Private function -------------------------------------------------------- */ -static BSP_GPIO_t GPIO_Get(uint16_t pin) { - switch (pin) { - case USER_KEY_Pin: - return BSP_GPIO_USER_KEY; - /* case XXX_Pin: - return BSP_GPIO_XXX; */ - default: - return BSP_GPIO_ERR; - } -} - -void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { - BSP_GPIO_t gpio = GPIO_Get(GPIO_Pin); - if (gpio != BSP_GPIO_ERR) { - if (GPIO_Callback[gpio][BSP_GPIO_EXTI_CB]) { - GPIO_Callback[gpio][BSP_GPIO_EXTI_CB](); - } - } -} - -/* Exported functions ------------------------------------------------------- */ -int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, BSP_GPIO_Callback_t type, void (*callback)(void)) { - if (callback == NULL || gpio >= BSP_GPIO_NUM || type >= BSP_GPIO_CB_NUM) return BSP_ERR_NULL; - - GPIO_Callback[gpio][type] = callback; - return BSP_OK; -} - -int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { - switch (gpio) { - case BSP_GPIO_USER_KEY: - HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn); - break; - - /* case BSP_GPIO_XXX: - HAL_NVIC_EnableIRQ(XXX_IRQn); - break; */ - - default: - return BSP_ERR; - } - return BSP_OK; -} - -int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { - switch (gpio) { - case BSP_GPIO_USER_KEY: - HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn); - break; - - /* case BSP_GPIO_XXX: - HAL_NVIC_DisableIRQ(XXX_IRQn); - break; */ - - default: - return BSP_ERR; - } - return BSP_OK; -} \ No newline at end of file diff --git a/bsp/i2c.c b/bsp/i2c.c index 6feb849..2885125 100644 --- a/bsp/i2c.c +++ b/bsp/i2c.c @@ -9,10 +9,7 @@ static void (*I2C_Callback[BSP_I2C_NUM][BSP_I2C_CB_NUM])(void); /* Private function -------------------------------------------------------- */ static BSP_I2C_t I2C_Get(I2C_HandleTypeDef *hi2c) { - if (hi2c->Instance == I2C1) - return BSP_I2C_EXAMPLE; - // else if (hi2c->Instance == I2CX) - // return BSP_I2C_XXX; +/* AUTO GENERATED I2C_GET */ else return BSP_I2C_ERR; } @@ -92,10 +89,7 @@ void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c) { /* Exported functions ------------------------------------------------------- */ I2C_HandleTypeDef *BSP_I2C_GetHandle(BSP_I2C_t i2c) { switch (i2c) { - case BSP_I2C_EXAMPLE: - return &hi2c1; - // case BSP_I2C_XXX: - // return &hi2cX; +/* AUTO GENERATED BSP_I2C_GET_HANDLE */ default: return NULL; } @@ -107,3 +101,76 @@ int8_t BSP_I2C_RegisterCallback(BSP_I2C_t i2c, BSP_I2C_Callback_t type, 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/bsp/i2c.h b/bsp/i2c.h index 9a34a37..ab75d48 100644 --- a/bsp/i2c.h +++ b/bsp/i2c.h @@ -6,6 +6,8 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include +#include +#include #include "bsp/bsp.h" @@ -17,8 +19,10 @@ extern "C" { /* I2C实体枚举，与设备对应 */ typedef enum { - BSP_I2C_EXAMPLE, - /* BSP_I2C_XXX,*/ +/* 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; @@ -42,6 +46,22 @@ 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); + + #ifdef __cplusplus } #endif diff --git a/bsp/led_gpio.c b/bsp/led_gpio.c deleted file mode 100644 index c34b179..0000000 --- a/bsp/led_gpio.c +++ /dev/null @@ -1,50 +0,0 @@ -/* Includes ----------------------------------------------------------------- */ -#include "bsp/led_gpio.h" -#include "bsp/bsp.h" -#include - -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ - -/* Private variables -------------------------------------------------------- */ -static uint32_t led_stats; // 使用位掩码记录每个通道的状态，最多支持32LED - -// 定义 LED 引脚和端口映射表：需要根据自己的修改，添加，或删减。 -static const BSP_LED_Config_t LED_CONFIGS[] = { - {GPIOA, GPIO_PIN_2}, // BSP_LED_1 - {GPIOA, GPIO_PIN_3}, // BSP_LED_2 - {GPIOA, GPIO_PIN_4}, // BSP_LED_3 -}; - -#define LED_CHANNEL_COUNT (sizeof(LED_CONFIGS) / sizeof(LED_CONFIGS[0])) // 通道数量 - -/* Private function --------------------------------------------------------- */ -/* Exported functions ------------------------------------------------------- */ -int8_t BSP_LED_Set(BSP_LED_Channel_t ch, BSP_LED_Status_t s) -{ - if (ch < LED_CHANNEL_COUNT) - { - GPIO_TypeDef *port = LED_CONFIGS[ch].port; - uint16_t pin = LED_CONFIGS[ch].pin; - switch (s) - { - case BSP_LED_ON: - led_stats |= (1 << ch); - HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET); // 点亮LED - break; - case BSP_LED_OFF: - led_stats &= ~(1 << ch); - HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET); // 熄灭LED - break; - case BSP_LED_TAGGLE: - led_stats ^= (1 << ch); - HAL_GPIO_TogglePin(port, pin); // 切换LED状态 - break; - default: - return BSP_ERR; - } - return BSP_OK; - } - return BSP_ERR; -} \ No newline at end of file diff --git a/bsp/led_gpio.h b/bsp/led_gpio.h deleted file mode 100644 index 2d413b9..0000000 --- a/bsp/led_gpio.h +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -/* Includes ----------------------------------------------------------------- */ -#include -#include "gpio.h" -/* Exported constants ------------------------------------------------------- */ -/* Exported macro ----------------------------------------------------------- */ -/* Exported types ----------------------------------------------------------- */ - -/* LED灯状态，设置用 */ -typedef enum -{ - BSP_LED_ON, - BSP_LED_OFF, - BSP_LED_TAGGLE, -} BSP_LED_Status_t; - -/* LED通道 */ -typedef enum -{ - BSP_LED_1, - BSP_LED_2, - BSP_LED_3, - /*BSP_LED_XXX*/ -} BSP_LED_Channel_t; - -/* LED GPIO 配置 */ -typedef struct -{ - GPIO_TypeDef *port; // GPIO 端口 (如 GPIOA, GPIOB) - uint16_t pin; // GPIO 引脚 -} BSP_LED_Config_t; - -/* Exported functions prototypes -------------------------------------------- */ - -int8_t BSP_LED_Set(BSP_LED_Channel_t ch, BSP_LED_Status_t s); \ No newline at end of file diff --git a/bsp/delay.h b/bsp/mm.c similarity index 51% rename from bsp/delay.h rename to bsp/mm.c index 1a95039..b78e3c4 100644 --- a/bsp/delay.h +++ b/bsp/mm.c @@ -1,8 +1,7 @@ /* Includes ----------------------------------------------------------------- */ -#include "bsp\delay.h" +#include "bsp\mm.h" -#include -#include +#include "FreeRTOS.h" /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ @@ -10,25 +9,6 @@ /* Private variables -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */ -int8_t BSP_Delay(uint32_t ms) { - uint32_t tick_period = 1000u / osKernelGetTickFreq(); - uint32_t ticks = ms / tick_period; +inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); } - 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; -} +inline void BSP_Free(void *pv) { vPortFree(pv); } diff --git a/bsp/buzzer_gpio.h b/bsp/mm.h similarity index 71% rename from bsp/buzzer_gpio.h rename to bsp/mm.h index 7a5f548..57bdcac 100644 --- a/bsp/buzzer_gpio.h +++ b/bsp/mm.h @@ -1,20 +1,20 @@ #pragma once +#ifdef __cplusplus +extern "C" { +#endif + /* Includes ----------------------------------------------------------------- */ +#include #include /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ - -/* 设置BUZZER状态 */ -typedef enum -{ - BSP_BUZZER_ON, - BSP_BUZZER_OFF, - BSP_BUZZER_TAGGLE, -} BSP_Buzzer_Status_t; - /* Exported functions prototypes -------------------------------------------- */ +void *BSP_Malloc(size_t size); +void BSP_Free(void *pv); -int8_t BSP_Buzzer_Set(BSP_Buzzer_Status_t s); +#ifdef __cplusplus +} +#endif diff --git a/bsp/pwm.c b/bsp/pwm.c new file mode 100644 index 0000000..34e9893 --- /dev/null +++ b/bsp/pwm.c @@ -0,0 +1,98 @@ +/* Includes ----------------------------------------------------------------- */ +#include "tim.h" +#include "bsp/pwm.h" +#include "bsp.h" + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + TIM_HandleTypeDef *tim; + uint16_t channel; +} BSP_PWM_Config_t; + +/* 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/bsp/servo_pwm.h b/bsp/pwm.h similarity index 50% rename from bsp/servo_pwm.h rename to bsp/pwm.h index aefb7f1..a1fa3c9 100644 --- a/bsp/servo_pwm.h +++ b/bsp/pwm.h @@ -7,39 +7,30 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include #include "tim.h" -#include "bsp/bsp.h" +#include "bsp.h" + /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ -typedef struct { - TIM_HandleTypeDef* htim; // 定时器句柄 - uint32_t channel; // 定时器通道 -} PWM_Channel_Config_t; - -#define PWM_RESOLUTION 1000 // ARR change begin -#define CYCLE 20 //ms - -typedef enum { - BSP_PWM_SERVO = 0, - BSP_PWM_IMU_HEAT = 1, +/* PWM通道 */ +typedef enum { +/* AUTO GENERATED BSP_PWM_ENUM */ + BSP_PWM_NUM, + BSP_PWM_ERR, } BSP_PWM_Channel_t; -const PWM_Channel_Config_t pwm_channel_config[] = { - [BSP_PWM_SERVO] = { &htim1, TIM_CHANNEL_1 }, // xxx 对应 TIMx 通道x - [BSP_PWM_IMU_HEAT] = { &htim1, TIM_CHANNEL_2 } -}; //change end - /* Exported functions prototypes -------------------------------------------- */ int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch); -int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle); +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); #ifdef __cplusplus } -#endif - - - - - +#endif \ No newline at end of file diff --git a/bsp/servo_pwm.c b/bsp/servo_pwm.c deleted file mode 100644 index 4648e7a..0000000 --- a/bsp/servo_pwm.c +++ /dev/null @@ -1,48 +0,0 @@ -/* Includes ----------------------------------------------------------------- */ -#include "servo_pwm.h" - -#include "main.h" - - -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -/* Private function -------------------------------------------------------- */ -/* Exported functions ------------------------------------------------------- */ - -int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) { - - TIM_HandleTypeDef* htim = pwm_channel_config[ch].htim; - uint32_t channel = pwm_channel_config[ch].channel; - - if(HAL_TIM_PWM_Start(htim, channel)!=HAL_OK){ - return -1; - }else return 0; -} - -int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) { - if (duty_cycle > 1.0f) return -1; - - uint16_t pulse = duty_cycle/CYCLE * PWM_RESOLUTION; - - if(__HAL_TIM_SET_COMPARE(pwm_channel_config[ch].htim, pwm_channel_config[ch].channel, pulse)!=HAL_OK){ - return -1; - }else return 0; -} - -int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch){ - - TIM_HandleTypeDef* htim = pwm_channel_config[ch].htim; - uint32_t channel = pwm_channel_config[ch].channel; - - if(HAL_TIM_PWM_Stop(htim, channel)!=HAL_OK){ - return -1; - }else return 0; - - -}; - - - - diff --git a/bsp/spi.c b/bsp/spi.c index 1e4fe86..b4701de 100644 --- a/bsp/spi.c +++ b/bsp/spi.c @@ -1,5 +1,6 @@ /* Includes ----------------------------------------------------------------- */ -#include "bsp\spi.h" +#include +#include "bsp/spi.h" /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ @@ -10,11 +11,7 @@ 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_EXAMPLE; - /* - else if (hspi->Instance == SPIX) - return BSP_SPI_XXX; - */ + return BSP_SPI_BMI088; else return BSP_SPI_ERR; } @@ -87,12 +84,8 @@ void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) { /* Exported functions ------------------------------------------------------- */ SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) { switch (spi) { - case BSP_SPI_EXAMPLE: + case BSP_SPI_BMI088: return &hspi1; - /* - case BSP_SPI_XXX: - return &hspiX; - */ default: return NULL; } @@ -104,3 +97,69 @@ int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, 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); +} diff --git a/bsp/spi.h b/bsp/spi.h index 4b55f0f..374b34a 100644 --- a/bsp/spi.h +++ b/bsp/spi.h @@ -6,6 +6,8 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include +#include +#include #include "bsp/bsp.h" @@ -17,8 +19,7 @@ extern "C" { /* SPI实体枚举，与设备对应 */ typedef enum { - BSP_SPI_EXAMPLE, - /* BSP_SPI_XXX,*/ +/* AUTO GENERATED BSP_SPI_NAME */ BSP_SPI_NUM, BSP_SPI_ERR, } BSP_SPI_t; @@ -41,6 +42,17 @@ 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); + #ifdef __cplusplus } #endif diff --git a/bsp/time.c b/bsp/time.c new file mode 100644 index 0000000..5817cea --- /dev/null +++ b/bsp/time.c @@ -0,0 +1,65 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/time.h" +#include "bsp.h" + +#include +#include "FreeRTOS.h" +#include "main.h" +#include "task.h" +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* 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"))); \ No newline at end of file diff --git a/bsp/time.h b/bsp/time.h new file mode 100644 index 0000000..5273f0f --- /dev/null +++ b/bsp/time.h @@ -0,0 +1,31 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/bsp.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* 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); + +#ifdef __cplusplus +} +#endif diff --git a/bsp/uart.c b/bsp/uart.c index 950d69b..8cde78a 100644 --- a/bsp/uart.c +++ b/bsp/uart.c @@ -1,5 +1,7 @@ /* Includes ----------------------------------------------------------------- */ -#include "bsp\uart.h" +#include + +#include "bsp/uart.h" /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ @@ -9,10 +11,7 @@ static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void); /* Private function -------------------------------------------------------- */ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) { - if (huart->Instance == USART1) - return BSP_UART_EXAMPLE; - // else if (huart->Instance == USARTX) - // return BSP_UART_XXX; +/* AUTO GENERATED UART_GET */ else return BSP_UART_ERR; } @@ -101,10 +100,7 @@ void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) { UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { switch (uart) { - case BSP_UART_EXAMPLE: - return &huart1; - // case BSP_UART_XXX: - // return &huartX; +/* AUTO GENERATED BSP_UART_GET_HANDLE */ default: return NULL; } @@ -113,6 +109,29 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { 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); + } +} \ No newline at end of file diff --git a/bsp/uart.h b/bsp/uart.h index 9c82fe3..324b1df 100644 --- a/bsp/uart.h +++ b/bsp/uart.h @@ -6,6 +6,8 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include +#include +#include #include "bsp/bsp.h" @@ -17,8 +19,7 @@ extern "C" { /* UART实体枚举，与设备对应 */ typedef enum { - BSP_UART_EXAMPLE, - /*BSP_UART_XXX*/ +/* AUTO GENERATED BSP_UART_NAME */ BSP_UART_NUM, BSP_UART_ERR, } BSP_UART_t; @@ -39,10 +40,14 @@ typedef enum { } BSP_UART_Callback_t; /* Exported functions prototypes -------------------------------------------- */ + UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart); 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); + #ifdef __cplusplus } #endif diff --git a/component/FreeRTOS_CLI.c b/component/FreeRTOS_CLI.c new file mode 100644 index 0000000..32421de --- /dev/null +++ b/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/component/FreeRTOS_CLI.h b/component/FreeRTOS_CLI.h new file mode 100644 index 0000000..e6d8266 --- /dev/null +++ b/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/component/QuaternionEKF.c b/component/QuaternionEKF.c deleted file mode 100644 index 6c69918..0000000 --- a/component/QuaternionEKF.c +++ /dev/null @@ -1,492 +0,0 @@ -/** - ****************************************************************************** - * @file QuaternionEKF.c - * @author Wang Hongxi - * @version V1.2.0 - * @date 2022/3/8 - * @brief attitude update with gyro bias estimate and chi-square test - ****************************************************************************** - * @attention - * 1st order LPF transfer function: - * 1 - * ——————— - * as + 1 - ****************************************************************************** - */ -#include "QuaternionEKF.h" - -QEKF_INS_t QEKF_INS; - -const float IMU_QuaternionEKF_F[36] = {1, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1}; -float IMU_QuaternionEKF_P[36] = {100000, 0.1, 0.1, 0.1, 0.1, 0.1, - 0.1, 100000, 0.1, 0.1, 0.1, 0.1, - 0.1, 0.1, 100000, 0.1, 0.1, 0.1, - 0.1, 0.1, 0.1, 100000, 0.1, 0.1, - 0.1, 0.1, 0.1, 0.1, 100, 0.1, - 0.1, 0.1, 0.1, 0.1, 0.1, 100}; -float IMU_QuaternionEKF_K[18]; -float IMU_QuaternionEKF_H[18]; - -static float invSqrt(float x); -static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf); -static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf); -static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf); -static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf); - -/** - * @brief Quaternion EKF initialization and some reference value - * @param[in] process_noise1 quaternion process noise 10 - * @param[in] process_noise2 gyro bias process noise 0.001 - * @param[in] measure_noise accel measure noise 1000000 - * @param[in] lambda fading coefficient 0.9996 - * @param[in] lpf lowpass filter coefficient 0 - */ -void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf) -{ - QEKF_INS.Initialized = 1; - QEKF_INS.Q1 = process_noise1; - QEKF_INS.Q2 = process_noise2; - QEKF_INS.R = measure_noise; - QEKF_INS.ChiSquareTestThreshold = 1e-8; - QEKF_INS.ConvergeFlag = 0; - QEKF_INS.ErrorCount = 0; - QEKF_INS.UpdateCount = 0; - if (lambda > 1) - { - lambda = 1; - } - QEKF_INS.lambda = lambda; - QEKF_INS.accLPFcoef = lpf; - - // 初始化矩阵维度信息 - Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 6, 0, 3); - Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data); - - // 姿态初始化 - QEKF_INS.IMU_QuaternionEKF.xhat_data[0] = 1; - QEKF_INS.IMU_QuaternionEKF.xhat_data[1] = 0; - QEKF_INS.IMU_QuaternionEKF.xhat_data[2] = 0; - QEKF_INS.IMU_QuaternionEKF.xhat_data[3] = 0; - - // 自定义函数初始化,用于扩展或增加kf的基础功能 - QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe; - QEKF_INS.IMU_QuaternionEKF.User_Func1_f = IMU_QuaternionEKF_F_Linearization_P_Fading; - QEKF_INS.IMU_QuaternionEKF.User_Func2_f = IMU_QuaternionEKF_SetH; - QEKF_INS.IMU_QuaternionEKF.User_Func3_f = IMU_QuaternionEKF_xhatUpdate; - - // 设定标志位,用自定函数替换kf标准步骤中的SetK(计算增益)以及xhatupdate(后验估计/融合) - QEKF_INS.IMU_QuaternionEKF.SkipEq3 = TRUE; - QEKF_INS.IMU_QuaternionEKF.SkipEq4 = TRUE; - - memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F)); - memcpy(QEKF_INS.IMU_QuaternionEKF.P_data, IMU_QuaternionEKF_P, sizeof(IMU_QuaternionEKF_P)); -} - -/** - * @brief Quaternion EKF update - * @param[in] gyro x y z in rad/s - * @param[in] accel x y z in m/s² - * @param[in] update period in s - */ -void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt) -{ - // 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵 - static float halfgxdt, halfgydt, halfgzdt; - static float accelInvNorm; - if (!QEKF_INS.Initialized) - { - IMU_QuaternionEKF_Init(10, 0.001, 1000000 * 10, 0.9996 * 0 + 1, 0); - } - - /* F, number with * represent vals to be set - 0 1* 2* 3* 4 5 - 6* 7 8* 9* 10 11 - 12* 13* 14 15* 16 17 - 18* 19* 20* 21 22 23 - 24 25 26 27 28 29 - 30 31 32 33 34 35 - */ - QEKF_INS.dt = dt; - - QEKF_INS.Gyro[0] = gx - QEKF_INS.GyroBias[0]; - QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1]; - QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2]; - - // set F - halfgxdt = 0.5f * QEKF_INS.Gyro[0] * dt; - halfgydt = 0.5f * QEKF_INS.Gyro[1] * dt; - halfgzdt = 0.5f * QEKF_INS.Gyro[2] * dt; - - // 此部分设定状态转移矩阵F的左上角部分 4x4子矩阵,即0.5(Ohm-Ohm^bias)*deltaT,右下角有一个2x2单位阵已经初始化好了 - // 注意在predict步F的右上角是4x2的零矩阵,因此每次predict的时候都会调用memcpy用单位阵覆盖前一轮线性化后的矩阵 - memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F)); - - QEKF_INS.IMU_QuaternionEKF.F_data[1] = -halfgxdt; - QEKF_INS.IMU_QuaternionEKF.F_data[2] = -halfgydt; - QEKF_INS.IMU_QuaternionEKF.F_data[3] = -halfgzdt; - - QEKF_INS.IMU_QuaternionEKF.F_data[6] = halfgxdt; - QEKF_INS.IMU_QuaternionEKF.F_data[8] = halfgzdt; - QEKF_INS.IMU_QuaternionEKF.F_data[9] = -halfgydt; - - QEKF_INS.IMU_QuaternionEKF.F_data[12] = halfgydt; - QEKF_INS.IMU_QuaternionEKF.F_data[13] = -halfgzdt; - QEKF_INS.IMU_QuaternionEKF.F_data[15] = halfgxdt; - - QEKF_INS.IMU_QuaternionEKF.F_data[18] = halfgzdt; - QEKF_INS.IMU_QuaternionEKF.F_data[19] = halfgydt; - QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt; - - // accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响 - if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波 - { - QEKF_INS.Accel[0] = ax; - QEKF_INS.Accel[1] = ay; - QEKF_INS.Accel[2] = az; - } - QEKF_INS.Accel[0] = QEKF_INS.Accel[0] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef); - QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef); - QEKF_INS.Accel[2] = QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef); - - // set z,单位化重力加速度向量 - accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] + QEKF_INS.Accel[1] * QEKF_INS.Accel[1] + QEKF_INS.Accel[2] * QEKF_INS.Accel[2]); - for (uint8_t i = 0; i < 3; i++) - { - QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值 - } - - // get body state - QEKF_INS.gyro_norm = 1.0f / invSqrt(QEKF_INS.Gyro[0] * QEKF_INS.Gyro[0] + - QEKF_INS.Gyro[1] * QEKF_INS.Gyro[1] + - QEKF_INS.Gyro[2] * QEKF_INS.Gyro[2]); - QEKF_INS.accl_norm = 1.0f / accelInvNorm; - - // 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度 - // 稍后在最后的姿态更新部分会利用StableFlag来确定 - if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f && QEKF_INS.accl_norm < 9.8f + 0.5f) - { - QEKF_INS.StableFlag = 1; - } - else - { - QEKF_INS.StableFlag = 0; - } - - // set Q R,过程噪声和观测噪声矩阵 - QEKF_INS.IMU_QuaternionEKF.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt; - QEKF_INS.IMU_QuaternionEKF.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt; - QEKF_INS.IMU_QuaternionEKF.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt; - QEKF_INS.IMU_QuaternionEKF.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt; - QEKF_INS.IMU_QuaternionEKF.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt; - QEKF_INS.IMU_QuaternionEKF.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt; - QEKF_INS.IMU_QuaternionEKF.R_data[0] = QEKF_INS.R; - QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R; - QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R; - - // 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用 - Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF); - - // 获取融合后的数据,包括四元数和xy零飘值 - QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0]; - QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1]; - QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2]; - QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3]; - QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4]; - QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5]; - QEKF_INS.GyroBias[2] = 0; // 大部分时候z轴通天,无法观测yaw的漂移 - - // 利用四元数反解欧拉角 - QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f; - QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; - QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; - - // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺) - if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f) - { - QEKF_INS.YawRoundCount--; - } - else if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast < -180.0f) - { - QEKF_INS.YawRoundCount++; - } - QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw; - QEKF_INS.YawAngleLast = QEKF_INS.Yaw; - QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用 -} - -/** - * @brief 用于更新线性化后的状态转移矩阵F右上角的一个4x2分块矩阵,稍后用于协方差矩阵P的更新; - * 并对零漂的方差进行限制,防止过度收敛并限幅防止发散 - * - * @param kf - */ -static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf) -{ - static float q0, q1, q2, q3; - static float qInvNorm; - - q0 = kf->xhatminus_data[0]; - q1 = kf->xhatminus_data[1]; - q2 = kf->xhatminus_data[2]; - q3 = kf->xhatminus_data[3]; - - // quaternion normalize - qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - for (uint8_t i = 0; i < 4; i++) - { - kf->xhatminus_data[i] *= qInvNorm; - } - /* F, number with * represent vals to be set - 0 1 2 3 4* 5* - 6 7 8 9 10* 11* - 12 13 14 15 16* 17* - 18 19 20 21 22* 23* - 24 25 26 27 28 29 - 30 31 32 33 34 35 - */ - // set F - kf->F_data[4] = q1 * QEKF_INS.dt / 2; - kf->F_data[5] = q2 * QEKF_INS.dt / 2; - - kf->F_data[10] = -q0 * QEKF_INS.dt / 2; - kf->F_data[11] = q3 * QEKF_INS.dt / 2; - - kf->F_data[16] = -q3 * QEKF_INS.dt / 2; - kf->F_data[17] = -q0 * QEKF_INS.dt / 2; - - kf->F_data[22] = q2 * QEKF_INS.dt / 2; - kf->F_data[23] = -q1 * QEKF_INS.dt / 2; - - // fading filter,防止零飘参数过度收敛 - kf->P_data[28] /= QEKF_INS.lambda; - kf->P_data[35] /= QEKF_INS.lambda; - - // 限幅,防止发散 - if (kf->P_data[28] > 10000) - { - kf->P_data[28] = 10000; - } - if (kf->P_data[35] > 10000) - { - kf->P_data[35] = 10000; - } -} - -/** - * @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H - * - * @param kf - */ -static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf) -{ - static float doubleq0, doubleq1, doubleq2, doubleq3; - /* H - 0 1 2 3 4 5 - 6 7 8 9 10 11 - 12 13 14 15 16 17 - last two cols are zero - */ - // set H - doubleq0 = 2 * kf->xhatminus_data[0]; - doubleq1 = 2 * kf->xhatminus_data[1]; - doubleq2 = 2 * kf->xhatminus_data[2]; - doubleq3 = 2 * kf->xhatminus_data[3]; - - memset(kf->H_data, 0, sizeof_float * kf->zSize * kf->xhatSize); - - kf->H_data[0] = -doubleq2; - kf->H_data[1] = doubleq3; - kf->H_data[2] = -doubleq0; - kf->H_data[3] = doubleq1; - - kf->H_data[6] = doubleq1; - kf->H_data[7] = doubleq0; - kf->H_data[8] = doubleq3; - kf->H_data[9] = doubleq2; - - kf->H_data[12] = doubleq0; - kf->H_data[13] = -doubleq1; - kf->H_data[14] = -doubleq2; - kf->H_data[15] = doubleq3; -} - -/** - * @brief 利用观测值和先验估计得到最优的后验估计 - * 加入了卡方检验以判断融合加速度的条件是否满足 - * 同时引入发散保护保证恶劣工况下的必要量测更新 - * - * @param kf - */ -static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf) -{ - static float q0, q1, q2, q3; - - kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z - kf->temp_matrix.numRows = kf->H.numRows; - kf->temp_matrix.numCols = kf->Pminus.numCols; - kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k) - kf->temp_matrix1.numRows = kf->temp_matrix.numRows; - kf->temp_matrix1.numCols = kf->HT.numCols; - kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT - kf->S.numRows = kf->R.numRows; - kf->S.numCols = kf->R.numCols; - kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R - kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R) - - q0 = kf->xhatminus_data[0]; - q1 = kf->xhatminus_data[1]; - q2 = kf->xhatminus_data[2]; - q3 = kf->xhatminus_data[3]; - - kf->temp_vector.numRows = kf->H.numRows; - kf->temp_vector.numCols = 1; - // 计算预测得到的重力加速度方向(通过姿态获取的) - kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2); - kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3); - kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k)) - - // 计算预测值和各个轴的方向余弦 - for (uint8_t i = 0; i < 3; i++) - { - QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i])); - } - - // 利用加速度计数据修正 - kf->temp_vector1.numRows = kf->z.numRows; - kf->temp_vector1.numCols = 1; - kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - h(xhat'(k)) - - // chi-square test,卡方检验 - kf->temp_matrix.numRows = kf->temp_vector1.numRows; - kf->temp_matrix.numCols = 1; - kf->MatStatus = Matrix_Multiply(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix); // temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k))) - kf->temp_vector.numRows = 1; - kf->temp_vector.numCols = kf->temp_vector1.numRows; - kf->MatStatus = Matrix_Transpose(&kf->temp_vector1, &kf->temp_vector); // temp_vector = z(k) - h(xhat'(k))' - kf->MatStatus = Matrix_Multiply(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.ChiSquare); - // rk is small,filter converged/converging - if (QEKF_INS.ChiSquare_Data[0] < 0.5f * QEKF_INS.ChiSquareTestThreshold) - { - QEKF_INS.ConvergeFlag = 1; - } - // rk is bigger than thre but once converged - if (QEKF_INS.ChiSquare_Data[0] > QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag) - { - if (QEKF_INS.StableFlag) - { - QEKF_INS.ErrorCount++; // 载体静止时仍无法通过卡方检验 - } - else - { - QEKF_INS.ErrorCount = 0; - } - - if (QEKF_INS.ErrorCount > 50) - { - // 滤波器发散 - QEKF_INS.ConvergeFlag = 0; - kf->SkipEq5 = FALSE; // step-5 is cov mat P updating - } - else - { - // 残差未通过卡方检验 仅预测 - // xhat(k) = xhat'(k) - // P(k) = P'(k) - memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize); - memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize); - kf->SkipEq5 = TRUE; // part5 is P updating - return; - } - } - else // if divergent or rk is not that big/acceptable,use adaptive gain - { - // scale adaptive,rk越小则增益越大,否则更相信预测值 - if (QEKF_INS.ChiSquare_Data[0] > 0.1f * QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag) - { - QEKF_INS.AdaptiveGainScale = (QEKF_INS.ChiSquareTestThreshold - QEKF_INS.ChiSquare_Data[0]) / (0.9f * QEKF_INS.ChiSquareTestThreshold); - } - else - { - QEKF_INS.AdaptiveGainScale = 1; - } - QEKF_INS.ErrorCount = 0; - kf->SkipEq5 = FALSE; - } - - // cal kf-gain K - kf->temp_matrix.numRows = kf->Pminus.numRows; - kf->temp_matrix.numCols = kf->HT.numCols; - kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT - kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K); - - // implement adaptive - for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++) - { - kf->K_data[i] *= QEKF_INS.AdaptiveGainScale; - } - for (uint8_t i = 4; i < 6; i++) - { - for (uint8_t j = 0; j < 3; j++) - { - kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad - } - } - - kf->temp_vector.numRows = kf->K.numRows; - kf->temp_vector.numCols = 1; - kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k)) - - // 零漂修正限幅,一般不会有过大的漂移 - if (QEKF_INS.ConvergeFlag) - { - for (uint8_t i = 4; i < 6; i++) - { - if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt) - { - kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt; - } - if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt) - { - kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt; - } - } - } - - // 不修正yaw轴数据 - kf->temp_vector.pData[3] = 0; - kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat); -} - -/** - * @brief EKF观测环节,其实就是把数据复制一下 - * - * @param kf kf类型定义 - */ -static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf) -{ - memcpy(IMU_QuaternionEKF_P, kf->P_data, sizeof(IMU_QuaternionEKF_P)); - memcpy(IMU_QuaternionEKF_K, kf->K_data, sizeof(IMU_QuaternionEKF_K)); - memcpy(IMU_QuaternionEKF_H, kf->H_data, sizeof(IMU_QuaternionEKF_H)); -} - -/** - * @brief 自定义1/sqrt(x),速度更快 - * - * @param x x - * @return float - */ -static float invSqrt(float x) -{ - float halfx = 0.5f * x; - float y = x; - long i = *(long *)&y; - i = 0x5f375a86 - (i >> 1); - y = *(float *)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} diff --git a/component/QuaternionEKF.h b/component/QuaternionEKF.h deleted file mode 100644 index f7b4d7e..0000000 --- a/component/QuaternionEKF.h +++ /dev/null @@ -1,75 +0,0 @@ -/** - ****************************************************************************** - * @file QuaternionEKF.h - * @author Wang Hongxi - * @version V1.2.0 - * @date 2022/3/8 - * @brief attitude update with gyro bias estimate and chi-square test - ****************************************************************************** - * @attention - * - ****************************************************************************** - */ -#ifndef _QUAT_EKF_H -#define _QUAT_EKF_H -#include "kalman_filter.h" - -/* boolean type definitions */ -#ifndef TRUE -#define TRUE 1 /**< boolean true */ -#endif - -#ifndef FALSE -#define FALSE 0 /**< boolean fails */ -#endif - -typedef struct -{ - uint8_t Initialized; - KalmanFilter_t IMU_QuaternionEKF; - uint8_t ConvergeFlag; - uint8_t StableFlag; - uint64_t ErrorCount; - uint64_t UpdateCount; - - float q[4]; // 四元数估计值 - float GyroBias[3]; // 陀螺仪零偏估计值 - - float Gyro[3]; - float Accel[3]; - - float OrientationCosine[3]; - - float accLPFcoef; - float gyro_norm; - float accl_norm; - float AdaptiveGainScale; - - float Roll; - float Pitch; - float Yaw; - - float YawTotalAngle; - - float Q1; // 四元数更新过程噪声 - float Q2; // 陀螺仪零偏过程噪声 - float R; // 加速度计量测噪声 - - float dt; // 姿态更新周期 - mat ChiSquare; - float ChiSquare_Data[1]; // 卡方检验检测函数 - float ChiSquareTestThreshold; // 卡方检验阈值 - float lambda; // 渐消因子 - - int16_t YawRoundCount; - - float YawAngleLast; -} QEKF_INS_t; - -extern QEKF_INS_t QEKF_INS; -extern float chiSquare; -extern float ChiSquareTestThreshold; -void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf); -void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt); - -#endif diff --git a/component/ahrs.c b/component/ahrs.c new file mode 100644 index 0000000..5886297 --- /dev/null +++ b/component/ahrs.c @@ -0,0 +1,405 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#include "ahrs.h" + +#include + +#include "user_math.h" + +#define BETA_IMU (0.033f) +#define BETA_AHRS (0.041f) + +/* 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)); } diff --git a/component/ahrs.h b/component/ahrs.h new file mode 100644 index 0000000..add8b8b --- /dev/null +++ b/component/ahrs.h @@ -0,0 +1,98 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* 欧拉角（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; + +/** + * @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); + +#ifdef __cplusplus +} +#endif diff --git a/component/capacity.c b/component/capacity.c new file mode 100644 index 0000000..a7bf801 --- /dev/null +++ b/component/capacity.c @@ -0,0 +1,58 @@ +/* + 剩余电量算法。 + 通过电压值计算剩余电量。 +*/ + +#include "capacity.h" + +/** + * @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; +} diff --git a/component/capacity.h b/component/capacity.h new file mode 100644 index 0000000..a3839d4 --- /dev/null +++ b/component/capacity.h @@ -0,0 +1,35 @@ +/* + 剩余电量算法。 + + 通过电压值计算剩余电量。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/** + * @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); + +#ifdef __cplusplus +} +#endif diff --git a/component/cmd.c b/component/cmd.c new file mode 100644 index 0000000..205869c --- /dev/null +++ b/component/cmd.c @@ -0,0 +1,375 @@ +/* + 控制命令 +*/ + +#include "cmd.h" + +#include + +/** + * @brief 行为转换为对应按键 + * + * @param cmd 主结构体 + * @param behavior 行为 + * @return uint16_t 行为对应的按键 + */ +static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd, + CMD_Behavior_t behavior) { + return cmd->param->map.key_map[behavior].key; +} + +static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd, + CMD_Behavior_t behavior) { + return cmd->param->map.key_map[behavior].active; +} + +/** + * @brief 检查按键是否按下 + * + * @param rc 遥控器数据 + * @param key 按键名称 + * @param stateful 是否为状态切换按键 + * @return true 按下 + * @return false 未按下 + */ +static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) { + /* 按下按键为鼠标左、右键 */ + if (key == CMD_L_CLICK) { + return rc->mouse.l_click; + } + if (key == CMD_R_CLICK) { + return rc->mouse.r_click; + } + return rc->key & (1u << key); +} + +static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd, + CMD_Behavior_t behavior) { + CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior); + CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior); + + bool now_key_pressed, last_key_pressed; + + /* 按下按键为鼠标左、右键 */ + if (key == CMD_L_CLICK) { + now_key_pressed = rc->mouse.l_click; + last_key_pressed = cmd->mouse_last.l_click; + } else if (key == CMD_R_CLICK) { + now_key_pressed = rc->mouse.r_click; + last_key_pressed = cmd->mouse_last.r_click; + } else { + now_key_pressed = rc->key & (1u << key); + last_key_pressed = cmd->key_last & (1u << key); + } + + switch (active) { + case CMD_ACTIVE_PRESSING: + return now_key_pressed && !last_key_pressed; + case CMD_ACTIVE_RASING: + return !now_key_pressed && last_key_pressed; + case CMD_ACTIVE_PRESSED: + return now_key_pressed; + } +} + +/** + * @brief 解析pc行为逻辑 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + * @param dt_sec 两次解析的间隔 + */ +static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + + /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */ + cmd->gimbal.delta_eulr.yaw = + (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse; + cmd->gimbal.delta_eulr.pit = + (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse; + cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f; + cmd->shoot.reverse_trig = false; + + /* 按键行为映射相关逻辑 */ + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) { + cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) { + cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) { + cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) { + cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) { + cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense; + cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) { + cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense; + cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) { + /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */ + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = true; + } else { + /* 切换至准备模式，停止射击 */ + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = false; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) { + /* 每按一次依次切换开火下一个模式 */ + cmd->shoot.fire_mode++; + cmd->shoot.fire_mode %= FIRE_MODE_NUM; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) { + /* 切换到小陀螺模式 */ + cmd->chassis.mode = CHASSIS_MODE_ROTOR; + cmd->chassis.mode_rotor = ROTOR_MODE_RAND; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) { + /* 每按一次开、关弹舱盖 */ + cmd->shoot.cover_open = !cmd->shoot.cover_open; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) { + if (cmd->ai_status == AI_STATUS_HITSWITCH) { + /* 停止ai的打符模式，停用host控制 */ + CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP); + cmd->host_overwrite = false; + cmd->ai_status = AI_STATUS_STOP; + } else if (cmd->ai_status == AI_STATUS_AUTOAIM) { + /* 自瞄模式中切换失败提醒 */ + } else { + /* ai切换至打符模式，启用host控制 */ + CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START); + cmd->ai_status = AI_STATUS_HITSWITCH; + cmd->host_overwrite = true; + } + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) { + if (cmd->ai_status == AI_STATUS_AUTOAIM) { + /* 停止ai的自瞄模式，停用host控制 */ + cmd->host_overwrite = false; + cmd->ai_status = AI_STATUS_STOP; + CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP); + } else { + /* ai切换至自瞄模式，启用host控制 */ + cmd->ai_status = AI_STATUS_AUTOAIM; + cmd->host_overwrite = true; + CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START); + } + } else { + cmd->host_overwrite = false; + // TODO: 修复逻辑 + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) { + /* 按下拨弹反转 */ + cmd->shoot.reverse_trig = true; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) { + cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35; + } + /* 保存当前按下的键位状态 */ + cmd->key_last = rc->key; + memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last)); +} + +/** + * @brief 解析rc行为逻辑 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + * @param dt_sec 两次解析的间隔 + */ +static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + switch (rc->sw_l) { + /* 左拨杆相应行为选择和解析 */ + case CMD_SW_UP: + cmd->chassis.mode = CHASSIS_MODE_BREAK; + break; + + case CMD_SW_MID: + cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL; + break; + + case CMD_SW_DOWN: + cmd->chassis.mode = CHASSIS_MODE_ROTOR; + cmd->chassis.mode_rotor = ROTOR_MODE_CW; + break; + + case CMD_SW_ERR: + cmd->chassis.mode = CHASSIS_MODE_RELAX; + break; + } + switch (rc->sw_r) { + /* 右拨杆相应行为选择和解析*/ + case CMD_SW_UP: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.mode = SHOOT_MODE_SAFE; + break; + + case CMD_SW_MID: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.fire = false; + cmd->shoot.mode = SHOOT_MODE_LOADED; + break; + + case CMD_SW_DOWN: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire_mode = FIRE_MODE_SINGLE; + cmd->shoot.fire = true; + break; + /* + case CMD_SW_UP: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_SAFE; + break; + + case CMD_SW_MID: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.fire = false; + cmd->shoot.mode = SHOOT_MODE_LOADED; + break; + + case CMD_SW_DOWN: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire_mode = FIRE_MODE_SINGLE; + cmd->shoot.fire = true; + break; + */ + case CMD_SW_ERR: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_RELAX; + } + /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ + cmd->chassis.ctrl_vec.vx = rc->ch_l_x; + cmd->chassis.ctrl_vec.vy = rc->ch_l_y; + cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc; + cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc; +} + +/** + * @brief rc失控时机器人恢复放松模式 + * + * @param cmd 主结构体 + */ +static void CMD_RcLostLogic(CMD_t *cmd) { + /* 机器人底盘、云台、射击运行模式恢复至放松模式 */ + cmd->chassis.mode = CHASSIS_MODE_RELAX; + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_RELAX; +} + +/** + * @brief 初始化命令解析 + * + * @param cmd 主结构体 + * @param param 参数 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) { + /* 指针检测 */ + if (cmd == NULL) return -1; + if (param == NULL) return -1; + + /* 设置机器人的命令参数，初始化控制方式为rc控制 */ + cmd->pc_ctrl = false; + cmd->param = param; + + return 0; +} + +/** + * @brief 检查是否启用上位机控制指令覆盖 + * + * @param cmd 主结构体 + * @return true 启用 + * @return false 不启用 + */ +inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; } + +/** + * @brief 解析命令 + * + * @param rc 遥控器数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + /* 指针检测 */ + if (rc == NULL) return -1; + if (cmd == NULL) return -1; + + /* 在pc控制和rc控制间切换 */ + if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && + CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q)) + cmd->pc_ctrl = true; + + if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && + CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E)) + cmd->pc_ctrl = false; + /*c当rc丢控时，恢复机器人至默认状态 */ + if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { + CMD_RcLostLogic(cmd); + } else { + if (cmd->pc_ctrl) { + CMD_PcLogic(rc, cmd, dt_sec); + } else { + CMD_RcLogic(rc, cmd, dt_sec); + } + } + return 0; +} + +/** + * @brief 解析上位机命令 + * + * @param host host数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) { + (void)dt_sec; /* 未使用dt_sec，消除警告 */ + /* 指针检测 */ + if (host == NULL) return -1; + if (cmd == NULL) return -1; + + /* 云台欧拉角设置为host相应的变化的欧拉角 */ + cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw; + cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit; + + /* host射击命令，设置不同的射击频率和弹丸初速度 */ + if (host->fire) { + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = true; + } else { + cmd->shoot.mode = SHOOT_MODE_SAFE; + } + return 0; +} + +/** + * @brief 添加向Referee发送的命令 + * + * @param ref 命令队列 + * @param cmd 要添加的命令 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) { + /* 指针检测 */ + if (ref == NULL) return -1; + /* 越界检测 */ + if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1; + + /* 添加机器人当前行为状态到画图的命令队列中 */ + ref->cmd[ref->counter] = cmd; + ref->counter++; + return 0; +} diff --git a/component/cmd.h b/component/cmd.h new file mode 100644 index 0000000..8561239 --- /dev/null +++ b/component/cmd.h @@ -0,0 +1,306 @@ +/* + 控制命令 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "component/ahrs.h" + +#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */ + +/* 机器人型号 */ +typedef enum { + ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */ + ROBOT_MODEL_HERO, /* 英雄机器人 */ + ROBOT_MODEL_ENGINEER, /* 工程机器人 */ + ROBOT_MODEL_DRONE, /* 空中机器人 */ + ROBOT_MODEL_SENTRY, /* 哨兵机器人 */ + ROBOT_MODEL_NUM, /* 型号数量 */ +} CMD_RobotModel_t; + +/* 底盘运行模式 */ +typedef enum { + CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ + CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ + CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ + CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */ + CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ + CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ + CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */ +} CMD_ChassisMode_t; + +/* 云台运行模式 */ +typedef enum { + GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ + GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ +} CMD_GimbalMode_t; + +/* 射击运行模式 */ +typedef enum { + SHOOT_MODE_RELAX, /* 放松模式，电机不输出 */ + SHOOT_MODE_SAFE, /* 保险模式，电机闭环控制保持静止 */ + SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */ +} CMD_ShootMode_t; + +typedef enum { + FIRE_MODE_SINGLE, /* 单发开火模式 */ + FIRE_MODE_BURST, /* N连发开火模式 */ + FIRE_MODE_CONT, /* 持续开火模式 */ + FIRE_MODE_NUM, +} CMD_FireMode_t; + +/* 小陀螺转动模式 */ +typedef enum { + ROTOR_MODE_CW, /* 顺时针转动 */ + ROTOR_MODE_CCW, /* 逆时针转动 */ + ROTOR_MODE_RAND, /* 随机转动 */ +} CMD_RotorMode_t; + +/* 底盘控制命令 */ +typedef struct { + CMD_ChassisMode_t mode; /* 底盘运行模式 */ + CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */ + MoveVector_t ctrl_vec; /* 底盘控制向量 */ +} CMD_ChassisCmd_t; + +/* 云台控制命令 */ +typedef struct { + CMD_GimbalMode_t mode; /* 云台运行模式 */ + AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */ +} CMD_GimbalCmd_t; + +/* 射击控制命令 */ +typedef struct { + CMD_ShootMode_t mode; /* 射击运行模式 */ + CMD_FireMode_t fire_mode; /* 开火模式 */ + bool fire; /*开火*/ + bool cover_open; /* 弹舱盖开关 */ + bool reverse_trig; /* 拨弹电机状态 */ +} CMD_ShootCmd_t; + +/* 拨杆位置 */ +typedef enum { + CMD_SW_ERR = 0, + CMD_SW_UP = 1, + CMD_SW_MID = 3, + CMD_SW_DOWN = 2, +} CMD_SwitchPos_t; + +/* 键盘按键值 */ +typedef enum { + CMD_KEY_W = 0, + CMD_KEY_S, + CMD_KEY_A, + CMD_KEY_D, + CMD_KEY_SHIFT, + CMD_KEY_CTRL, + CMD_KEY_Q, + CMD_KEY_E, + CMD_KEY_R, + CMD_KEY_F, + CMD_KEY_G, + CMD_KEY_Z, + CMD_KEY_X, + CMD_KEY_C, + CMD_KEY_V, + CMD_KEY_B, + CMD_L_CLICK, + CMD_R_CLICK, + CMD_KEY_NUM, +} CMD_KeyValue_t; + +/* 行为值序列 */ +typedef enum { + CMD_BEHAVIOR_FORE = 0, /* 向前 */ + CMD_BEHAVIOR_BACK, /* 向后 */ + CMD_BEHAVIOR_LEFT, /* 向左 */ + CMD_BEHAVIOR_RIGHT, /* 向右 */ + CMD_BEHAVIOR_ACCELERATE, /* 加速 */ + CMD_BEHAVIOR_DECELEBRATE, /* 减速 */ + CMD_BEHAVIOR_FIRE, /* 开火 */ + CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */ + CMD_BEHAVIOR_BUFF, /* 打符模式 */ + CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */ + CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */ + CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */ + CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */ + CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */ + CMD_BEHAVIOR_NUM, +} CMD_Behavior_t; + +typedef enum { + CMD_ACTIVE_PRESSING, /* 按下时触发 */ + CMD_ACTIVE_RASING, /* 抬起时触发 */ + CMD_ACTIVE_PRESSED, /* 按住时触发 */ +} CMD_ActiveType_t; + +typedef struct { + CMD_ActiveType_t active; + CMD_KeyValue_t key; +} CMD_KeyMapItem_t; + +/* 行为映射的对应按键数组 */ +typedef struct { + CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM]; +} CMD_KeyMap_Params_t; + +/* 位移灵敏度参数 */ +typedef struct { + float move_sense; /* 移动灵敏度 */ + float move_fast_sense; /* 加速灵敏度 */ + float move_slow_sense; /* 减速灵敏度 */ +} CMD_Move_Params_t; + +typedef struct { + uint16_t width; + uint16_t height; +} CMD_Screen_t; + +/* 命令参数 */ +typedef struct { + float sens_mouse; /* 鼠标灵敏度 */ + float sens_rc; /* 遥控器摇杆灵敏度 */ + CMD_KeyMap_Params_t map; /* 按键映射行为命令 */ + CMD_Move_Params_t move; /* 位移灵敏度参数 */ + CMD_Screen_t screen; /* 屏幕分辨率参数 */ +} CMD_Params_t; + +/* AI行为状态 */ +typedef enum { + AI_STATUS_STOP, /* 停止状态 */ + AI_STATUS_AUTOAIM, /* 自瞄状态 */ + AI_STATUS_HITSWITCH, /* 打符状态 */ + AI_STATUS_AUTOMATIC /* 自动状态 */ +} CMD_AI_Status_t; + +/* UI所用行为状态 */ +typedef enum { + CMD_UI_NOTHING, /* 当前无状态 */ + CMD_UI_AUTO_AIM_START, /* 自瞄状态开启 */ + CMD_UI_AUTO_AIM_STOP, /* 自瞄状态关闭 */ + CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */ + CMD_UI_HIT_SWITCH_STOP /* 打符状态关闭 */ +} CMD_UI_t; + +/*裁判系统发送的命令*/ +typedef struct { + CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */ + uint8_t counter; /* 命令计数 */ +} CMD_RefereeCmd_t; + +typedef struct { + bool pc_ctrl; /* 是否使用键鼠控制 */ + bool host_overwrite; /* 是否Host控制 */ + uint16_t key_last; /* 上次按键键值 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse_last; /* 鼠标值 */ + + CMD_AI_Status_t ai_status; /* AI状态 */ + + const CMD_Params_t *param; /* 命令参数 */ + + CMD_ChassisCmd_t chassis; /* 底盘控制命令 */ + CMD_GimbalCmd_t gimbal; /* 云台控制命令 */ + CMD_ShootCmd_t shoot; /* 射击控制命令 */ + CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */ +} CMD_t; + +typedef struct { + float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ + float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ + float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ + float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ + + float ch_res; /* 第五通道值 */ + + CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */ + CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse; /* 鼠标值 */ + + uint16_t key; /* 按键值 */ + + uint16_t res; /* 保留，未启用 */ +} CMD_RC_t; + +typedef struct { + AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */ + + struct { + float vx; /* x轴移动速度 */ + float vy; /* y轴移动速度 */ + float wz; /* z轴转动速度 */ + } chassis_move_vec; /* 底盘移动向量 */ + + bool fire; /* 开火状态 */ +} CMD_Host_t; + +/** + * @brief 解析行为命令 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + */ +int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param); + +/** + * @brief 检查是否启用上位机控制指令覆盖 + * + * @param cmd 主结构体 + * @return true 启用 + * @return false 不启用 + */ +bool CMD_CheckHostOverwrite(CMD_t *cmd); + +/** + * @brief 解析命令 + * + * @param rc 遥控器数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec); + +/** + * @brief 解析上位机命令 + * + * @param host host数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec); + +/** + * @brief 添加向Referee发送的命令 + * + * @param ref 命令队列 + * @param cmd 要添加的命令 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd); + +#ifdef __cplusplus +} +#endif diff --git a/component/crc16_rm.c b/component/crc16.c similarity index 99% rename from component/crc16_rm.c rename to component/crc16.c index cacdad1..2d9de2a 100644 --- a/component/crc16_rm.c +++ b/component/crc16.c @@ -1,4 +1,4 @@ -#include "crc16_rm.h" +#include "crc16.h" static const uint16_t crc16_tab[256] = { 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, diff --git a/component/crc16_rm.h b/component/crc16.h similarity index 91% rename from component/crc16_rm.h rename to component/crc16.h index 9f349ec..dc54b30 100644 --- a/component/crc16_rm.h +++ b/component/crc16.h @@ -6,6 +6,8 @@ extern "C" { #include +#include "user_math.h" + #define CRC16_INIT 0XFFFF uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc); diff --git a/component/crc8_rm.c b/component/crc8.c similarity index 97% rename from component/crc8_rm.c rename to component/crc8.c index 3b27aec..bf0bcf9 100644 --- a/component/crc8_rm.c +++ b/component/crc8.c @@ -1,8 +1,4 @@ -/* - 参考了Linux -*/ - -#include "crc8_rm.h" +#include "crc8.h" static const uint8_t crc8_tab[256] = { 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, diff --git a/component/crc8_rm.h b/component/crc8.h similarity index 92% rename from component/crc8_rm.h rename to component/crc8.h index e273e68..61b221a 100644 --- a/component/crc8_rm.h +++ b/component/crc8.h @@ -1,7 +1,3 @@ -/* - 参考了Linux -*/ - #pragma once #ifdef __cplusplus diff --git a/component/dependencies.csv b/component/dependencies.csv index c38bef7..c3f1e6d 100644 --- a/component/dependencies.csv +++ b/component/dependencies.csv @@ -1,3 +1,8 @@ +ahrs,component/user_math.h +capacity,component/user_math.h +cmd,component/ahrs +error_detect,bsp/mm pid,component/filter -pid,component/user_math -filter,component/user_math \ No newline at end of file +filter,component/ahrs +mixer,component/user_math.h +ui,component/user_math.h \ No newline at end of file diff --git a/component/describe.csv b/component/describe.csv index 903a492..2752291 100644 --- a/component/describe.csv +++ b/component/describe.csv @@ -1 +1,14 @@ -pid,好用的 \ No newline at end of file +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/component/error_detect.c b/component/error_detect.c new file mode 100644 index 0000000..b29e591 --- /dev/null +++ b/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/component/error_detect.h b/component/error_detect.h new file mode 100644 index 0000000..eb79614 --- /dev/null +++ b/component/error_detect.h @@ -0,0 +1,66 @@ +/* + 错误检测。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +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; + +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); + +#ifdef __cplusplus +} +#endif diff --git a/component/filter.c b/component/filter.c index 5905b36..5375b8e 100644 --- a/component/filter.c +++ b/component/filter.c @@ -3,7 +3,6 @@ */ #include "filter.h" -#include #include "user_math.h" diff --git a/component/filter.h b/component/filter.h index b8fe50b..c075946 100644 --- a/component/filter.h +++ b/component/filter.h @@ -8,6 +8,8 @@ extern "C" { #endif +#include "user_math.h" + /* 二阶低通滤波器 */ typedef struct { float cutoff_freq; /* 截止频率 */ diff --git a/component/limiter.c b/component/limiter.c new file mode 100644 index 0000000..71e4bf1 --- /dev/null +++ b/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/component/limiter.h b/component/limiter.h new file mode 100644 index 0000000..7c24cb5 --- /dev/null +++ b/component/limiter.h @@ -0,0 +1,55 @@ +/* + 限制器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/** + * @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/component/mixer.c b/component/mixer.c new file mode 100644 index 0000000..554b92e --- /dev/null +++ b/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/component/mixer.h b/component/mixer.h new file mode 100644 index 0000000..98de3b0 --- /dev/null +++ b/component/mixer.h @@ -0,0 +1,60 @@ +/* + 混合器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/** 四轮布局 */ +/* 前 */ +/* 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; /* 混合器主结构体 */ + +/** + * @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); + +#ifdef __cplusplus +} +#endif diff --git a/component/pid.c b/component/pid.c index 0eaa255..0a3c7d4 100644 --- a/component/pid.c +++ b/component/pid.c @@ -13,9 +13,6 @@ #include "pid.h" -#include -#include "user_math.h" - #define SIGMA 0.000001f /** diff --git a/component/pid.h b/component/pid.h index 1af4410..a93acaa 100644 --- a/component/pid.h +++ b/component/pid.h @@ -12,6 +12,7 @@ extern "C" { #include #include "filter.h" +#include "user_math.h" /* PID模式 */ typedef enum { diff --git a/component/ui.c b/component/ui.c index 4544784..c3126bd 100644 --- a/component/ui.c +++ b/component/ui.c @@ -1,7 +1,7 @@ /* UI相关命令 */ -#include "component\ui.h" +#include "component/ui.h" #include diff --git a/component/ui.h b/component/ui.h index 5580d3e..e569985 100644 --- a/component/ui.h +++ b/component/ui.h @@ -11,7 +11,7 @@ extern "C" { #include #include -#include "component\user_math.h" +#include "component/user_math.h" #define UI_DEL_OPERATION_NOTHING (0) #define UI_DEL_OPERATION_DEL (1) diff --git a/component/user_math.c b/component/user_math.c index a196e80..f3f3964 100644 --- a/component/user_math.c +++ b/component/user_math.c @@ -5,7 +5,7 @@ #include "user_math.h" #include -#include + inline float InvSqrt(float x) { //#if 0 /* Fast inverse square-root */ @@ -38,6 +38,13 @@ inline void Clip(float *origin, float min, float max) { 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 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 * 例如编码器：相差1.5PI其实等于相差-0.5PI @@ -86,3 +93,40 @@ inline void CircleAdd(float *origin, float delta, float range) { * @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(); +// } +// } diff --git a/component/user_math.h b/component/user_math.h index 5b6f2d6..fea820f 100644 --- a/component/user_math.h +++ b/component/user_math.h @@ -12,6 +12,8 @@ extern "C" { #include #include #include +#include + #define M_DEG2RAD_MULT (0.01745329251f) #define M_RAD2DEG_MULT (57.2957795131f) @@ -23,6 +25,10 @@ extern "C" { #define M_2PI 6.28318530717958647692f #endif +#ifndef __packed + #define __packed __attribute__((__packed__)) +#endif /* __packed */ + #define max(a, b) \ ({ \ __typeof__(a) _a = (a); \ @@ -54,6 +60,13 @@ void Clip(float *origin, float min, float max); float Sign(float in); +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +void ResetMoveVector(MoveVector_t *mv); + /** * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 * 例如编码器：相差1.5PI其实等于相差-0.5PI @@ -83,6 +96,16 @@ void CircleAdd(float *origin, float delta, float range); */ 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 @@ -128,3 +151,11 @@ void CircleReverse(float *origin); */ #define VERIFY(expr) ((void)(expr)) #endif + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line); diff --git a/config.csv b/config.csv new file mode 100644 index 0000000..d17e078 --- /dev/null +++ b/config.csv @@ -0,0 +1,4 @@ +bsp,can,dwt,gpio,i2c,mm,spi,uart,pwm,time +component,ahrs,capacity,cmd,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math +device,dr16,bmi088,ist8310,motor,motor_rm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,servo,buzzer,led,ws2812 +module, \ No newline at end of file diff --git a/device/.DS_Store b/device/.DS_Store index 42ce4ca..f5ef0ae 100644 Binary files a/device/.DS_Store and b/device/.DS_Store differ diff --git a/device/bmi088.c b/device/bmi088.c new file mode 100644 index 0000000..328f728 --- /dev/null +++ b/device/bmi088.c @@ -0,0 +1,368 @@ +/* + 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" + +/* 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; + +/* 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 -------------------------------------------------------- */ +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/device/bmi088.h b/device/bmi088.h new file mode 100644 index 0000000..0f33ccc --- /dev/null +++ b/device/bmi088.h @@ -0,0 +1,65 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#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; + } 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; + +/* 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); + +#ifdef __cplusplus +} +#endif diff --git a/device/bmp280_i2c.c b/device/bmp280_i2c.c deleted file mode 100644 index 1cac0f2..0000000 --- a/device/bmp280_i2c.c +++ /dev/null @@ -1,301 +0,0 @@ -#include "bmp280_i2c.h" -#include "bsp/i2c.h" - -#define I2C_Handle BSP_I2C_GetHandle(BSP_I2C_BMP280) - -/** - * @brief 读寄存器 - * @param regAdd 寄存器开始地址 - * @param pdata 存储数据的指针 - * @param size 寄存器个数 - * @retval 无 - */ -void bmp280_readReg(uint8_t regAdd, uint8_t *pdata, uint8_t size) { - HAL_I2C_Mem_Read(I2C_Handle, BMP280_I2C_ADDR << 1, regAdd, I2C_MEMADD_SIZE_8BIT, pdata, size, 1000); -} - - - -/** - * @brief 写1个寄存器 - * @param regAdd 寄存器开始地址 - * @param pdata 存储数据的指针 - * @retval 0 写入成功 - * 1 写入失败 - */ -uint8_t bmp280_writeReg(uint8_t regAdd, uint8_t *pdata) { - if (HAL_I2C_Mem_Write(I2C_Handle, BMP280_I2C_ADDR << 1, regAdd, I2C_MEMADD_SIZE_8BIT, pdata, 1, 1000) == HAL_OK) { - return 0; - } - return 1; -} - - -/** - * @brief 读取设备物理id，用于调试 - * @param 无 - * @retval 设备id - */ -uint8_t bmp280_get_id(void) { - uint8_t temp = 0; - bmp280_readReg(BMP280_ID, &temp, 1); - return temp; -} - -/** - * @brief 重启设备 - * @param 无 - * @retval 0 重启成功 - * 1 重启失败 - */ -uint8_t bmp280_reset(void) { - uint8_t temp = 0xB6; - return bmp280_writeReg(BMP280_RESET, &temp); -} - -/** - * @brief 获取设备状态 - * @param 无 - * @retval 0 空闲 - * 1 正在测量或者正在复制 - */ -uint8_t bmp280_getStatus(void) { - uint8_t temp = 0; - bmp280_readReg(BMP280_STATUS, &temp, 1); - return (temp & 0x09) ? 1 : 0; -} - - - /** - * @brief 工作模式设置推荐 - * @param mode 0 睡眠模式 - * 1 单次测量模式，测量完成后回到休眠模式 - * 2 连续测量模式 - * @retval 0 设置成功 - * 1 设置失败 - */ -uint8_t bmp280_setMode(uint8_t mode) -{ - uint8_t temp=0; - bmp280_readReg(BMP280_CTRL_MEAS,&temp,1); - switch(mode) - { - case 0: - temp&=0xFC; - break; - case 1: - temp&=0xFC; - temp|=0x01; - break; - case 2: - temp&=0xFC; - temp|=0x03; - break; - default: - return 1; - } - return bmp280_writeReg(BMP280_CTRL_MEAS,&temp); -} - - - /** - * @brief 过采样设置 - * @param mode temp&press 0 禁用 - * 1 过采样×1 - * 2 过采样×2 - * 3 过采样×4 - * .. ..... - * 5 过采样×16 - * @retval 0 设置成功 - * 1 设置失败 - */ -uint8_t bmp280_setOversampling(uint8_t osrs_p,uint8_t osrs_t) -{ - uint8_t temp=0; - bmp280_readReg(BMP280_CTRL_MEAS,&temp,1); - temp&=0xE3; - osrs_p = osrs_p<<2; - osrs_p&= 0x1C; - temp|=osrs_p; - - temp&=0x1F; - osrs_t = osrs_t<<5; - osrs_t&= 0xE0; - temp|=osrs_t; - return bmp280_writeReg(BMP280_CTRL_MEAS,&temp); -} - - - /** - * @brief 滤波器系数和采样间隔时间设置 - * @param Standbyt 0 0.5ms filter 0 关闭滤波器 - * 1 62.5ms 1 2 - * 2 125ms 2 4 - * 3 250ms 3 8 - * 4 500ms 4 16 - * 5 1000ms - * 6 2000ms - * 7 4000ms - * @retval 0 设置成功 - * 1 设置失败 - */ -uint8_t bmp280_setConfig(uint8_t Standbyt,uint8_t filter) -{ - uint8_t temp=0; - temp = Standbyt<<5; - filter&=0x07; - filter=filter<<2; - temp|=filter; - return bmp280_writeReg(BMP280_CONFIG,&temp); -} - - - /** - * @brief 获取校准系数 - * @param calib 储存系数的结构体 - * @retval 无 - */ -void bmp280_getCalibration(bmp280_calib *calib) -{ - uint8_t buf[20]; - bmp280_readReg(BMP280_DIGT,buf,6); - calib->dig_t1 =(uint16_t)(bmp280_msblsb_to_u16(buf[1], buf[0])); - calib->dig_t2 =(int16_t)(bmp280_msblsb_to_u16(buf[3], buf[2])); - calib->dig_t3 =(int16_t)(bmp280_msblsb_to_u16(buf[5], buf[4])); - bmp280_readReg(BMP280_DIGP,buf,18); - calib->dig_p1 = (uint16_t)(bmp280_msblsb_to_u16(buf[1], buf[0])); - calib->dig_p2 =(int16_t)(bmp280_msblsb_to_u16(buf[3], buf[2])); - calib->dig_p3 =(int16_t)(bmp280_msblsb_to_u16(buf[5], buf[4])); - calib->dig_p4 =(int16_t)(bmp280_msblsb_to_u16(buf[7], buf[6])); - calib->dig_p5 =(int16_t)(bmp280_msblsb_to_u16(buf[9], buf[8])); - calib->dig_p6 =(int16_t)(bmp280_msblsb_to_u16(buf[11], buf[10])); - calib->dig_p7 =(int16_t)(bmp280_msblsb_to_u16(buf[13], buf[12])); - calib->dig_p8 =(int16_t)(bmp280_msblsb_to_u16(buf[15], buf[14])); - calib->dig_p9 =(int16_t)(bmp280_msblsb_to_u16(buf[17], buf[16])); -} - - - - /** - * @brief 获取温度 - * @param calib 系数的结构体 - * *temperature 温度值指针 - * *t_fine 精细分辨率温度值指针 - * @retval 无 - */ -void bmp280_getTemperature(bmp280_calib *calib,double *temperature,int32_t *t_fine) -{ - uint8_t buf[3]; - uint32_t data_xlsb; - uint32_t data_lsb; - uint32_t data_msb; - int32_t uncomp_temperature; - double var1, var2; - - bmp280_readReg(BMP280_TEMP,buf,3); - data_msb = (int32_t)buf[0] << 12; - data_lsb = (int32_t)buf[1] << 4; - data_xlsb = (int32_t)buf[2] >> 4; - uncomp_temperature = (int32_t)(data_msb | data_lsb | data_xlsb); - - - var1 = (((double) uncomp_temperature) / 16384.0 - ((double) calib->dig_t1) / 1024.0) * - ((double) calib->dig_t2); - var2 = - ((((double) uncomp_temperature) / 131072.0 - ((double) calib->dig_t1) / 8192.0) * - (((double) uncomp_temperature) / 131072.0 - ((double) calib->dig_t1) / 8192.0)) * - ((double) calib->dig_t3); - *t_fine = (int32_t) (var1 + var2); - *temperature = (var1 + var2) / 5120.0; -} - - - /** - * @brief 获取气压 - * @param calib 系数的结构体 - * *pressure 气压值指针 - * *t_fine 精细分辨率温度值指针 - * @retval 无 - */ -void bmp280_getPressure(bmp280_calib *calib,double *pressure,int32_t *t_fine) -{ - uint8_t buf[3]; - uint32_t data_xlsb; - uint32_t data_lsb; - uint32_t data_msb; - int32_t uncomp_pressure; - double var1, var2; - - bmp280_readReg(BMP280_PRES,buf,3); - data_msb = (uint32_t)buf[0] << 12; - data_lsb = (uint32_t)buf[1] << 4; - data_xlsb = (uint32_t)buf[2] >> 4; - uncomp_pressure = (data_msb | data_lsb | data_xlsb); - - var1 = ((double) *t_fine / 2.0) - 64000.0; - var2 = var1 * var1 * ((double) calib->dig_p6) / 32768.0; - var2 = var2 + var1 * ((double) calib->dig_p5) * 2.0; - var2 = (var2 / 4.0) + (((double) calib->dig_p4) * 65536.0); - var1 = (((double)calib->dig_p3) * var1 * var1 / 524288.0 + ((double)calib->dig_p2) * var1) / - 524288.0; - var1 = (1.0 + var1 / 32768.0) * ((double) calib->dig_p1); - *pressure = 1048576.0 - (double)uncomp_pressure; - *pressure = (*pressure - (var2 / 4096.0)) * 6250.0 / var1; - var1 = ((double)calib->dig_p9) * *pressure * *pressure / 2147483648.0; - var2 = *pressure * ((double)calib->dig_p8) / 32768.0; - - *pressure = *pressure + (var1 + var2 + ((double)calib->dig_p7)) / 16.0; -} - - - /** - * @brief 初始化 - * @param *calib 系数的结构体指针 - * @retval 0 设置成功 - * 1 设置失败 - */ -uint8_t bmp280_init(bmp280_calib *calib) -{ - uint8_t rslt; - rslt = bmp280_get_id(); - if(rslt == BMP2_CHIP_ID) - { - bmp280_getCalibration(calib); - rslt = bmp280_setOversampling(5,2); - if(rslt) - { - return 1; - } - rslt = bmp280_setConfig(0,4); - if(rslt) - { - return 1; - } - rslt = bmp280_setMode(2); - if(rslt) - { - return 1; - } - } - else - { - return 1; - } - return 0; -} - - - /** - * @brief 获取最终数据 - * @param *calib 系数的结构体指针 - * @retval 无 - */ -void bmp280_getdata(bmp280_calib *calib,float *temperature,float *pressure) -{ - double temp_T,temp_P; - int32_t t_fine; - bmp280_getTemperature(calib,&temp_T,&t_fine); - bmp280_getPressure(calib,&temp_P,&t_fine); - *temperature = (float)temp_T; - *pressure = (float)temp_P; -} diff --git a/device/bmp280_i2c.h b/device/bmp280_i2c.h deleted file mode 100644 index 9ec329d..0000000 --- a/device/bmp280_i2c.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -/*底层接口定义*/ -#include "bsp/i2c.h" -#include "stdint.h" - -#define BMP280_I2C_ADDR 0x76 // BMP280 默认 I2C 地址 - -/*寄存器地址*/ -#define BMP280_ID 0xD0 // 设备ID地址 -#define BMP280_RESET 0xE0 // 设备重启 -#define BMP280_STATUS 0xF3 // 设备状态 -#define BMP280_CTRL_MEAS 0xF4 // 数据采集和模式设置 -#define BMP280_CONFIG 0xF5 // 采样速率，滤波器和接口设置 -#define BMP280_DIGT 0x88 // 温度校准系数起始位置 -#define BMP280_DIGP 0x8E // 气压校准系数起始位置 -#define BMP280_TEMP 0xFA // 温度储存起始位置 -#define BMP280_PRES 0xF7 // 气压储存起始位置 - -#define BMP2_CHIP_ID 0x58 // 设备ID地址 - -#define bmp280_msblsb_to_u16(msb, lsb) (((uint16_t)msb << 8) | ((uint16_t)lsb)) - -typedef struct { - unsigned short dig_t1; - signed short dig_t2; - signed short dig_t3; - unsigned short dig_p1; - signed short dig_p2; - signed short dig_p3; - signed short dig_p4; - signed short dig_p5; - signed short dig_p6; - signed short dig_p7; - signed short dig_p8; - signed short dig_p9; -} bmp280_calib; - -uint8_t bmp280_get_id(void); -uint8_t bmp280_reset(void); -uint8_t bmp280_getStatus(void); -uint8_t bmp280_setMode(uint8_t mode); -uint8_t bmp280_setOversampling(uint8_t osrs_p, uint8_t osrs_t); -uint8_t bmp280_setConfig(uint8_t Standbyt, uint8_t filter); -void bmp280_getCalibration(bmp280_calib *calib); -void bmp280_getTemperature(bmp280_calib *calib, double *temperature, int32_t *t_fine); -void bmp280_getPressure(bmp280_calib *calib, double *pressure, int32_t *t_fine); -uint8_t bmp280_init(bmp280_calib *calib); -void bmp280_getdata(bmp280_calib *calib, float *temperature, float *pressure); diff --git a/device/buzzer.c b/device/buzzer.c new file mode 100644 index 0000000..89f616b --- /dev/null +++ b/device/buzzer.c @@ -0,0 +1,44 @@ +#include "device/buzzer.h" + + +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; + + 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; + + 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 ; + + 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; +} diff --git a/device/buzzer.h b/device/buzzer.h new file mode 100644 index 0000000..3939e23 --- /dev/null +++ b/device/buzzer.h @@ -0,0 +1,35 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device.h" +#include "bsp/pwm.h" +#include + +/* Exported constants ------------------------------------------------------- */ + +/* Exported types ----------------------------------------------------------- */ +typedef struct { + DEVICE_Header_t header; + BSP_PWM_Channel_t channel; +} BUZZER_t; + +/* Exported functions prototypes -------------------------------------------- */ + +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); + + +int8_t BUZZER_Start(BUZZER_t *buzzer); + + +int8_t BUZZER_Stop(BUZZER_t *buzzer); + + +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); + +#ifdef __cplusplus +} +#endif diff --git a/device/config.yaml b/device/config.yaml new file mode 100644 index 0000000..21dd5aa --- /dev/null +++ b/device/config.yaml @@ -0,0 +1,219 @@ +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" + + 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"] + thread_signals: + - name: "SIGNAL_VESC_DATA_REDY" + files: + header: "motor_vesc.h" + source: "motor_vesc.c" + + motor_odrive: + name: "ODrive 电机" + description: "ODrive 电机驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + thread_signals: + - name: "SIGNAL_ODRIVE_DATA_REDY" + files: + header: "motor_odrive.h" + source: "motor_odrive.c" + + motor_rm: + name: "RM 电机" + description: "RM 电机驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + thread_signals: + - name: "SIGNAL_RM_MOTOR_DATA_REDY" + 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"] + thread_signals: + - name: "SIGNAL_DM_IMU_DATA_REDY" + 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"] + thread_signals: + - name: "SIGNAL_LK_MOTOR_DATA_REDY" + files: + header: "motor_lk.h" + source: "motor_lk.c" + + motor_lz: + name: "LZ 电机" + description: "LZ 电机驱动" + dependencies: + bsp: ["can", "time", "mm"] + component: ["user_math"] + thread_signals: + - name: "SIGNAL_LZ_MOTOR_DATA_REDY" + 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: [] + thread_signals: [] + files: + header: "vofa.h" + source: "vofa.c" \ No newline at end of file diff --git a/device/dependencies.csv b/device/dependencies.csv deleted file mode 100644 index 3f4d215..0000000 --- a/device/dependencies.csv +++ /dev/null @@ -1,5 +0,0 @@ -oled_i2c,bsp/i2c -bmp280_i2c,bsp/i2c -pc_uart,bsp/uart -key_gpio,bsp/gpio_exti -servo,bsp/servo_pwm \ No newline at end of file diff --git a/device/describe.csv b/device/describe.csv deleted file mode 100644 index 68b476a..0000000 --- a/device/describe.csv +++ /dev/null @@ -1 +0,0 @@ -servo,测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息测试消息 \ No newline at end of file diff --git a/device/device.h b/device/device.h index 308aabc..79f1140 100644 --- a/device/device.h +++ b/device/device.h @@ -4,12 +4,28 @@ extern "C" { #endif +#include +#include + #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; + #ifdef __cplusplus } #endif diff --git a/device/dm_imu.c b/device/dm_imu.c new file mode 100644 index 0000000..96c1feb --- /dev/null +++ b/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; +} + +/** + * @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/device/dm_imu.h b/device/dm_imu.h new file mode 100644 index 0000000..3965980 --- /dev/null +++ b/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/device/dr16.c b/device/dr16.c new file mode 100644 index 0000000..c2c8f95 --- /dev/null +++ b/device/dr16.c @@ -0,0 +1,85 @@ +/* + DR16接收机 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dr16.h" + +#include + +#include "bsp/uart.h" +#include "bsp/time.h" +/* Private define ----------------------------------------------------------- */ +#define DR16_CH_VALUE_MIN (364u) +#define DR16_CH_VALUE_MID (1024u) +#define DR16_CH_VALUE_MAX (1684u) + +/* 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->data.ch_r_x < DR16_CH_VALUE_MIN) || + (dr16->data.ch_r_x > DR16_CH_VALUE_MAX)) + return true; + + if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) || + (dr16->data.ch_r_y > DR16_CH_VALUE_MAX)) + return true; + + if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) || + (dr16->data.ch_l_x > DR16_CH_VALUE_MAX)) + return true; + + if ((dr16->data.ch_l_y < DR16_CH_VALUE_MIN) || + (dr16->data.ch_l_y > DR16_CH_VALUE_MAX)) + return true; + + if (dr16->data.sw_l == 0) return true; + + if (dr16->data.sw_r == 0) return true; + + return false; +} + +/* 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->data), + sizeof(dr16->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); +} diff --git a/device/dr16.h b/device/dr16.h new file mode 100644 index 0000000..6972647 --- /dev/null +++ b/device/dr16.h @@ -0,0 +1,47 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "component/user_math.h" +#include "device/device.h" + +/* 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_Data_t; + +typedef struct { + DEVICE_Header_t header; + 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); + + +#ifdef __cplusplus +} +#endif diff --git a/component/ist8310.c b/device/ist8310.c similarity index 85% rename from component/ist8310.c rename to device/ist8310.c index 1e2d5aa..cb5ae18 100644 --- a/component/ist8310.c +++ b/device/ist8310.c @@ -1,6 +1,5 @@ /* - IST8310 地磁传感器。 - + IST8310 地磁传感器。 */ /* Includes ----------------------------------------------------------------- */ @@ -10,9 +9,9 @@ #include #include -#include "bsp\delay.h" -#include "bsp\gpio.h" -#include "bsp\i2c.h" +#include "bsp/time.h" +#include "bsp/gpio.h" +#include "bsp/i2c.h" /* Private define ----------------------------------------------------------- */ #define IST8310_WAI (0x00) @@ -34,9 +33,9 @@ #define IST8310_LEN_RX_BUFF (6) /* Private macro ------------------------------------------------------------ */ #define IST8310_SET() \ - HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_SET) + BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_SET) #define IST8310_RESET() \ - HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_RESET) + BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_RESET) /* Private typedef ---------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */ @@ -47,22 +46,17 @@ static bool inited = false; /* Private function -------------------------------------------------------- */ static void IST8310_WriteSingle(uint8_t reg, uint8_t data) { - HAL_I2C_Mem_Write(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg, - I2C_MEMADD_SIZE_8BIT, &data, 1, 100); + BSP_I2C_MemWriteByte(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data); } static uint8_t IST8310_ReadSingle(uint8_t reg) { - uint8_t buf = 0; - HAL_I2C_Mem_Read(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, reg, - I2C_MEMADD_SIZE_8BIT, &buf, 1, 100); - return buf; + 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; - - HAL_I2C_Mem_Read_DMA(BSP_I2C_GetHandle(BSP_I2C_COMP), IST8310_IIC_ADDRESS, - reg, I2C_MEMADD_SIZE_8BIT, data, len); + BSP_I2C_MemRead(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data, len, true); } static void IST8310_MemRxCpltCallback(void) { @@ -83,9 +77,9 @@ int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali) { ist8310->cali = cali; IST8310_RESET(); - BSP_Delay(50); + BSP_TIME_Delay(50); IST8310_SET(); - BSP_Delay(50); + BSP_TIME_Delay(50); if (IST8310_ReadSingle(IST8310_WAI) != IST8310_CHIP_ID) return DEVICE_ERR_NO_DEV; @@ -105,7 +99,7 @@ int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali) { IST8310_WriteSingle(IST8310_AVGCNTL, 0x09); IST8310_WriteSingle(IST8310_PDCNTL, 0xC0); IST8310_WriteSingle(IST8310_CNTL1, 0x0B); - BSP_Delay(10); + BSP_TIME_Delay(10); inited = true; diff --git a/component/ist8310.h b/device/ist8310.h similarity index 93% rename from component/ist8310.h rename to device/ist8310.h index b11128e..df16cdd 100644 --- a/component/ist8310.h +++ b/device/ist8310.h @@ -9,8 +9,8 @@ extern "C" { #include #include -#include "component\ahrs.h" -#include "device\device.h" +#include "component/ahrs.h" +#include "device/device.h" /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -30,6 +30,7 @@ typedef struct { } IST8310_Cali_t; /* IST8310校准数据 */ typedef struct { + DEVICE_Header_t header; AHRS_Magn_t magn; const IST8310_Cali_t *cali; } IST8310_t; diff --git a/device/key_gpio.c b/device/key_gpio.c deleted file mode 100644 index a5ce605..0000000 --- a/device/key_gpio.c +++ /dev/null @@ -1,65 +0,0 @@ -/* Includes ----------------------------------------------------------------- */ -#include "key_gpio.h" -#include "device.h" -#include "bsp/gpio_exti.h" -#include "gpio.h" -/* Private define ----------------------------------------------------------- */ -#define DEBOUNCE_TIME_MS 20 -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -/* Private function -------------------------------------------------------- */ -/* ⲿ־λ־λ */ -volatile uint8_t key_flag = 0; // 1=£0=ɿ -volatile uint8_t key_exti = 0; -volatile uint8_t key_pressed = 0; // ȫֱ־λ -static uint32_t last_debounce_time = 0; // - -/* Private function -------------------------------------------------------- */ -static void KEY_Interrupt_Callback(void) { - // л־λ״̬ - - key_flag = !key_flag; - key_exti = 1; - -} - -/* Exported functions ------------------------------------------------------- */ -void KEY_Process(void) -{ - BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, BSP_GPIO_EXTI_CB, KEY_Interrupt_Callback); - - if(key_exti == 1) - { - uint32_t now = HAL_GetTick(); - // Ƿ񳬹ʱ - if ((now - last_debounce_time) > DEBOUNCE_TIME_MS) { - // Ч״̬谴Ϊ͵ƽ - if(key_flag == 0) - { - key_pressed = DEVICE_KEY_RELEASED; - } - if(key_flag == 1) - { - key_pressed = DEVICE_KEY_PRESSED; - } - } - else - { - HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6); - - } - last_debounce_time = now; // ʱ - key_exti = 0; - } - else - { - - } - } - -uint8_t KEY_Get_State(void) { - return key_pressed; -} - - diff --git a/device/led.c b/device/led.c new file mode 100644 index 0000000..9ae734d --- /dev/null +++ b/device/led.c @@ -0,0 +1,47 @@ +/* + led控制 +*/ +/*Includes -----------------------------------------*/ +#include "device/led.h" +#include "bsp/gpio.h" +#include "bsp/pwm.h" +#include "device.h" + + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ + +DEVICE_LED_t LED_Map={ + BSP_GPIO_BLUE, +BSP_PWM_TIM5_CH1, +}; + +int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle) +{ + switch(sign){ + case 'p': + case 'P': + 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(LED_Map.channel); + BSP_PWM_SetComp(LED_Map.channel, pulse); + break; + + case 'g': + case 'G': + BSP_GPIO_WritePin(LED_Map.gpio,value); + break; + default: + return DEVICE_ERR_INITED; // 错误：无效的控制方式 + } + return DEVICE_OK; +} + + + + + + diff --git a/device/led.h b/device/led.h new file mode 100644 index 0000000..39fa98b --- /dev/null +++ b/device/led.h @@ -0,0 +1,33 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/gpio.h" +#include "bsp/pwm.h" +#include "bsp/bsp.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + + +/* LED ??? */ +typedef struct { +BSP_GPIO_t gpio; +BSP_PWM_Channel_t channel; +} DEVICE_LED_t; + + + extern DEVICE_LED_t LED_Map; +/* Exported functions prototypes -------------------------------------------- */ + + +int8_t BSP_LED_Set(char sign,DEVICE_LED_t ch,bool value,float duty_cycle); + +#ifdef __cplusplus +} +#endif diff --git a/device/motor.c b/device/motor.c new file mode 100644 index 0000000..e71327d --- /dev/null +++ b/device/motor.c @@ -0,0 +1,37 @@ +/* + 电机通用函数 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "motor.h" + +#include + + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +/* Private function -------------------------------------------------------- */ + +/* Exported functions ------------------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + 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/device/motor.h b/device/motor.h new file mode 100644 index 0000000..6dc6a85 --- /dev/null +++ b/device/motor.h @@ -0,0 +1,52 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" + +/* 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; + +/* 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); + +#ifdef __cplusplus +} +#endif diff --git a/device/motor_lk.c b/device/motor_lk.c new file mode 100644 index 0000000..d145868 --- /dev/null +++ b/device/motor_lk.c @@ -0,0 +1,318 @@ +/* + 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" + +/* 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位编码器最大值 + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private functions -------------------------------------------------------- */ +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) { + // 调试信息：打印接收到的数据 + // printf("LK Motor ID:%d, CMD:0x%02X, Data: %02X %02X %02X %02X %02X %02X %02X %02X\n", + // motor->param.id, msg->data[0], msg->data[0], msg->data[1], msg->data[2], + // msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7]); + + // 检查命令字节是否为反馈命令 + 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: + // MF/MG电机：转矩电流值 + motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module); + break; + default: + // MS电机：功率值（范围-1000~1000） + motor->motor.feedback.torque_current = (float)raw_current_or_power; // 将功率存储在torque_current字段中 + break; + } + + // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB + motor->motor.feedback.rotor_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]); + + // 解析编码器值 (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π) + motor->motor.feedback.rotor_abs_angle = (float)raw_encoder / (float)encoder_max * M_2PI; +} + +/* 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 = 0x140 + 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 = 0x140 + 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; + + // 转矩闭环控制命令 - 将输出值转换为转矩控制值 + int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE); + + // 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID） + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = 0x140 + param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 设置转矩闭环控制命令数据 + tx_frame.data[0] = LK_CMD_TORQUE_CTRL; // 命令字节 + tx_frame.data[1] = 0x00; // NULL + tx_frame.data[2] = 0x00; // NULL + tx_frame.data[3] = 0x00; // NULL + tx_frame.data[4] = (uint8_t)(torque_control & 0xFF); // 转矩电流控制值低字节 + tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); // 转矩电流控制值高字节 + tx_frame.data[6] = 0x00; // NULL + tx_frame.data[7] = 0x00; // NULL + + 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 = 0x140 + 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 = 0x140 + 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/device/motor_lk.h b/device/motor_lk.h new file mode 100644 index 0000000..f17dde0 --- /dev/null +++ b/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/device/motor_lz.c b/device/motor_lz.c new file mode 100644 index 0000000..e9b6607 --- /dev/null +++ b/device/motor_lz.c @@ -0,0 +1,527 @@ +/* + 灵足电机驱动 +*/ +/* 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.4f) +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +/* Private macro ------------------------------------------------------------ */ + +MOTOR_LZ_MotionParam_t lz_recover_param = { + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 20.0f, + .kd = 1.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; + + // 检查是否为反馈数据帧 (通信类型2) + // 需要使用原始ID来解析，因为parsed_id已经被IdParser处理过了 + uint8_t cmd_type = (msg->original_id >> 24) & 0x1F; + if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return; + + // 解析原始ID中的数据区2 (bit23~8) + uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF; + uint8_t motor_can_id = id_data2 & 0xFF; // Bit8~15: 当前电机CAN ID + uint8_t fault_info = (id_data2 >> 8) & 0x3F; // Bit16~21: 故障信息 + uint8_t mode_state = (id_data2 >> 14) & 0x03; // Bit22~23: 模式状态 + + // 更新电机CAN ID + motor->lz_feedback.motor_can_id = motor_can_id; + + // 解析故障信息 + motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0; // bit16 + motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0; // bit17 + motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0; // bit18 + motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0; // bit19 + motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0; // bit20 + motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0; // bit21 + + // 解析模式状态 + motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state; + + // 解析数据区 + // Byte0~1: 当前角度 (高字节在前，低字节在后) + uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); + motor->lz_feedback.current_angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD); + + // Byte2~3: 当前角速度 (高字节在前，低字节在后) + uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]); + motor->lz_feedback.current_velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S); + + // Byte4~5: 当前力矩 (高字节在前，低字节在后) + uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]); + motor->lz_feedback.current_torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM); + + // Byte6~7: 当前温度 (温度*10) (高字节在前，低字节在后) + 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 = motor->lz_feedback.current_angle; + motor->motor.feedback.rotor_speed = motor->lz_feedback.current_velocity * 180.0f / M_PI * 6.0f; // 转换为RPM + motor->motor.feedback.torque_current = motor->lz_feedback.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; +} + +/** + * @brief 反初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_DeInit(void) { + // 注销ID解析器 + return BSP_CAN_UnregisterIdParser() == 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; + + // 更新运控参数 + memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t)); + + // 根据协议，bit23~8数据区2包含力矩信息 + // 力矩范围：-60Nm~60Nm 对应 0~65535 + uint16_t raw_torque = MOTOR_LZ_FloatToRaw(motion_param->torque, LZ_TORQUE_RANGE_NM); + + // 构建扩展ID - 运控模式控制指令 + // bit28~24: 0x1 (运控模式) + // bit23~8: 力矩数据 (0~65535)，协议中描述为"Byte2:力矩" + // bit7~0: 目标电机CAN_ID + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id); + + // 准备8字节数据区 + uint8_t data[8]; + + // Byte0~1: 目标角度 [0~65535] 对应 (-12.57f~12.57f rad) (高字节在前，低字节在后) + uint16_t raw_angle = MOTOR_LZ_FloatToRaw(motion_param->target_angle, LZ_ANGLE_RANGE_RAD); + data[0] = (raw_angle >> 8) & 0xFF; // 高字节 + data[1] = raw_angle & 0xFF; // 低字节 + + // Byte2~3: 目标角速度 [0~65535] 对应 (-20rad/s~20rad/s) (高字节在前，低字节在后) + uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(motion_param->target_velocity, LZ_VELOCITY_RANGE_RAD_S); + data[2] = (raw_velocity >> 8) & 0xFF; // 高字节 + data[3] = raw_velocity & 0xFF; // 低字节 + + // Byte4~5: Kp [0~65535] 对应 (0.0~5000.0) (高字节在前，低字节在后) + uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(motion_param->kp, LZ_KP_MAX); + data[4] = (raw_kp >> 8) & 0xFF; // 高字节 + data[5] = raw_kp & 0xFF; // 低字节 + + // Byte6~7: Kd [0~65535] 对应 (0.0~100.0) (高字节在前，低字节在后) + uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(motion_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_Disable(param, false); +} + +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; +} + +int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 创建运控参数，只设置力矩，其他参数为0 + MOTOR_LZ_MotionParam_t motion_param = {0}; + motion_param.torque = torque; + motion_param.target_angle = 0.0f; + motion_param.target_velocity = 0.0f; + motion_param.kp = 0.0f; + motion_param.kd = 0.0f; + + return MOTOR_LZ_MotionControl(param, &motion_param); +} + +int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 创建运控参数，设置位置和速度限制 + MOTOR_LZ_MotionParam_t motion_param = {0}; + motion_param.target_angle = target_angle; + motion_param.target_velocity = max_velocity; + motion_param.torque = 0.0f; + motion_param.kp = 100.0f; // 默认位置增益 + motion_param.kd = 5.0f; // 默认微分增益 + + return MOTOR_LZ_MotionControl(param, &motion_param); +} + +int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 创建运控参数，只设置速度 + MOTOR_LZ_MotionParam_t motion_param = {0}; + motion_param.target_angle = 0.0f; + motion_param.target_velocity = target_velocity; + motion_param.torque = 0.0f; + motion_param.kp = 0.0f; + motion_param.kd = 1.0f; // 少量阻尼 + + return MOTOR_LZ_MotionControl(param, &motion_param); +} + +int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 获取当前角度 + MOTOR_LZ_Feedback_t *feedback = MOTOR_LZ_GetFeedback(param); + if (feedback == NULL) return DEVICE_ERR_NO_DEV; + + float current_angle = feedback->current_angle; + + // 计算目标角度为0时的最短路径 + float angle_diff = -current_angle; // 目标是0，所以差值就是-current_angle + // 限制最大差值，防止过大跳变 + if (angle_diff > LZ_MAX_RECOVER_DIFF_RAD) angle_diff = LZ_MAX_RECOVER_DIFF_RAD; + if (angle_diff < -LZ_MAX_RECOVER_DIFF_RAD) angle_diff = -LZ_MAX_RECOVER_DIFF_RAD; + + float target_angle = current_angle + angle_diff; + + // 创建运控参数，设置位置和速度限制 + MOTOR_LZ_MotionParam_t motion_param = lz_recover_param; // 使用预设的恢复参数 + motion_param.target_angle = target_angle; + + return MOTOR_LZ_MotionControl(param, &motion_param); +} \ No newline at end of file diff --git a/device/motor_lz.h b/device/motor_lz.h new file mode 100644 index 0000000..8bc3bd1 --- /dev/null +++ b/device/motor_lz.h @@ -0,0 +1,220 @@ +#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 反初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_DeInit(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); + +int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/device/motor_odrive.c b/device/motor_odrive.c new file mode 100644 index 0000000..74b9319 --- /dev/null +++ b/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/device/motor_odrive.h b/device/motor_odrive.h new file mode 100644 index 0000000..ffc5282 --- /dev/null +++ b/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/device/motor_rm.c b/device/motor_rm.c new file mode 100644 index 0000000..49a867e --- /dev/null +++ b/device/motor_rm.c @@ -0,0 +1,253 @@ +/* + 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" + +/* 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) /* 电机转矩电流分辨率 */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +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 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_current = (int16_t)((msg->data[4] << 8) | msg->data[5]); + motor->motor.feedback.rotor_abs_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI; + motor->motor.feedback.rotor_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]); + int16_t lsb = MOTOR_RM_GetLSB(motor->param.module); + motor->motor.feedback.torque_current = raw_current * lsb / (float)MOTOR_CUR_RES; + motor->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); + 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; + 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/device/motor_rm.h b/device/motor_rm.h new file mode 100644 index 0000000..8dd6a76 --- /dev/null +++ b/device/motor_rm.h @@ -0,0 +1,124 @@ +#pragma once + +#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; +} MOTOR_RM_Param_t; + +/*电机实例*/ +typedef struct MOTOR_RM_t { + MOTOR_RM_Param_t param; + MOTOR_t motor; +} 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/device/motor_vesc.c b/device/motor_vesc.c new file mode 100644 index 0000000..68333f7 --- /dev/null +++ b/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/device/motor_vesc.h b/device/motor_vesc.h new file mode 100644 index 0000000..7209218 --- /dev/null +++ b/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/device/oled_i2c.c b/device/oled_i2c.c deleted file mode 100644 index f0100a0..0000000 --- a/device/oled_i2c.c +++ /dev/null @@ -1,267 +0,0 @@ -/* Includes ----------------------------------------------------------------- */ -#include "device/oled_i2c.h" -#include "bsp/i2c.h" -#include -#include -/* Private define ----------------------------------------------------------- */ -#define OLED_I2C_ADDR 0x78 // OLED I2C 地址 -#define OLED_WIDTH 128 -#define OLED_HEIGHT 64 - -/* Private variables -------------------------------------------------------- */ -static uint8_t oled_buffer[OLED_WIDTH * OLED_HEIGHT / 8]; -static struct { - uint8_t x_min; - uint8_t x_max; - uint8_t y_min; - uint8_t y_max; - uint8_t dirty; // 标志是否有脏区域 -} dirty_rect = {0, 0, 0, 0, 0}; - -/* Private function prototypes ---------------------------------------------- */ -static void OLED_WriteCommand(uint8_t cmd) { - uint8_t data[2] = {0x00, cmd}; - HAL_I2C_Master_Transmit(BSP_I2C_GetHandle(BSP_I2C_OLED), OLED_I2C_ADDR, data, 2, HAL_MAX_DELAY); -} - -static void OLED_WriteData(uint8_t *data, uint16_t size) { - uint8_t buffer[size + 1]; - buffer[0] = 0x40; - memcpy(&buffer[1], data, size); - HAL_I2C_Master_Transmit(BSP_I2C_GetHandle(BSP_I2C_OLED), OLED_I2C_ADDR, buffer, size + 1, HAL_MAX_DELAY); -} - -static void OLED_MarkDirty(uint8_t x, uint8_t y); -static void OLED_UpdateDirtyScreen(void); - -static const uint8_t oled_font[95][8] = { - {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,}, /* " ", 0 */ - {0x00,0x00,0x00,0xcf,0xcf,0x00,0x00,0x00,}, /* "!", 1 */ - {0x00,0x0c,0x06,0x00,0x0c,0x06,0x00,0x00,}, /* """, 2 */ - {0x24,0xe4,0x3c,0x27,0xe4,0x3c,0x27,0x24,}, /* "#", 3 */ - {0x00,0x20,0x46,0xf9,0x9f,0x62,0x04,0x00,}, /* "$", 4 */ - {0x06,0x09,0xc6,0x30,0x0c,0x63,0x90,0x60,}, /* "%", 5 */ - {0x00,0x00,0x6e,0x91,0xa9,0x46,0xa0,0x00,}, /* "&", 6 */ - {0x00,0x00,0x00,0x1c,0x0e,0x00,0x00,0x00,}, /* "'", 7 */ - {0x00,0x00,0x3c,0x42,0x81,0x00,0x00,0x00,}, /* "(", 8 */ - {0x00,0x00,0x00,0x81,0x42,0x3c,0x00,0x00,}, /* ")", 9 */ - {0x00,0x10,0x54,0x38,0x38,0x54,0x10,0x00,}, /* "*", 10 */ - {0x00,0x10,0x10,0xfc,0x10,0x10,0x00,0x00,}, /* "+", 11 */ - {0x00,0x00,0x00,0xc0,0x60,0x00,0x00,0x00,}, /* ",", 12 */ - {0x00,0x00,0x10,0x10,0x10,0x10,0x00,0x00,}, /* "-", 13 */ - {0x00,0x00,0x00,0x00,0xc0,0xc0,0x00,0x00,}, /* ".", 14 */ - {0x00,0x00,0x00,0xc0,0x38,0x07,0x00,0x00,}, /* "/", 15 */ - {0x00,0x00,0x7c,0x92,0x8a,0x7c,0x00,0x00,}, /* "0", 16 */ - {0x00,0x00,0x00,0x84,0xfe,0x80,0x00,0x00,}, /* "1", 17 */ - {0x00,0x00,0x8c,0xc2,0xa2,0x9c,0x00,0x00,}, /* "2", 18 */ - {0x00,0x00,0x44,0x92,0x92,0x6c,0x00,0x00,}, /* "3", 19 */ - {0x00,0x20,0x38,0x24,0xfe,0x20,0x00,0x00,}, /* "4", 20 */ - {0x00,0x00,0x5e,0x92,0x92,0x62,0x00,0x00,}, /* "5", 21 */ - {0x00,0x00,0x78,0x94,0x92,0x62,0x00,0x00,}, /* "6", 22 */ - {0x00,0x00,0x82,0x62,0x1a,0x06,0x00,0x00,}, /* "7", 23 */ - {0x00,0x00,0x6c,0x92,0x92,0x6c,0x00,0x00,}, /* "8", 24 */ - {0x00,0x00,0x8c,0x52,0x32,0x1c,0x00,0x00,}, /* "9", 25 */ - {0x00,0x00,0x00,0x6c,0x6c,0x00,0x00,0x00,}, /* ":", 26 */ - {0x00,0x00,0x80,0xec,0x6c,0x00,0x00,0x00,}, /* ";", 27 */ - {0x00,0x00,0x10,0x28,0x44,0x00,0x00,0x00,}, /* "<", 28 */ - {0x00,0x00,0x24,0x24,0x24,0x24,0x00,0x00,}, /* "=", 29 */ - {0x00,0x00,0x00,0x44,0x28,0x10,0x00,0x00,}, /* ">", 30 */ - {0x00,0x00,0x0c,0xa2,0x92,0x1c,0x00,0x00,}, /* "?", 31 */ - {0x00,0x3c,0x42,0x99,0xa5,0xa2,0x3c,0x00,}, /* "@", 32 */ - {0x00,0xe0,0x1c,0x12,0x12,0x1c,0xe0,0x00,}, /* "A", 33 */ - {0x00,0xfe,0x92,0x92,0x9c,0x90,0x60,0x00,}, /* "B", 34 */ - {0x00,0x38,0x44,0x82,0x82,0x82,0x44,0x00,}, /* "C", 35 */ - {0x00,0xfe,0x82,0x82,0x82,0x82,0x7c,0x00,}, /* "D", 36 */ - {0x00,0xfe,0x92,0x92,0x92,0x92,0x92,0x00,}, /* "E", 37 */ - {0x00,0xfe,0x12,0x12,0x12,0x12,0x02,0x00,}, /* "F", 38 */ - {0x00,0x7c,0x82,0x92,0x92,0x72,0x00,0x00,}, /* "G", 39 */ - {0x00,0xfe,0x10,0x10,0x10,0x10,0xfe,0x00,}, /* "H", 40 */ - {0x00,0x82,0x82,0xfe,0x82,0x82,0x00,0x00,}, /* "I", 41 */ - {0x00,0x82,0x82,0x7e,0x02,0x02,0x00,0x00,}, /* "J", 42 */ - {0x00,0xfe,0x10,0x28,0x44,0x82,0x00,0x00,}, /* "K", 43 */ - {0x00,0xfe,0x80,0x80,0x80,0x80,0x00,0x00,}, /* "L", 44 */ - {0xfc,0x02,0x04,0xf8,0x04,0x02,0xfc,0x00,}, /* "M", 45 */ - {0x00,0xfe,0x04,0x18,0x30,0x40,0xfe,0x00,}, /* "N", 46 */ - {0x00,0x7c,0x82,0x82,0x82,0x82,0x7c,0x00,}, /* "O", 47 */ - {0x00,0x00,0xfe,0x12,0x12,0x0c,0x00,0x00,}, /* "P", 48 */ - {0x00,0x00,0x3c,0x42,0xc2,0xbc,0x00,0x00,}, /* "Q", 49 */ - {0x00,0x00,0xfe,0x32,0x52,0x8c,0x00,0x00,}, /* "R", 50 */ - {0x00,0x00,0x4c,0x92,0x92,0x64,0x00,0x00,}, /* "S", 51 */ - {0x00,0x02,0x02,0xfe,0x02,0x02,0x00,0x00,}, /* "T", 52 */ - {0x00,0x7e,0x80,0x80,0x80,0x80,0x7e,0x00,}, /* "U", 53 */ - {0x00,0x0c,0x30,0xc0,0x30,0x0c,0x00,0x00,}, /* "V", 54 */ - {0x7c,0x80,0x80,0x78,0x80,0x80,0x7c,0x00,}, /* "W", 55 */ - {0x00,0x84,0x48,0x30,0x30,0x48,0x84,0x00,}, /* "X", 56 */ - {0x00,0x06,0x08,0xf0,0x08,0x06,0x00,0x00,}, /* "Y", 57 */ - {0x00,0x00,0xc2,0xa2,0x92,0x8e,0x00,0x00,}, /* "Z", 58 */ - {0x00,0x00,0xfe,0x82,0x82,0x82,0x00,0x00,}, /* "[", 59 */ - {0x00,0x00,0x06,0x18,0x60,0x80,0x00,0x00,}, /* "\", 60 */ - {0x00,0x00,0x82,0x82,0x82,0xfe,0x00,0x00,}, /* "]", 61 */ - {0x00,0x30,0x0c,0x02,0x0c,0x30,0x00,0x00,}, /* "^", 62 */ - {0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x00,}, /* "_", 63 */ - {0x00,0x00,0x04,0x0c,0x18,0x00,0x00,0x00,}, /* "`", 64 */ - {0x00,0x00,0x60,0x90,0x90,0xe0,0x00,0x00,}, /* "a", 65 */ - {0x00,0x00,0xf8,0xa0,0xe0,0x00,0x00,0x00,}, /* "b", 66 */ - {0x00,0x00,0x60,0x90,0x90,0x00,0x00,0x00,}, /* "c", 67 */ - {0x00,0x00,0xe0,0xa0,0xf8,0x00,0x00,0x00,}, /* "d", 68 */ - {0x00,0x00,0x70,0xa8,0xa8,0x90,0x00,0x00,}, /* "e", 69 */ - {0x00,0x00,0x10,0xf8,0x14,0x00,0x00,0x00,}, /* "f", 70 */ - {0x00,0x00,0xd8,0xa4,0x7c,0x00,0x00,0x00,}, /* "g", 71 */ - {0x00,0x00,0xf8,0x20,0xe0,0x00,0x00,0x00,}, /* "h", 72 */ - {0x00,0x00,0x00,0xe8,0x00,0x00,0x00,0x00,}, /* "i", 73 */ - {0x00,0x00,0x40,0x90,0x74,0x00,0x00,0x00,}, /* "j", 74 */ - {0x00,0x00,0xf8,0x60,0x90,0x00,0x00,0x00,}, /* "k", 75 */ - {0x00,0x00,0x78,0x80,0x80,0x00,0x00,0x00,}, /* "l", 76 */ - {0x00,0xe0,0x10,0xe0,0x10,0xe0,0x00,0x00,}, /* "m", 77 */ - {0x00,0x00,0xf0,0x10,0x10,0xe0,0x00,0x00,}, /* "n", 78 */ - {0x00,0x00,0x60,0x90,0x90,0x60,0x00,0x00,}, /* "o", 79 */ - {0x00,0x00,0xf0,0x48,0x48,0x30,0x00,0x00,}, /* "p", 80 */ - {0x00,0x00,0x30,0x48,0x48,0xf0,0x00,0x00,}, /* "q", 81 */ - {0x00,0x00,0x00,0xf0,0x20,0x10,0x00,0x00,}, /* "r", 82 */ - {0x00,0x00,0x90,0xa8,0xa8,0x48,0x00,0x00,}, /* "s", 83 */ - {0x00,0x10,0x10,0xf8,0x90,0x90,0x00,0x00,}, /* "t", 84 */ - {0x00,0x00,0x78,0x80,0x80,0xf8,0x00,0x00,}, /* "u", 85 */ - {0x00,0x18,0x60,0x80,0x60,0x18,0x00,0x00,}, /* "v", 86 */ - {0x00,0x38,0xc0,0x38,0xc0,0x38,0x00,0x00,}, /* "w", 87 */ - {0x00,0x88,0x50,0x20,0x50,0x88,0x00,0x00,}, /* "x", 88 */ - {0x00,0x8c,0x50,0x20,0x10,0x0c,0x00,0x00,}, /* "y", 89 */ - {0x00,0x88,0xc8,0xa8,0x98,0x88,0x00,0x00,}, /* "z", 90 */ - {0x00,0x00,0x10,0x7c,0x82,0x00,0x00,0x00,}, /* "{", 91 */ - {0x00,0x00,0x00,0xfe,0x00,0x00,0x00,0x00,}, /* "|", 92 */ - {0x00,0x00,0x00,0x82,0x7c,0x10,0x00,0x00,}, /* "}", 93 */ - {0x00,0x08,0x04,0x04,0x08,0x10,0x10,0x08,}, /* "~", 94 */ -}; - - -/* Exported functions ------------------------------------------------------- */ - -void OLED_Init(void) { - OLED_WriteCommand(0xAE); // 关闭显示 - OLED_WriteCommand(0x20); // 设置内存寻址模式 - OLED_WriteCommand(0x10); // 页寻址模式 - OLED_WriteCommand(0xB0); // 设置页起始地址 - OLED_WriteCommand(0xC8); // 设置COM扫描方向 - OLED_WriteCommand(0x00); // 设置低列地址 - OLED_WriteCommand(0x10); // 设置高列地址 - OLED_WriteCommand(0x40); // 设置显示起始行 - OLED_WriteCommand(0x81); // 设置对比度 - OLED_WriteCommand(0xFF); // 最大对比度 - OLED_WriteCommand(0xA1); // 设置段重映射 - OLED_WriteCommand(0xA6); // 正常显示 - OLED_WriteCommand(0xA8); // 设置多路复用比率 - OLED_WriteCommand(0x3F); // 1/64 - OLED_WriteCommand(0xA4); // 输出跟随 RAM 内容 - OLED_WriteCommand(0xD3); // 设置显示偏移 - OLED_WriteCommand(0x00); // 无偏移 - OLED_WriteCommand(0xD5); // 设置显示时钟分频比/振荡频率 - OLED_WriteCommand(0xF0); // 高频 - OLED_WriteCommand(0xD9); // 设置预充电周期 - OLED_WriteCommand(0x22); // 修复缺少分号 - OLED_WriteCommand(0xDA); // 设置COM引脚硬件配置 - OLED_WriteCommand(0x12); // 修复缺少分号 - OLED_WriteCommand(0xDB); // 设置VCOMH电压 - OLED_WriteCommand(0x20); // 修复缺少分号 - OLED_WriteCommand(0x8D); // 设置充电泵 - OLED_WriteCommand(0x14); // 修复缺少分号 - OLED_WriteCommand(0xAF); // 打开显示 -} - -void OLED_Clear(void) { - memset(oled_buffer, 0, sizeof(oled_buffer)); - dirty_rect.x_min = 0; - dirty_rect.x_max = OLED_WIDTH - 1; - dirty_rect.y_min = 0; - dirty_rect.y_max = OLED_HEIGHT - 1; - dirty_rect.dirty = 1; - OLED_UpdateScreen(); -} - -void OLED_UpdateScreen(void) { - OLED_UpdateDirtyScreen(); -} - -void OLED_DrawPixel(uint8_t x, uint8_t y, uint8_t color) { - if (x >= OLED_WIDTH || y >= OLED_HEIGHT) return; - - if (color) { - if (!(oled_buffer[x + (y / 8) * OLED_WIDTH] & (1 << (y % 8)))) { - oled_buffer[x + (y / 8) * OLED_WIDTH] |= (1 << (y % 8)); - OLED_MarkDirty(x, y); - } - } else { - if (oled_buffer[x + (y / 8) * OLED_WIDTH] & (1 << (y % 8))) { - oled_buffer[x + (y / 8) * OLED_WIDTH] &= ~(1 << (y % 8)); - OLED_MarkDirty(x, y); - } - } -} - -void OLED_DrawChar(uint8_t x, uint8_t y, char ch, uint8_t color) { - if (ch < ' ' || ch > '~') return; - - if (x >= OLED_WIDTH || y >= OLED_HEIGHT || x + 8 > OLED_WIDTH || y + 8 > OLED_HEIGHT) { - return; - } - - const uint8_t *font_data = oled_font[ch - ' ']; - - for (uint8_t i = 0; i < 8; i++) { - uint8_t column_data = font_data[i]; - for (uint8_t j = 0; j < 8; j++) { - if (column_data & (1 << j)) { - OLED_DrawPixel(x + i, y + j, color); - } else { - OLED_DrawPixel(x + i, y + j, !color); - } - } - } -} - -void OLED_DrawString(uint8_t x, uint8_t y, const char *str, uint8_t color) { - while (*str) { - OLED_DrawChar(x, y, *str, color); - x += 8; - if (x + 8 > OLED_WIDTH) { - x = 0; - y += 8; - } - if (y + 8 > OLED_HEIGHT) { - break; - } - str++; - } -} - -/* Private functions -------------------------------------------------------- */ - -static void OLED_MarkDirty(uint8_t x, uint8_t y) { - if (!dirty_rect.dirty) { - dirty_rect.x_min = x; - dirty_rect.x_max = x; - dirty_rect.y_min = y; - dirty_rect.y_max = y; - dirty_rect.dirty = 1; - } else { - if (x < dirty_rect.x_min) dirty_rect.x_min = x; - if (x > dirty_rect.x_max) dirty_rect.x_max = x; - if (y < dirty_rect.y_min) dirty_rect.y_min = y; - if (y > dirty_rect.y_max) dirty_rect.y_max = y; - } -} - -static void OLED_UpdateDirtyScreen(void) { - if (!dirty_rect.dirty) return; - - uint8_t y_start = dirty_rect.y_min / 8; - uint8_t y_end = dirty_rect.y_max / 8; - - for (uint8_t i = y_start; i <= y_end; i++) { - OLED_WriteCommand(0xB0 + i); - OLED_WriteCommand(dirty_rect.x_min & 0x0F); - OLED_WriteCommand(0x10 | (dirty_rect.x_min >> 4)); - uint8_t width = dirty_rect.x_max - dirty_rect.x_min + 1; - OLED_WriteData(&oled_buffer[dirty_rect.x_min + i * OLED_WIDTH], width); - } - - dirty_rect.dirty = 0; -} diff --git a/device/oled_i2c.h b/device/oled_i2c.h deleted file mode 100644 index 4958802..0000000 --- a/device/oled_i2c.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------------- */ -#include - -/* Exported constants ------------------------------------------------------- */ -#define OLED_COLOR_BLACK 0 -#define OLED_COLOR_WHITE 1 - -/* Exported functions prototypes -------------------------------------------- */ -void OLED_Init(void); -void OLED_Clear(void); -void OLED_UpdateScreen(void); -void OLED_DrawPixel(uint8_t x, uint8_t y, uint8_t color); -void OLED_DrawString(uint8_t x, uint8_t y, const char *str, uint8_t color); -void OLED_DrawChar(uint8_t x, uint8_t y, char ch, uint8_t color); -void OLED_ShowChinese(uint8_t x, uint8_t y, uint8_t index); - - -#ifdef __cplusplus -} -#endif diff --git a/device/pc_uart.c b/device/pc_uart.c deleted file mode 100644 index eaedcbd..0000000 --- a/device/pc_uart.c +++ /dev/null @@ -1,57 +0,0 @@ -/* Includes ----------------------------------------------------------------- */ -#include "pc_uart.h" - -#include - -#include "bsp\uart.h" -#include "device.h" - -#define UART_HANDLE BSP_UART_GetHandle(BSP_UART_PC) - -#define AI_LEN_RX_BUFF (sizeof(UART_RxData_t)) - -static bool rx_flag = false; - -static uint8_t rxbuf[AI_LEN_RX_BUFF]; - -static void UART_RxCpltCallback(void) { rx_flag = true; } - -int UART_Init(UART_t *huart) -{ - UNUSED(huart); - //注册回调函数 - HAL_UART_RegisterCallback(UART_HANDLE, BSP_UART_RX_CPLT_CB, UART_RxCpltCallback); - return DEVICE_OK -} - -int UART_StartReceive(UART_t *huart) -{ - UNUSED(huart); - HAL_UART_Receive_DMA(UART_HANDLE, rxbuf, AI_LEN_RX_BUFF); - return DEVICE_OK; -} - -bool UART_IsReceiveComplete(void) -{ - return rx_flag; -} - -int8_t UART_ParseData(UART_t *huart) -{ - - memcpy(&huart->rx_data, rxbuf, sizeof(UART_RxData_t)); -} - -void UART_PackTx(UART_t *huart, UART_TxData_t *tx_data) -{ - memcpy(tx_data, huart->tx_data, sizeof(UART_TxData_t)); -} - -int8_t UART_StartSend(UART_t *huart) -{ - if (HAL_UART_Transmit_DMA(UART_HANDLE, huart->tx_data, sizeof(UART_TxData_t)) == HAL_OK) - { - return DEVICE_OK - } - return DEVICE_ERR; -} \ No newline at end of file diff --git a/device/pc_uart.h b/device/pc_uart.h deleted file mode 100644 index 683eed4..0000000 --- a/device/pc_uart.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - UART通讯模板 -*/ - -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -/* Includes ----------------------------------------------------------------- */ -#include -#include - -/* Exported constants ------------------------------------------------------- */ -/* Exported macro ----------------------------------------------------------- */ -/* Exported types ----------------------------------------------------------- */ - -typedef struct -{ - uint8_t head; - uint8_t data; - uint8_t crc; -} UART_RxData_t; - -typedef struct -{ - uint8_t head; - uint8_t data; - uint8_t crc; -} UART_TxData_t; - -typedef struct -{ - UART_RxData_t rx_data; // Received data buffer - UART_TxData_t tx_data; // Transmit data buffer -} UART_t; - -/* Exported functions prototypes -------------------------------------------- */ - -int UART_Init(UART_t *huart); -int UART_StartReceive(UART_t *huart); -bool UART_IsReceiveComplete(void); -int8_t UART_ParseData(UART_t *huart); -void UART_PackTx(UART_t *huart, UART_TxData_t *tx_data); -int8_t UART_StartSend(UART_t *huart); -#ifdef __cplusplus -} -#endif diff --git a/device/servo.c b/device/servo.c index 12841fe..85cdcd7 100644 --- a/device/servo.c +++ b/device/servo.c @@ -1,36 +1,39 @@ -/* Includes ----------------------------------------------------------------- */ -#include "main.h" -#include "servo.h" - -#include "bsp/servo_pwm.h" - - -/* Private define ----------------------------------------------------------- */ -#define MIN_CYCLE 0.5f //change begin -#define MAX_CYCLE 2.5f -#define ANGLE_LIMIT 180 //change end -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -/* Private function -------------------------------------------------------- */ -/* Exported functions ------------------------------------------------------- */ -int serve_Init(BSP_PWM_Channel_t ch) -{ - if(BSP_PWM_Start(ch)!=0){ - return -1; - }else return 0; -} - - -int set_servo_angle(BSP_PWM_Channel_t ch,float angle) -{ - if (angle < 0.0f || angle > ANGLE_LIMIT) { - return -1; // ЧĽǶ - } - - float duty_cycle=MIN_CYCLE+(MAX_CYCLE-MIN_CYCLE)*(angle/ANGLE_LIMIT); - if(BSP_PWM_Set(ch,duty_cycle)!=0){ - return -1; - }else return 0; -} - +/* + pwmƶ +*/ + +/*Includes -----------------------------------------*/ + +#include "bsp/pwm.h" +#include "servo.h" + +#define SERVO_MIN_DUTY 0.025f +#define SERVO_MAX_DUTY 0.125f + +/** + * @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/device/servo.h b/device/servo.h index 97337df..e59ac4e 100644 --- a/device/servo.h +++ b/device/servo.h @@ -1,41 +1,52 @@ -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------------- */ -#include -#include "tim.h" -#include "bsp/bsp.h" -#include "bsp/servo_pwm.h" -/* Exported constants ------------------------------------------------------- */ -/* Exported macro ----------------------------------------------------------- */ -/* Exported types ----------------------------------------------------------- */ - - - -extern int serve_Init(BSP_PWM_Channel_t ch); -extern int set_servo_angle(BSP_PWM_Channel_t ch,float angle); - - - - - - - - - - - - - - -#ifdef __cplusplus -} -#endif - - - - - +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/pwm.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/** + * @brief + */ +typedef struct { + BSP_PWM_Channel_t pwm_ch; + float min_duty; + float max_duty; +} SERVO_t; + +/** + * @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); + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/device/vofa.c b/device/vofa.c new file mode 100644 index 0000000..bfc3b5a --- /dev/null +++ b/device/vofa.c @@ -0,0 +1,89 @@ +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include "device/vofa.h" +#include "bsp/uart.h" +/* Private define ----------------------------------------------------------- */ + +#define MAX_CHANNEL 64u // ʵͨ + +#define JUSTFLOAT_TAIL 0x7F800000 +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* 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 -------------------------------------------------------- */ + +/************************ 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/device/vofa.h b/device/vofa.h new file mode 100644 index 0000000..7eda280 --- /dev/null +++ b/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/device/ws2812.c b/device/ws2812.c new file mode 100644 index 0000000..45747b9 --- /dev/null +++ b/device/ws2812.c @@ -0,0 +1,96 @@ +/* Includes ----------------------------------------------------------------- */ +#include "ws2812.h" +#include "device.h" + +#include "bsp/pwm.h" +#include +/* 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 +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* 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 -------------------------------------------------------- */ +/* 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/device/key_gpio.h b/device/ws2812.h similarity index 52% rename from device/key_gpio.h rename to device/ws2812.h index 8e0024a..9a26cfa 100644 --- a/device/key_gpio.h +++ b/device/ws2812.h @@ -1,21 +1,19 @@ -#ifndef KEY_GPIO_H -#define KEY_GPIO_H -/* Includes ----------------------------------------------------------------- */ -#include -#include "main.h" - -/* Exported constants ------------------------------------------------------- */ -/* Exported macro ----------------------------------------------------------- */ -/* Exported types ----------------------------------------------------------- */ - -///* KEY״̬ */ -typedef enum -{ - DEVICE_KEY_RELEASED, //ͷ - DEVICE_KEY_PRESSED, // -} DEVICE_KEY_Status_t; - -void KEY_Process(void); -uint8_t KEY_Get_State(void); - -#endif +#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/module/dependencies.csv b/module/dependencies.csv deleted file mode 100644 index 3f4d215..0000000 --- a/module/dependencies.csv +++ /dev/null @@ -1,5 +0,0 @@ -oled_i2c,bsp/i2c -bmp280_i2c,bsp/i2c -pc_uart,bsp/uart -key_gpio,bsp/gpio_exti -servo,bsp/servo_pwm \ No newline at end of file diff --git a/module/describe.csv b/module/describe.csv deleted file mode 100644 index e69de29..0000000