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

使用新版代码更新

Browse Source
This commit is contained in:
Robofish 2025-09-20 00:45:52 +08:00
parent 4c6686a26a
commit 5ed34c0388
41 changed files with 1075 additions and 826 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
User/bsp/bsp.h
View File

@ -4,6 +4,14 @@
extern "C" { extern "C" {
#endif #endif
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define BSP_OK (0) #define BSP_OK (0)
#define BSP_ERR (-1) #define BSP_ERR (-1)
#define BSP_ERR_NULL (-2) #define BSP_ERR_NULL (-2)
@ -11,6 +19,10 @@ extern "C" {
#define BSP_ERR_NO_DEV (-4) #define BSP_ERR_NO_DEV (-4)
#define BSP_ERR_TIMEOUT (-5) #define BSP_ERR_TIMEOUT (-5)
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

119
User/bsp/can.c
View File

@ -2,11 +2,20 @@
#include "bsp/can.h" #include "bsp/can.h"
#include "bsp/bsp.h" #include "bsp/bsp.h"
#include <can.h> #include <can.h>
#include <cmsis_os.h> #include <cmsis_os2.h>
#include <string.h> #include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */ #define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */
#define CAN_TX_MAILBOX_NUM 3 /* CAN发送邮箱数量 */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -18,6 +27,10 @@ typedef struct BSP_CAN_QueueNode {
struct BSP_CAN_QueueNode *next; /* 指向下一个节点的指针 */ struct BSP_CAN_QueueNode *next; /* 指向下一个节点的指针 */
} BSP_CAN_QueueNode_t; } BSP_CAN_QueueNode_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static BSP_CAN_QueueNode_t *queue_list = NULL; static BSP_CAN_QueueNode_t *queue_list = NULL;
static osMutexId_t queue_mutex = NULL; static osMutexId_t queue_mutex = NULL;
@ -36,6 +49,9 @@ static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* Private functions -------------------------------------------------------- */ /* Private functions -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/** /**
* @brief 根据CAN句柄获取BSP_CAN实例 * @brief 根据CAN句柄获取BSP_CAN实例
@ -213,51 +229,57 @@ static void BSP_CAN_RxFifo1Callback(void) {
/* HAL Callback Functions --------------------------------------------------- */ /* HAL Callback Functions --------------------------------------------------- */
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan); BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) { 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](); 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) { void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan); BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) { 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](); 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) { void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan); BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) { 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](); 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) { void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan); BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) { 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](); 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) { void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan); BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) { 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](); 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) { void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan); BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) { 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](); 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) { void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
@ -353,9 +375,10 @@ int8_t BSP_CAN_Init(void) {
HAL_CAN_ConfigFilter(&hcan1, &can1_filter); HAL_CAN_ConfigFilter(&hcan1, &can1_filter);
HAL_CAN_Start(&hcan1); HAL_CAN_Start(&hcan1);
// 注册CAN1回调函数 // 自动注册CAN1接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback); BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
// 激活CAN1中断
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING |
CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断 CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断
@ -365,10 +388,11 @@ int8_t BSP_CAN_Init(void) {
HAL_CAN_ConfigFilter(&hcan2, &can1_filter); // 通过 CAN1 配置 HAL_CAN_ConfigFilter(&hcan2, &can1_filter); // 通过 CAN1 配置
HAL_CAN_Start(&hcan2); HAL_CAN_Start(&hcan2);
// 注册CAN2回调函数 // 自动注册CAN2接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback); BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING |
CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断 // 激活CAN2中断
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
inited = true; inited = true;
@ -620,40 +644,43 @@ uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
return BSP_CAN_DefaultIdParser(original_id, frame_type); return BSP_CAN_DefaultIdParser(original_id, frame_type);
} }
int8_t BSP_CAN_WaitForEmptyMailbox(BSP_CAN_t can, uint32_t timeout) { int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) {
if (!inited) { if (!inited) {
return BSP_ERR_INITED; return BSP_ERR_INITED;
} }
if (can >= BSP_CAN_NUM) { if (can >= BSP_CAN_NUM) {
return BSP_ERR; return BSP_ERR;
} }
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) { if (hcan == NULL) {
return BSP_ERR; return BSP_ERR_NULL;
} }
uint32_t start_time = HAL_GetTick(); uint32_t start_time = HAL_GetTick();
while (1) { // 如果超时时间为0，立即检查并返回
// 检查是否有空闲的发送邮箱 if (timeout == 0) {
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan);
return (free_level > 0) ? BSP_OK : BSP_ERR_TIMEOUT;
}
// 等待至少有一个邮箱空闲
while (true) {
uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan);
if (free_level > 0) {
return BSP_OK; return BSP_OK;
} }
// 检查超时 // 检查超时
if (timeout != osWaitForever) { if (timeout != BSP_CAN_TIMEOUT_FOREVER) {
uint32_t elapsed = HAL_GetTick() - start_time; uint32_t elapsed = HAL_GetTick() - start_time;
if (elapsed >= timeout) { if (elapsed >= timeout) {
return BSP_ERR_TIMEOUT; return BSP_ERR_TIMEOUT;
} }
} }
// 短暂延时，避免过度占用CPU // 短暂延时，避免占用过多CPU
osDelay(1); osDelay(1);
} }
} }
/* USER CAN FUNCTIONS BEGIN */
/* USER CAN FUNCTIONS END */

35
User/bsp/can.h
View File

@ -12,12 +12,20 @@ extern "C" {
#include <stdbool.h> #include <stdbool.h>
#include <cmsis_os.h> #include <cmsis_os.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
#define BSP_CAN_MAX_DLC 8 #define BSP_CAN_MAX_DLC 8
#define BSP_CAN_DEFAULT_QUEUE_SIZE 10 #define BSP_CAN_DEFAULT_QUEUE_SIZE 10
#define BSP_CAN_TIMEOUT_IMMEDIATE 0 #define BSP_CAN_TIMEOUT_IMMEDIATE 0
#define BSP_CAN_TIMEOUT_FOREVER osWaitForever #define BSP_CAN_TIMEOUT_FOREVER osWaitForever
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
typedef enum { typedef enum {
@ -94,6 +102,10 @@ typedef struct {
/* ID解析回调函数类型 */ /* ID解析回调函数类型 */
typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type); typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
/** /**
@ -161,6 +173,14 @@ int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame
*/ */
int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame); int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
/**
* @brief 等待CAN发送邮箱空闲
* @param can CAN 枚举
* @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout);
/** /**
* @brief 注册 CAN ID 接收队列 * @brief 注册 CAN ID 接收队列
* @param can CAN 枚举 * @param can CAN 枚举
@ -225,19 +245,10 @@ int8_t BSP_CAN_UnregisterIdParser(void);
*/ */
uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type); uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/** /* USER FUNCTION BEGIN */
* @brief 等待CAN发送邮箱为空
* @param can CAN通道 /* USER FUNCTION END */
* @param timeout 超时时间(ms)，可以使用BSP_CAN_TIMEOUT_FOREVER表示永久等待
* @retval BSP_OK 成功找到空邮箱
* @retval BSP_ERR_TIMEOUT 超时
* @retval BSP_ERR_INITED 未初始化
* @retval BSP_ERR 其他错误
*/
int8_t BSP_CAN_WaitForEmptyMailbox(BSP_CAN_t can, uint32_t timeout);
/* USER CAN FUNCTIONS BEGIN */
/* USER CAN FUNCTIONS END */
#ifdef __cplusplus #ifdef __cplusplus
} }

165
User/bsp/can_debug.c
View File

@ -1,165 +0,0 @@
/**
* @brief CAN调试工具 - 用于诊断CAN发送问题
* 使用示例：
*
* BSP_CAN_StateInfo_t state;
* if (BSP_CAN_GetStateInfo(BSP_CAN_1, &state) == BSP_OK) {
* // 打印状态信息
* printf("CAN State: %d\n", state.can_state);
* printf("Error Code: 0x%08X\n", state.error_code);
* printf("Free Mailboxes: %d\n", state.free_mailboxes);
* printf("MSR: 0x%08X\n", state.can_msr);
* printf("ESR: 0x%08X\n", state.can_esr);
* }
*/
#include "bsp/can.h"
#include <stdio.h>
void BSP_CAN_Debug_PrintState(BSP_CAN_t can) {
BSP_CAN_StateInfo_t state;
if (BSP_CAN_GetStateInfo(can, &state) != BSP_OK) {
printf("Failed to get CAN%d state\n", can + 1);
return;
}
printf("=== CAN%d State Info ===\n", can + 1);
// CAN状态
switch (state.can_state) {
case HAL_CAN_STATE_RESET:
printf("State: RESET\n");
break;
case HAL_CAN_STATE_READY:
printf("State: READY\n");
break;
case HAL_CAN_STATE_LISTENING:
printf("State: LISTENING\n");
break;
case HAL_CAN_STATE_SLEEP_PENDING:
printf("State: SLEEP_PENDING\n");
break;
case HAL_CAN_STATE_SLEEP_ACTIVE:
printf("State: SLEEP_ACTIVE\n");
break;
case HAL_CAN_STATE_ERROR:
printf("State: ERROR\n");
break;
default:
printf("State: UNKNOWN(%d)\n", state.can_state);
break;
}
// 错误状态
printf("Error Code: 0x%08X", state.error_code);
if (state.error_code != HAL_CAN_ERROR_NONE) {
printf(" (");
if (state.error_code & HAL_CAN_ERROR_EWG) printf("EWG ");
if (state.error_code & HAL_CAN_ERROR_EPV) printf("EPV ");
if (state.error_code & HAL_CAN_ERROR_BOF) printf("BOF ");
if (state.error_code & HAL_CAN_ERROR_STF) printf("STF ");
if (state.error_code & HAL_CAN_ERROR_FOR) printf("FOR ");
if (state.error_code & HAL_CAN_ERROR_ACK) printf("ACK ");
if (state.error_code & HAL_CAN_ERROR_BR) printf("BR ");
if (state.error_code & HAL_CAN_ERROR_BD) printf("BD ");
if (state.error_code & HAL_CAN_ERROR_CRC) printf("CRC ");
printf(")");
}
printf("\n");
// 邮箱和FIFO状态
printf("Free Mailboxes: %d/3\n", state.free_mailboxes);
printf("RX FIFO0 Level: %d\n", state.rx_fifo0_level);
printf("RX FIFO1 Level: %d\n", state.rx_fifo1_level);
// 寄存器状态
printf("MSR: 0x%08X\n", state.can_msr);
printf("ESR: 0x%08X", state.can_esr);
// 解析ESR寄存器
uint8_t lec = (state.can_esr >> 4) & 0x07;
uint8_t tec = (state.can_esr >> 16) & 0xFF;
uint8_t rec = (state.can_esr >> 24) & 0xFF;
printf(" (LEC:%d TEC:%d REC:%d)\n", lec, tec, rec);
// 错误计数器分析
if (tec > 96 || rec > 96) {
printf("WARNING: High error count detected!\n");
}
if (state.can_esr & (1 << 2)) { // BOFF bit
printf("WARNING: CAN is in Bus-Off state!\n");
}
if (state.can_esr & (1 << 1)) { // EPVF bit
printf("WARNING: Error Passive Flag set!\n");
}
if (state.can_esr & (1 << 0)) { // EWGF bit
printf("WARNING: Error Warning Flag set!\n");
}
printf("========================\n");
}
// 诊断发送失败的可能原因
void BSP_CAN_Debug_DiagnoseTxFailure(BSP_CAN_t can) {
BSP_CAN_StateInfo_t state;
if (BSP_CAN_GetStateInfo(can, &state) != BSP_OK) {
printf("Failed to get CAN%d state for diagnosis\n", can + 1);
return;
}
printf("=== CAN%d TX Failure Diagnosis ===\n", can + 1);
// 检查各种可能的问题
bool found_issue = false;
if (state.can_state != HAL_CAN_STATE_LISTENING && state.can_state != HAL_CAN_STATE_READY) {
printf("ISSUE: CAN not in operational state (current: %d)\n", state.can_state);
found_issue = true;
}
if (state.free_mailboxes == 0) {
printf("ISSUE: No free TX mailboxes available\n");
found_issue = true;
}
if (state.error_code != HAL_CAN_ERROR_NONE) {
printf("ISSUE: CAN has error flags (0x%08X)\n", state.error_code);
found_issue = true;
}
if (state.can_esr & (1 << 2)) { // BOFF bit
printf("ISSUE: CAN is in Bus-Off state - need restart\n");
found_issue = true;
}
uint8_t tec = (state.can_esr >> 16) & 0xFF;
uint8_t rec = (state.can_esr >> 24) & 0xFF;
if (tec > 127) {
printf("ISSUE: TX Error Counter too high (%d) - bus issues?\n", tec);
found_issue = true;
}
if (rec > 127) {
printf("ISSUE: RX Error Counter too high (%d) - bus issues?\n", rec);
found_issue = true;
}
// 检查MSR寄存器
if (!(state.can_msr & (1 << 1))) { // INAK bit - should be 0 in normal mode
printf("ISSUE: CAN may not be properly initialized\n");
found_issue = true;
}
if (!found_issue) {
printf("No obvious hardware issues found.\n");
printf("Possible causes:\n");
printf("- CAN bus not connected or terminated properly\n");
printf("- Wrong bit rate configuration\n");
printf("- Other node not acknowledging\n");
printf("- Message ID conflicts or filtering issues\n");
}
printf("===================================\n");
}

16
User/bsp/dwt.c
View File

@ -12,6 +12,18 @@
*/ */
#include "bsp/dwt.h" #include "bsp/dwt.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
DWT_Time_t SysTime; DWT_Time_t SysTime;
static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us; static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us;
static uint32_t CYCCNT_RountCount; static uint32_t CYCCNT_RountCount;
@ -19,6 +31,10 @@ static uint32_t CYCCNT_LAST;
uint64_t CYCCNT64; uint64_t CYCCNT64;
static void DWT_CNT_Update(void); static void DWT_CNT_Update(void);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
void DWT_Init(uint32_t CPU_Freq_mHz) void DWT_Init(uint32_t CPU_Freq_mHz)
{ {
/* 使能DWT外设 */ /* 使能DWT外设 */

16
User/bsp/dwt.h
View File

@ -16,6 +16,14 @@
#include "main.h" #include "main.h"
#include "stdint.h" #include "stdint.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
typedef struct typedef struct
{ {
uint32_t s; uint32_t s;
@ -23,6 +31,10 @@ typedef struct
uint16_t us; uint16_t us;
} DWT_Time_t; } DWT_Time_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
void DWT_Init(uint32_t CPU_Freq_mHz); void DWT_Init(uint32_t CPU_Freq_mHz);
float DWT_GetDeltaT(uint32_t *cnt_last); float DWT_GetDeltaT(uint32_t *cnt_last);
double DWT_GetDeltaT64(uint32_t *cnt_last); double DWT_GetDeltaT64(uint32_t *cnt_last);
@ -34,4 +46,8 @@ void DWT_SysTimeUpdate(void);
extern DWT_Time_t SysTime; extern DWT_Time_t SysTime;
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#endif /* DWT_H_ */ #endif /* DWT_H_ */

16
User/bsp/gpio.c
View File

@ -4,7 +4,15 @@
#include <gpio.h> #include <gpio.h>
#include <main.h> #include <main.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
typedef struct { typedef struct {
@ -12,6 +20,10 @@ typedef struct {
GPIO_TypeDef *gpio; GPIO_TypeDef *gpio;
} BSP_GPIO_MAP_t; } BSP_GPIO_MAP_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = { static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = {
{USER_KEY_Pin, USER_KEY_GPIO_Port}, {USER_KEY_Pin, USER_KEY_GPIO_Port},
@ -30,6 +42,10 @@ static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = {
static void (*GPIO_Callback[16])(void); static void (*GPIO_Callback[16])(void);
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
for (uint8_t i = 0; i < 16; i++) { for (uint8_t i = 0; i < 16; i++) {
if (GPIO_Pin & (1 << i)) { if (GPIO_Pin & (1 << i)) {

12
User/bsp/gpio.h
View File

@ -10,8 +10,16 @@ extern "C" {
#include "bsp/bsp.h" #include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
typedef enum { typedef enum {
BSP_GPIO_USER_KEY, BSP_GPIO_USER_KEY,
@ -40,6 +48,10 @@ int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio);
bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio); bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

16
User/bsp/mm.c
View File

@ -3,12 +3,28 @@
#include "FreeRTOS.h" #include "FreeRTOS.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); } inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); }
inline void BSP_Free(void *pv) { vPortFree(pv); } inline void BSP_Free(void *pv) { vPortFree(pv); }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

12
User/bsp/mm.h
View File

@ -8,13 +8,25 @@ extern "C" {
#include <stddef.h> #include <stddef.h>
#include <stdint.h> #include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
void *BSP_Malloc(size_t size); void *BSP_Malloc(size_t size);
void BSP_Free(void *pv); void BSP_Free(void *pv);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

14
User/bsp/pwm.c
View File

@ -3,7 +3,15 @@
#include "bsp/pwm.h" #include "bsp/pwm.h"
#include "bsp.h" #include "bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
typedef struct { typedef struct {
@ -11,6 +19,10 @@ typedef struct {
uint16_t channel; uint16_t channel;
} BSP_PWM_Config_t; } BSP_PWM_Config_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = { static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = {
{&htim8, TIM_CHANNEL_1}, {&htim8, TIM_CHANNEL_1},
@ -46,7 +58,6 @@ int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) {
if (duty_cycle < 0.0f) { if (duty_cycle < 0.0f) {
duty_cycle = 0.0f; duty_cycle = 0.0f;
} }
// 获取ARR值（周期值） // 获取ARR值（周期值）
uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim); uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim);
@ -54,6 +65,7 @@ int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) {
uint32_t ccr = (uint32_t)(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); __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr);
return BSP_OK; return BSP_OK;
} }

12
User/bsp/pwm.h
View File

@ -9,9 +9,17 @@ extern "C" {
#include "tim.h" #include "tim.h"
#include "bsp.h" #include "bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* PWM通道 */ /* PWM通道 */
typedef enum { typedef enum {
@ -42,6 +50,10 @@ 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_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length);
int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch); int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

16
User/bsp/spi.c
View File

@ -2,9 +2,21 @@
#include <spi.h> #include <spi.h>
#include "bsp/spi.h" #include "bsp/spi.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void); static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void);
@ -163,3 +175,7 @@ int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size
BSP_SPI_Transmit(spi, &reg, 1u, true); BSP_SPI_Transmit(spi, &reg, 1u, true);
return BSP_SPI_Transmit(spi, data, size, true); return BSP_SPI_Transmit(spi, data, size, true);
} }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

12
User/bsp/spi.h
View File

@ -11,8 +11,16 @@ extern "C" {
#include "bsp/bsp.h" #include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */ /* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */
@ -53,6 +61,10 @@ 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_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); int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

16
User/bsp/time.c
View File

@ -6,9 +6,21 @@
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "main.h" #include "main.h"
#include "task.h" #include "task.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
@ -63,3 +75,7 @@ int8_t BSP_TIME_Delay_us(uint32_t us) {
} }
int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms"))); int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms")));
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

12
User/bsp/time.h
View File

@ -9,8 +9,16 @@ extern "C" {
#include "bsp/bsp.h" #include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
uint32_t BSP_TIME_Get_ms(); uint32_t BSP_TIME_Get_ms();
@ -26,6 +34,10 @@ int8_t BSP_TIME_Delay_us(uint32_t us);
int8_t BSP_TIME_Delay(uint32_t ms); int8_t BSP_TIME_Delay(uint32_t ms);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

16
User/bsp/uart.c
View File

@ -3,9 +3,21 @@
#include "bsp/uart.h" #include "bsp/uart.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void); static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
@ -137,3 +149,7 @@ int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma)
return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size); return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size);
} }
} }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

15
User/bsp/uart.h
View File

@ -11,8 +11,16 @@ extern "C" {
#include "bsp/bsp.h" #include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */ /* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */
@ -42,12 +50,19 @@ typedef enum {
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart); UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart);
void BSP_UART_IRQHandler(UART_HandleTypeDef *huart);
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void)); void (*callback)(void));
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); 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); int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

12
User/component/ahrs.c
View File

@ -9,9 +9,17 @@
#include "user_math.h" #include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define BETA_IMU (0.033f) #define BETA_IMU (0.033f)
#define BETA_AHRS (0.041f) #define BETA_AHRS (0.041f)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 2 * proportional gain (Kp) */ /* 2 * proportional gain (Kp) */
static float beta = BETA_IMU; static float beta = BETA_IMU;
@ -403,3 +411,7 @@ int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
* \param eulr 被操作的数据 * \param eulr 被操作的数据
*/ */
void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); } void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

16
User/component/ahrs.h
View File

@ -11,6 +11,14 @@ extern "C" {
#include "user_math.h" #include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 欧拉角（Euler angle） */ /* 欧拉角（Euler angle） */
typedef struct { typedef struct {
float yaw; /* 偏航角（Yaw angle） */ float yaw; /* 偏航角（Yaw angle） */
@ -55,6 +63,10 @@ typedef struct {
float inv_sample_freq; /* 采样频率的的倒数 */ float inv_sample_freq; /* 采样频率的的倒数 */
} AHRS_t; } AHRS_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/** /**
* @brief 初始化姿态解算 * @brief 初始化姿态解算
* *
@ -93,6 +105,10 @@ int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
*/ */
void AHRS_ResetEulr(AHRS_Eulr_t *eulr); void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

12
User/component/cmd.c
View File

@ -6,6 +6,14 @@
#include <string.h> #include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/** /**
* @brief 行为转换为对应按键 * @brief 行为转换为对应按键
* *
@ -373,3 +381,7 @@ int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
ref->counter++; ref->counter++;
return 0; return 0;
} }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

16
User/component/cmd.h
View File

@ -11,9 +11,17 @@ extern "C" {
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include "ahrs.h" #include "component/ahrs.h"
#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */ /* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define CMD_REFEREE_MAX_NUM (3) /* Lines 16 omitted */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 机器人型号 */ /* 机器人型号 */
typedef enum { typedef enum {
@ -301,6 +309,10 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec);
*/ */
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd); int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

5
User/component/component_config.yaml
View File

@ -1,9 +1,10 @@
ahrs: ahrs:
dependencies: dependencies:
- component/filter - component/user_math.h
enabled: true enabled: true
cmd: cmd:
dependencies: [] dependencies:
- component/ahrs
enabled: true enabled: true
filter: filter:
dependencies: dependencies:

16
User/component/filter.h
View File

@ -10,6 +10,14 @@ extern "C" {
#include "user_math.h" #include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 二阶低通滤波器 */ /* 二阶低通滤波器 */
typedef struct { typedef struct {
float cutoff_freq; /* 截止频率 */ float cutoff_freq; /* 截止频率 */
@ -42,6 +50,10 @@ typedef struct {
} NotchFilter_t; } NotchFilter_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/** /**
* @brief 初始化滤波器 * @brief 初始化滤波器
* *
@ -99,6 +111,10 @@ float NotchFilter_Apply(NotchFilter_t *f, float sample);
*/ */
float NotchFilter_Reset(NotchFilter_t *f, float sample); float NotchFilter_Reset(NotchFilter_t *f, float sample);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

23
User/component/limiter.c
View File

@ -105,26 +105,3 @@ float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
else else
return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq; return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
} }
/**
* @brief 速度限制器，计算目标速度
* @param accl_limit 加速度限制
* @param speed_now 当前速度
* @param speed_target 目标速度
* @param dt 时间间隔
* @return float 计算得到的目标速度
*/
float SpeedLimit_TargetSpeed(float accl_limit, float speed_now, float speed_target, float dt){
float speed_diff = speed_target - speed_now;
float max_speed_change = accl_limit * dt;
if (fabsf(speed_diff) > max_speed_change) {
if (speed_diff > 0) {
return speed_now + max_speed_change;
} else {
return speed_now - max_speed_change;
}
} else {
return speed_target;
}
}

18
User/component/limiter.h
View File

@ -11,6 +11,14 @@ extern "C" {
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/** /**
* @brief 限制底盘功率不超过power_limit * @brief 限制底盘功率不超过power_limit
* *
@ -53,13 +61,3 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer);
*/ */
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big); float heat_increase, bool is_big);
/**
* @brief 速度限制器，计算目标速度
* @param accl_limit 加速度限制
* @param speed_now 当前速度
* @param speed_target 目标速度
* @param dt 时间间隔
* @return float 计算得到的目标速度
*/
float SpeedLimit_TargetSpeed(float accl_limit, float speed_now, float speed_target, float dt);

12
User/component/pid.h
View File

@ -14,6 +14,14 @@ extern "C" {
#include "filter.h" #include "filter.h"
#include "user_math.h" #include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* PID模式 */ /* PID模式 */
typedef enum { typedef enum {
KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */ KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
@ -90,6 +98,10 @@ int8_t PID_ResetIntegral(KPID_t *pid);
*/ */
int8_t PID_Reset(KPID_t *pid); int8_t PID_Reset(KPID_t *pid);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

8
User/component/user_math.c
View File

@ -3,8 +3,10 @@
*/ */
#include "user_math.h" #include "user_math.h"
#include <string.h> #include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
inline float InvSqrt(float x) { inline float InvSqrt(float x) {
//#if 0 //#if 0
@ -130,3 +132,7 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
// __NOP(); // __NOP();
// } // }
// } // }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

18
User/component/user_math.h
View File

@ -14,6 +14,10 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#include <stddef.h> #include <stddef.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define M_DEG2RAD_MULT (0.01745329251f) #define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f) #define M_RAD2DEG_MULT (57.2957795131f)
@ -43,6 +47,12 @@ extern "C" {
_a < _b ? _a : _b; \ _a < _b ? _a : _b; \
}) })
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 移动向量 */ /* 移动向量 */
typedef struct { typedef struct {
float vx; /* 前后平移 */ float vx; /* 前后平移 */
@ -50,6 +60,10 @@ typedef struct {
float wz; /* 转动 */ float wz; /* 转动 */
} MoveVector_t; } MoveVector_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
float InvSqrt(float x); float InvSqrt(float x);
float AbsClip(float in, float limit); float AbsClip(float in, float limit);
@ -159,3 +173,7 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
// * @param line 行号 // * @param line 行号
// */ // */
// void VerifyFailed(const char *file, uint32_t line); // void VerifyFailed(const char *file, uint32_t line);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

12
User/device/buzzer.c
View File

@ -1,5 +1,13 @@
#include "device/buzzer.h" #include "device/buzzer.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) { int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
if (buzzer == NULL) return DEVICE_ERR; if (buzzer == NULL) return DEVICE_ERR;
@ -42,3 +50,7 @@ int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
return result; return result;
} }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

16
User/device/buzzer.h
View File

@ -9,14 +9,26 @@ extern "C" {
#include "bsp/pwm.h" #include "bsp/pwm.h"
#include <stddef.h> #include <stddef.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
typedef struct { typedef struct {
DEVICE_Header_t header; DEVICE_Header_t header;
BSP_PWM_Channel_t channel; BSP_PWM_Channel_t channel;
} BUZZER_t; } BUZZER_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
@ -30,6 +42,10 @@ int8_t BUZZER_Stop(BUZZER_t *buzzer);
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

16
User/device/device.h
View File

@ -7,6 +7,14 @@ extern "C" {
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define DEVICE_OK (0) #define DEVICE_OK (0)
#define DEVICE_ERR (-1) #define DEVICE_ERR (-1)
#define DEVICE_ERR_NULL (-2) #define DEVICE_ERR_NULL (-2)
@ -26,6 +34,14 @@ typedef struct {
uint64_t last_online_time; uint64_t last_online_time;
} DEVICE_Header_t; } DEVICE_Header_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

3
User/device/device_config.yaml
View File

@ -1,5 +1,6 @@
buzzer: buzzer:
bsp_config: {} bsp_config:
BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1
enabled: true enabled: true
dm_imu: dm_imu:
bsp_config: {} bsp_config: {}

4
User/device/dm_imu.c
View File

@ -1,5 +1,5 @@
/* /*
DM_IMU数据获取 DM_IMU数据获取（CAN）
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@ -166,7 +166,7 @@ int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
.dlc = 4, .dlc = 4,
}; };
memcpy(frame.data, tx_data, 4); memcpy(frame.data, tx_data, 4);
BSP_CAN_WaitTxMailboxEmpty(imu->param.can, 1); // 等待发送邮箱空闲
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame); int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
} }

14
User/device/dr16.c
View File

@ -1,5 +1,5 @@
/* /*
DR16接收机 DR16接收机
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@ -9,11 +9,19 @@
#include "bsp/uart.h" #include "bsp/uart.h"
#include "bsp/time.h" #include "bsp/time.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define DR16_CH_VALUE_MIN (364u) #define DR16_CH_VALUE_MIN (364u)
#define DR16_CH_VALUE_MID (1024u) #define DR16_CH_VALUE_MID (1024u)
#define DR16_CH_VALUE_MAX (1684u) #define DR16_CH_VALUE_MAX (1684u)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
@ -83,3 +91,7 @@ bool DR16_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) == return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_DR16_RAW_REDY); SIGNAL_DR16_RAW_REDY);
} }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

11
User/device/dr16.h
View File

@ -10,6 +10,14 @@ extern "C" {
#include "component/user_math.h" #include "component/user_math.h"
#include "device/device.h" #include "device/device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
@ -41,6 +49,9 @@ int8_t DR16_Restart(void);
int8_t DR16_StartDmaRecv(DR16_t *dr16); int8_t DR16_StartDmaRecv(DR16_t *dr16);
bool DR16_WaitDmaCplt(uint32_t timeout); bool DR16_WaitDmaCplt(uint32_t timeout);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }

25
User/device/motor.c
View File

@ -1,5 +1,5 @@
/* /*
DR16接收机 电机通用函数
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@ -7,30 +7,43 @@
#include <string.h> #include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) { float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL; if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.rotor_abs_angle; return motor->feedback.rotor_abs_angle;
} }
float MOTOR_GetRotorSpeed(const MOTOR_t *motor) { float MOTOR_GetRotorSpeed(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL; if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.rotor_speed; return motor->feedback.rotor_speed;
} }
float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) { float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL; if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.torque_current; return motor->feedback.torque_current;
} }
float MOTOR_GetTemp(const MOTOR_t *motor) { float MOTOR_GetTemp(const MOTOR_t *motor) {

16
User/device/motor.h
View File

@ -7,6 +7,14 @@ extern "C" {
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "device/device.h" #include "device/device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
@ -41,12 +49,20 @@ typedef struct {
MOTOR_Feedback_t feedback; MOTOR_Feedback_t feedback;
} MOTOR_t; } MOTOR_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor); float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor);
float MOTOR_GetRotorSpeed(const MOTOR_t *motor); float MOTOR_GetRotorSpeed(const MOTOR_t *motor);
float MOTOR_GetTorqueCurrent(const MOTOR_t *motor); float MOTOR_GetTorqueCurrent(const MOTOR_t *motor);
float MOTOR_GetTemp(const MOTOR_t *motor); float MOTOR_GetTemp(const MOTOR_t *motor);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

37
User/device/motor_lz.c
View File

@ -33,13 +33,6 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
MOTOR_LZ_MotionParam_t lz_recover_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 30.0f,
.kd = 1.0f,
.torque = 0.0f,
};
MOTOR_LZ_MotionParam_t lz_relax_param = { MOTOR_LZ_MotionParam_t lz_relax_param = {
.target_angle = 0.0f, .target_angle = 0.0f,
.target_velocity = 0.0f, .target_velocity = 0.0f,
@ -141,7 +134,7 @@ static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *dat
} else { } else {
memset(tx_frame.data, 0, dlc); memset(tx_frame.data, 0, dlc);
} }
BSP_CAN_WaitTxMailboxEmpty(can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
} }
@ -455,31 +448,3 @@ static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
} }
return NULL; return NULL;
} }
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);
}

27
User/device/motor_lz.h
View File

@ -145,6 +145,31 @@ int8_t MOTOR_LZ_UpdateAll(void);
*/ */
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param); 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 电机使能运行 * @brief 电机使能运行
* @param param 电机参数 * @param param 电机参数
@ -188,8 +213,6 @@ int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
*/ */
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param); int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif