mirror of
https://github.com/goldenfishs/MRobot.git
synced 2026-02-04 18:00:19 +08:00
Merge branch 'User_code' of github.com:goldenfishs/MRobot into User_code
This commit is contained in:
commit
6933dce729
BIN
bsp/.DS_Store
vendored
BIN
bsp/.DS_Store
vendored
Binary file not shown.
@ -1,5 +1,6 @@
|
|||||||
uart,请开启uart的dma和中断
|
uart,请开启uart的dma和中断
|
||||||
can,请开启can中断，使用函数前请确保can已经初始化。一定要开启can发送中断！！！
|
can,请开启can中断，使用函数前请确保can已经初始化。一定要开启can发送中断！！！
|
||||||
|
fdcan,请开启fdcan中断，支持经典CAN和FDCAN模式。会自动根据IOC配置生成对应的宏定义。
|
||||||
gpio,会自动读取cubemx中配置为gpio的引脚，并自动区分输入输出和中断。
|
gpio,会自动读取cubemx中配置为gpio的引脚，并自动区分输入输出和中断。
|
||||||
spi,请开启spi的dma和中断
|
spi,请开启spi的dma和中断
|
||||||
i2c,要求开始spi中断
|
i2c,要求开始spi中断
|
||||||
@ -8,4 +9,4 @@ time,获取时间戳函数，需要开启freerots
|
|||||||
dwt,需要开启dwt，获取时间
|
dwt,需要开启dwt，获取时间
|
||||||
i2c,请开启i2c的dma和中断
|
i2c,请开启i2c的dma和中断
|
||||||
pwm,用于选择那些勇于输出pwm
|
pwm,用于选择那些勇于输出pwm
|
||||||
|
flash,自动识别MCU型号并配置Flash，支持STM32F1(Page)/F4(Sector)/H7(Sector)，自动处理单Bank/双Bank配置
|
||||||
|
|||||||
|
576
bsp/fdcan/fdcan.c
Normal file
576
bsp/fdcan/fdcan.c
Normal file
@ -0,0 +1,576 @@
|
|||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "fdcan.h"
|
||||||
|
#include "bsp/fdcan.h"
|
||||||
|
#include "bsp/bsp.h"
|
||||||
|
#include <fdcan.h>
|
||||||
|
#include <cmsis_os2.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
#define FDCAN_QUEUE_MUTEX_TIMEOUT 100
|
||||||
|
|
||||||
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
|
||||||
|
/* ===== FDCAN_FilterTypeDef 配置表 =====
|
||||||
|
* 定义每个FDCAN实例的过滤器参数表。
|
||||||
|
* 过滤器表参数说明：
|
||||||
|
* idx idtype ftype id1 id2 rxidx
|
||||||
|
* 过滤器编号 标识符类型 过滤器类型 过滤器ID1 过滤器ID2 接收缓冲区索引
|
||||||
|
*/
|
||||||
|
#ifdef FDCAN1_EN
|
||||||
|
#define FDCAN1_FILTER_CONFIG_TABLE(X) \
|
||||||
|
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
|
||||||
|
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
|
||||||
|
#define FDCAN1_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
|
||||||
|
#endif
|
||||||
|
#ifdef FDCAN2_EN
|
||||||
|
#define FDCAN2_FILTER_CONFIG_TABLE(X) \
|
||||||
|
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
|
||||||
|
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
|
||||||
|
#define FDCAN2_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
|
||||||
|
#endif
|
||||||
|
#ifdef FDCAN3_EN
|
||||||
|
#define FDCAN3_FILTER_CONFIG_TABLE(X) \
|
||||||
|
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
|
||||||
|
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
|
||||||
|
#define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* ====宏展开实现==== */
|
||||||
|
#define FDCAN_FILTER_TO_RXFIFO_ENUM_INNER(FIFOIndex) FDCAN_FILTER_TO_RXFIFO##FIFOIndex
|
||||||
|
#define FDCAN_FILTER_TO_RXFIFO_ENUM(FIFOIndex) FDCAN_FILTER_TO_RXFIFO_ENUM_INNER(FIFOIndex)
|
||||||
|
#define FDCAN_CONFIG_FILTER(idx, idtype, ftype, id1, id2, rxidx) \
|
||||||
|
sFilterConfig.FilterIndex = (idx); \
|
||||||
|
sFilterConfig.IdType = (idtype); \
|
||||||
|
sFilterConfig.FilterType = (ftype); \
|
||||||
|
sFilterConfig.FilterConfig = (FDCAN_FILTER_TO_RXFIFO_ENUM(FDCANX_RX_FIFO)); \
|
||||||
|
sFilterConfig.FilterID1 = (id1); \
|
||||||
|
sFilterConfig.FilterID2 = (id2); \
|
||||||
|
sFilterConfig.RxBufferIndex = (rxidx); \
|
||||||
|
HAL_FDCAN_ConfigFilter(&hfdcan, &sFilterConfig);
|
||||||
|
|
||||||
|
#define FDCAN_NOTIFY_FLAG_RXFIFO_INNER(FIFO_IDX) FDCAN_IT_RX_FIFO##FIFO_IDX##_NEW_MESSAGE
|
||||||
|
#define FDCAN_NOTIFY_FLAG_RXFIFO(FIFO_IDX) FDCAN_NOTIFY_FLAG_RXFIFO_INNER(FIFO_IDX)
|
||||||
|
#define FDCANx_NOTIFY_FLAGS(FIFO_MACRO) (FDCAN_NOTIFY_FLAG_RXFIFO(FIFO_MACRO) | FDCAN_IT_TX_EVT_FIFO_NEW_DATA | FDCAN_IT_RAM_ACCESS_FAILURE)
|
||||||
|
|
||||||
|
#define FDCANX_MSG_PENDING_CB_INNER(FIFO_IDX) HAL_FDCAN_RX_FIFO##FIFO_IDX##_MSG_PENDING_CB
|
||||||
|
#define FDCANX_MSG_PENDING_CB(FIFO_IDX) FDCANX_MSG_PENDING_CB_INNER(FIFO_IDX)
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
typedef struct BSP_FDCAN_QueueNode {
|
||||||
|
BSP_FDCAN_t fdcan;
|
||||||
|
uint32_t can_id;
|
||||||
|
osMessageQueueId_t queue;
|
||||||
|
uint8_t queue_size;
|
||||||
|
struct BSP_FDCAN_QueueNode *next;
|
||||||
|
} BSP_FDCAN_QueueNode_t;
|
||||||
|
|
||||||
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
static BSP_FDCAN_QueueNode_t *queue_list = NULL;
|
||||||
|
static osMutexId_t queue_mutex = NULL;
|
||||||
|
static void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
|
||||||
|
static bool inited = false;
|
||||||
|
static BSP_FDCAN_IdParser_t id_parser = NULL;
|
||||||
|
static BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
|
||||||
|
static const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
|
||||||
|
|
||||||
|
/* Private function prototypes ---------------------------------------------- */
|
||||||
|
static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan);
|
||||||
|
static osMessageQueueId_t BSP_FDCAN_FindQueue(BSP_FDCAN_t fdcan, uint32_t can_id);
|
||||||
|
static int8_t BSP_FDCAN_CreateIdQueue(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size);
|
||||||
|
static void BSP_FDCAN_RxFifo0Callback(void);
|
||||||
|
static void BSP_FDCAN_RxFifo1Callback(void);
|
||||||
|
static void BSP_FDCAN_TxCompleteCallback(void);
|
||||||
|
static BSP_FDCAN_FrameType_t BSP_FDCAN_GetFrameType(FDCAN_RxHeaderTypeDef *header);
|
||||||
|
static uint32_t BSP_FDCAN_DefaultIdParser(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type);
|
||||||
|
static void BSP_FDCAN_TxQueueInit(BSP_FDCAN_t fdcan);
|
||||||
|
static bool BSP_FDCAN_TxQueuePush(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg);
|
||||||
|
static bool BSP_FDCAN_TxQueuePop(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg);
|
||||||
|
static bool BSP_FDCAN_TxQueueIsEmpty(BSP_FDCAN_t fdcan);
|
||||||
|
|
||||||
|
/* Private functions -------------------------------------------------------- */
|
||||||
|
static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan) {
|
||||||
|
if (hfdcan == NULL) return BSP_FDCAN_ERR;
|
||||||
|
if (hfdcan->Instance == FDCAN1) return BSP_FDCAN_1;
|
||||||
|
else if (hfdcan->Instance == FDCAN2) return BSP_FDCAN_2;
|
||||||
|
else if (hfdcan->Instance == FDCAN3) return BSP_FDCAN_3;
|
||||||
|
else return BSP_FDCAN_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
static osMessageQueueId_t BSP_FDCAN_FindQueue(BSP_FDCAN_t fdcan, uint32_t can_id) {
|
||||||
|
BSP_FDCAN_QueueNode_t *node = queue_list;
|
||||||
|
while (node != NULL) {
|
||||||
|
if (node->fdcan == fdcan && node->can_id == can_id) return node->queue;
|
||||||
|
node = node->next;
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int8_t BSP_FDCAN_CreateIdQueue(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size) {
|
||||||
|
if (queue_size == 0) queue_size = BSP_FDCAN_DEFAULT_QUEUE_SIZE;
|
||||||
|
if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT;
|
||||||
|
BSP_FDCAN_QueueNode_t *node = queue_list;
|
||||||
|
while (node != NULL) {
|
||||||
|
if (node->fdcan == fdcan && node->can_id == can_id) {
|
||||||
|
osMutexRelease(queue_mutex);
|
||||||
|
return BSP_ERR;
|
||||||
|
}
|
||||||
|
node = node->next;
|
||||||
|
}
|
||||||
|
|
||||||
|
BSP_FDCAN_QueueNode_t *new_node = (BSP_FDCAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_FDCAN_QueueNode_t));
|
||||||
|
if (new_node == NULL) { osMutexRelease(queue_mutex); return BSP_ERR_NULL; }
|
||||||
|
new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_FDCAN_Message_t), NULL);
|
||||||
|
if (new_node->queue == NULL) { BSP_Free(new_node); osMutexRelease(queue_mutex); return BSP_ERR; }
|
||||||
|
new_node->fdcan = fdcan;
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
static BSP_FDCAN_FrameType_t BSP_FDCAN_GetFrameType(FDCAN_RxHeaderTypeDef *header) {
|
||||||
|
if (header->RxFrameType == FDCAN_REMOTE_FRAME) {
|
||||||
|
return (header->IdType == FDCAN_EXTENDED_ID) ? BSP_FDCAN_FRAME_EXT_REMOTE : BSP_FDCAN_FRAME_STD_REMOTE;
|
||||||
|
} else {
|
||||||
|
return (header->IdType == FDCAN_EXTENDED_ID) ? BSP_FDCAN_FRAME_EXT_DATA : BSP_FDCAN_FRAME_STD_DATA;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint32_t BSP_FDCAN_DefaultIdParser(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type) {
|
||||||
|
(void)frame_type;
|
||||||
|
return original_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint32_t BSP_FDCAN_EncodeDLC(uint8_t dlc) {
|
||||||
|
if (dlc <= 8) return dlc;
|
||||||
|
if (dlc <= 12) return FDCAN_DLC_BYTES_12;
|
||||||
|
if (dlc <= 16) return FDCAN_DLC_BYTES_16;
|
||||||
|
if (dlc <= 20) return FDCAN_DLC_BYTES_20;
|
||||||
|
if (dlc <= 24) return FDCAN_DLC_BYTES_24;
|
||||||
|
if (dlc <= 32) return FDCAN_DLC_BYTES_32;
|
||||||
|
if (dlc <= 48) return FDCAN_DLC_BYTES_48;
|
||||||
|
return FDCAN_DLC_BYTES_64;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void BSP_FDCAN_TxQueueInit(BSP_FDCAN_t fdcan) {
|
||||||
|
if (fdcan >= BSP_FDCAN_NUM) return;
|
||||||
|
tx_queues[fdcan].head = 0;
|
||||||
|
tx_queues[fdcan].tail = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool BSP_FDCAN_TxQueuePush(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg) {
|
||||||
|
if (fdcan >= BSP_FDCAN_NUM || msg == NULL) return false;
|
||||||
|
BSP_FDCAN_TxQueue_t *queue = &tx_queues[fdcan];
|
||||||
|
uint32_t next_head = (queue->head + 1) % BSP_FDCAN_TX_QUEUE_SIZE;
|
||||||
|
if (next_head == queue->tail) return false;
|
||||||
|
queue->buffer[queue->head] = *msg;
|
||||||
|
queue->head = next_head;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool BSP_FDCAN_TxQueuePop(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg) {
|
||||||
|
if (fdcan >= BSP_FDCAN_NUM || msg == NULL) return false;
|
||||||
|
BSP_FDCAN_TxQueue_t *queue = &tx_queues[fdcan];
|
||||||
|
if (queue->head == queue->tail) return false;
|
||||||
|
*msg = queue->buffer[queue->tail];
|
||||||
|
queue->tail = (queue->tail + 1) % BSP_FDCAN_TX_QUEUE_SIZE;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool BSP_FDCAN_TxQueueIsEmpty(BSP_FDCAN_t fdcan) {
|
||||||
|
if (fdcan >= BSP_FDCAN_NUM) return true;
|
||||||
|
return tx_queues[fdcan].head == tx_queues[fdcan].tail;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void BSP_FDCAN_TxCompleteCallback(void) {
|
||||||
|
for (int i = 0; i < BSP_FDCAN_NUM; i++) {
|
||||||
|
BSP_FDCAN_t fdcan = (BSP_FDCAN_t)i;
|
||||||
|
FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(fdcan);
|
||||||
|
if (hfdcan == NULL) continue;
|
||||||
|
// 消费所有 TX EVENT FIFO 事件，防止堵塞
|
||||||
|
FDCAN_TxEventFifoTypeDef tx_event;
|
||||||
|
while (HAL_FDCAN_GetTxEvent(hfdcan, &tx_event) == HAL_OK) {
|
||||||
|
// 可在此统计 MessageMarker、ID、时间戳等
|
||||||
|
}
|
||||||
|
// 续写软件队列到硬件 FIFO
|
||||||
|
BSP_FDCAN_TxMessage_t msg;
|
||||||
|
while (!BSP_FDCAN_TxQueueIsEmpty(fdcan)) {
|
||||||
|
if (HAL_FDCAN_GetTxFifoFreeLevel(hfdcan) == 0) break;
|
||||||
|
if (!BSP_FDCAN_TxQueuePop(fdcan, &msg)) break;
|
||||||
|
HAL_StatusTypeDef res = HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &msg.header, msg.data);
|
||||||
|
if (res != HAL_OK) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void BSP_FDCAN_RxFifo0Callback(void) {
|
||||||
|
FDCAN_RxHeaderTypeDef rx_header;
|
||||||
|
uint8_t rx_data[BSP_FDCAN_MAX_DLC];
|
||||||
|
for (int fdcan_idx = 0; fdcan_idx < BSP_FDCAN_NUM; fdcan_idx++) {
|
||||||
|
FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle((BSP_FDCAN_t)fdcan_idx);
|
||||||
|
if (hfdcan == NULL) continue;
|
||||||
|
while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) > 0) {
|
||||||
|
if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) {
|
||||||
|
uint32_t original_id = (rx_header.IdType == FDCAN_STANDARD_ID) ? rx_header.Identifier&0x7ff : rx_header.Identifier&0x1fffffff;
|
||||||
|
BSP_FDCAN_FrameType_t frame_type = BSP_FDCAN_GetFrameType(&rx_header);
|
||||||
|
uint32_t parsed_id = BSP_FDCAN_ParseId(original_id, frame_type);
|
||||||
|
osMessageQueueId_t queue = BSP_FDCAN_FindQueue((BSP_FDCAN_t)fdcan_idx, parsed_id);
|
||||||
|
if (queue != NULL) {
|
||||||
|
BSP_FDCAN_Message_t msg;
|
||||||
|
msg.frame_type = frame_type;
|
||||||
|
msg.original_id = original_id;
|
||||||
|
msg.parsed_id = parsed_id;
|
||||||
|
uint8_t real_len = fdcan_dlc2len[rx_header.DataLength & 0xF];
|
||||||
|
msg.dlc = real_len;
|
||||||
|
if (msg.dlc > BSP_FDCAN_MAX_DLC) msg.dlc = BSP_FDCAN_MAX_DLC;
|
||||||
|
memset(msg.data, 0, BSP_FDCAN_MAX_DLC);//现在是最大缓冲区写法所以全清零
|
||||||
|
memcpy(msg.data, rx_data, msg.dlc);
|
||||||
|
osMessageQueuePut(queue, &msg, 0, 0);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void BSP_FDCAN_RxFifo1Callback(void) {
|
||||||
|
FDCAN_RxHeaderTypeDef rx_header;
|
||||||
|
uint8_t rx_data[BSP_FDCAN_MAX_DLC];
|
||||||
|
for (int fdcan_idx = 0; fdcan_idx < BSP_FDCAN_NUM; fdcan_idx++) {
|
||||||
|
FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle((BSP_FDCAN_t)fdcan_idx);
|
||||||
|
if (hfdcan == NULL) continue;
|
||||||
|
while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO1) > 0) {
|
||||||
|
if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
|
||||||
|
uint32_t original_id = (rx_header.IdType == FDCAN_STANDARD_ID) ? rx_header.Identifier&0x7ff : rx_header.Identifier&0x1fffffff;
|
||||||
|
BSP_FDCAN_FrameType_t frame_type = BSP_FDCAN_GetFrameType(&rx_header);
|
||||||
|
uint32_t parsed_id = BSP_FDCAN_ParseId(original_id, frame_type);
|
||||||
|
osMessageQueueId_t queue = BSP_FDCAN_FindQueue((BSP_FDCAN_t)fdcan_idx, parsed_id);
|
||||||
|
if (queue != NULL) {
|
||||||
|
BSP_FDCAN_Message_t msg;
|
||||||
|
msg.frame_type = frame_type;
|
||||||
|
msg.original_id = original_id;
|
||||||
|
msg.parsed_id = parsed_id;
|
||||||
|
uint8_t real_len = fdcan_dlc2len[rx_header.DataLength & 0xF];
|
||||||
|
msg.dlc = real_len;
|
||||||
|
if (msg.dlc > BSP_FDCAN_MAX_DLC) msg.dlc = BSP_FDCAN_MAX_DLC;
|
||||||
|
memset(msg.data, 0, BSP_FDCAN_MAX_DLC);//现在是最大缓冲区写法所以全清零
|
||||||
|
memcpy(msg.data, rx_data, msg.dlc);
|
||||||
|
osMessageQueuePut(queue, &msg, 0, 0);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* HAL Callback Stubs (map HAL FDCAN callbacks to user callbacks) */
|
||||||
|
void HAL_FDCAN_TxEventFifoCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t TxEventFifoITs) {
|
||||||
|
BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
|
||||||
|
if (bsp_fdcan != BSP_FDCAN_ERR) {
|
||||||
|
if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_EVENT_FIFO_CB])
|
||||||
|
FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_EVENT_FIFO_CB]();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndex) {
|
||||||
|
BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
|
||||||
|
if (bsp_fdcan != BSP_FDCAN_ERR) {
|
||||||
|
if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_COMPLETE_CB])
|
||||||
|
FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_COMPLETE_CB]();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_FDCAN_TxBufferAbortCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndex) {
|
||||||
|
BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
|
||||||
|
if (bsp_fdcan != BSP_FDCAN_ERR) {
|
||||||
|
if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_ABORT_CB])
|
||||||
|
FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_ABORT_CB]();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
|
||||||
|
BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
|
||||||
|
if (bsp_fdcan != BSP_FDCAN_ERR) {
|
||||||
|
if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB])
|
||||||
|
FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB]();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo1ITs) {
|
||||||
|
BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
|
||||||
|
if (bsp_fdcan != BSP_FDCAN_ERR) {
|
||||||
|
if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB])
|
||||||
|
FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB]();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HAL_FDCAN_ErrorCallback(FDCAN_HandleTypeDef *hfdcan) {
|
||||||
|
BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan);
|
||||||
|
if (bsp_fdcan != BSP_FDCAN_ERR) {
|
||||||
|
if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_ERROR_CB])
|
||||||
|
FDCAN_Callback[bsp_fdcan][HAL_FDCAN_ERROR_CB]();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
int8_t BSP_FDCAN_Init(void) {
|
||||||
|
if (inited) return BSP_ERR_INITED;
|
||||||
|
|
||||||
|
memset(FDCAN_Callback, 0, sizeof(FDCAN_Callback));
|
||||||
|
for (int i = 0; i < BSP_FDCAN_NUM; i++) BSP_FDCAN_TxQueueInit((BSP_FDCAN_t)i);
|
||||||
|
id_parser = BSP_FDCAN_DefaultIdParser;
|
||||||
|
queue_mutex = osMutexNew(NULL);
|
||||||
|
if (queue_mutex == NULL) return BSP_ERR;
|
||||||
|
|
||||||
|
inited = true;
|
||||||
|
|
||||||
|
/* 配置并启动 FDCAN 实例，绑定中断/回调 */
|
||||||
|
|
||||||
|
//========== 过滤器配置说明：==========================
|
||||||
|
// 过滤器编号：相对于每个（相当于经典can过滤器的bank）
|
||||||
|
// sFilterConfig.FilterIndex = 0 to 127(标准ID) or 0 to 63(扩展ID);
|
||||||
|
// 关于过滤器索引的说明：
|
||||||
|
// 由stm32h7xx_hal_fdcan.c的第1874行代码可知滤波器地址计算方式如下：
|
||||||
|
// StandardFilterSA（字节） = SRAMCAN_BASE + (MessageRAMOffset * 4U)
|
||||||
|
// 标准滤波器物理地址（字节） = StandardFilterSA + (FilterIndex * 4U)（每个标准滤波器占 4 字节 = 1 word,扩展的则是8个字节）
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// 标识符类型:
|
||||||
|
// sFilterConfig.IdType = FDCAN_STANDARD_ID or FDCAN_EXTENDED_ID;
|
||||||
|
// 过滤器类型: （仅介绍掩码模式）
|
||||||
|
// sFilterConfig.FilterType = FDCAN_FILTER_MASK;(掩码模式）
|
||||||
|
// 过滤器配置:
|
||||||
|
// sFilterConfig.FilterConfig = FDCAN_FILTER_DISABLE; (禁用该过滤器条目)
|
||||||
|
// FDCAN_FILTER_TO_RXFIFO0; (将匹配的消息放入 FIFO 0（普通优先级）)
|
||||||
|
// FDCAN_FILTER_TO_RXFIFO1; (将匹配的消息放入 FIFO 1（高优先级）)
|
||||||
|
// FDCAN_FILTER_TO_RXBUFFER; (将匹配的消息放入 指定的接收缓冲区)
|
||||||
|
// FDCAN_FILTER_REJECT; (拒绝接收该标识符对应的报文)
|
||||||
|
// FDCAN_FILTER_ACCEPT; (接受所有消息)
|
||||||
|
// FDCAN_FILTER_HP (过滤器匹配时，将报文标记为高优先级)
|
||||||
|
// FDCAN_FILTER_TO_RXFIFO0_HP (过滤器匹配时，将报文标记为高优先级并存储至接收FIFO 0)
|
||||||
|
// FDCAN_FILTER_TO_RXFIFO1_HP (过滤器匹配时，将报文标记为高优先级并存储至接收FIFO 1)
|
||||||
|
// FDCAN_FILTER_TO_RXBUFFER (将报文存储至接收缓冲区，过滤器类型（FilterType）配置项失效 )
|
||||||
|
// 过滤器ID与掩码(FilterType掩码模式下)
|
||||||
|
// 比较值（要匹配的 ID 的参考位）
|
||||||
|
// sFilterConfig.FilterID1 = 0 to 0x7FF; 标准ID
|
||||||
|
// 0 to 0x1FFFFFFF 扩展ID
|
||||||
|
// 掩码（1=比较该位，0=忽略该位）
|
||||||
|
// sFilterConfig.FilterID2 = 0 to 0x7FF; 标准ID
|
||||||
|
// 0 to 0x1FFFFFFF 扩展ID
|
||||||
|
// 接收缓冲区索引
|
||||||
|
// FilterConfig == FDCAN_FILTER_TO_RXBUFFER 时有效;必须小于RxBuffersNbr配置的实际Rx buffer数量
|
||||||
|
// sFilterConfig.RxBufferIndex = 0 to (RxBuffersNbr - 1);
|
||||||
|
// 标记校准信息（用于 FDCAN 校准/时钟相关单元作特殊处理或统计）
|
||||||
|
// 仅在FilterConfig 设为 FDCAN_FILTER_TO_RXBUFFER 时才有意义，通常设置为0
|
||||||
|
// IsCalibrationMsg = 0 or 1;
|
||||||
|
// fdcan_filter_table.h
|
||||||
|
//=================================================================================
|
||||||
|
/* 依据上述说明，配置过滤器并启动FDCAN */
|
||||||
|
FDCAN_FilterTypeDef sFilterConfig;
|
||||||
|
|
||||||
|
#ifdef FDCAN1_EN
|
||||||
|
#define hfdcan hfdcan1
|
||||||
|
#define FDCANX_RX_FIFO FDCAN1_RX_FIFO
|
||||||
|
FDCAN1_FILTER_CONFIG_TABLE(FDCAN_CONFIG_FILTER)
|
||||||
|
#undef hfdcan
|
||||||
|
#undef FDCANX_RX_FIFO
|
||||||
|
HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN1_GLOBAL_FILTER);
|
||||||
|
HAL_FDCAN_ActivateNotification(&hfdcan1, FDCANx_NOTIFY_FLAGS(FDCAN1_RX_FIFO), 0);
|
||||||
|
BSP_FDCAN_RegisterCallback(BSP_FDCAN_1, FDCANX_MSG_PENDING_CB(FDCAN1_RX_FIFO), BSP_FDCAN_RxFifo0Callback);
|
||||||
|
BSP_FDCAN_RegisterCallback(BSP_FDCAN_1, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback);
|
||||||
|
HAL_FDCAN_Start(&hfdcan1);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef FDCAN2_EN
|
||||||
|
#define hfdcan hfdcan2
|
||||||
|
#define FDCANX_RX_FIFO FDCAN2_RX_FIFO
|
||||||
|
FDCAN2_FILTER_CONFIG_TABLE(FDCAN_CONFIG_FILTER)
|
||||||
|
#undef hfdcan
|
||||||
|
#undef FDCANX_RX_FIFO
|
||||||
|
HAL_FDCAN_ConfigGlobalFilter(&hfdcan2, FDCAN2_GLOBAL_FILTER);
|
||||||
|
HAL_FDCAN_ActivateNotification(&hfdcan2, FDCANx_NOTIFY_FLAGS(FDCAN2_RX_FIFO), 0);
|
||||||
|
BSP_FDCAN_RegisterCallback(BSP_FDCAN_2, FDCANX_MSG_PENDING_CB(FDCAN2_RX_FIFO), BSP_FDCAN_RxFifo1Callback);
|
||||||
|
BSP_FDCAN_RegisterCallback(BSP_FDCAN_2, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback);
|
||||||
|
HAL_FDCAN_Start(&hfdcan2);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef FDCAN3_EN
|
||||||
|
#define hfdcan hfdcan3
|
||||||
|
#define FDCANX_RX_FIFO FDCAN3_RX_FIFO
|
||||||
|
FDCAN3_FILTER_CONFIG_TABLE(FDCAN_CONFIG_FILTER)
|
||||||
|
#undef hfdcan
|
||||||
|
#undef FDCANX_RX_FIFO
|
||||||
|
HAL_FDCAN_ConfigGlobalFilter(&hfdcan3, FDCAN3_GLOBAL_FILTER);
|
||||||
|
HAL_FDCAN_ActivateNotification(&hfdcan3, FDCANx_NOTIFY_FLAGS(FDCAN3_RX_FIFO), 0);
|
||||||
|
BSP_FDCAN_RegisterCallback(BSP_FDCAN_3, FDCANX_MSG_PENDING_CB(FDCAN3_RX_FIFO), BSP_FDCAN_RxFifo1Callback);
|
||||||
|
BSP_FDCAN_RegisterCallback(BSP_FDCAN_3, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback);
|
||||||
|
HAL_FDCAN_Start(&hfdcan3);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#undef FDCAN_FILTER_TO_RXFIFO_ENUM_INNER
|
||||||
|
#undef FDCAN_FILTER_TO_RXFIFO_ENUM
|
||||||
|
#undef FDCAN_CONFIG_FILTER
|
||||||
|
#undef FDCAN_NOTIFY_FLAG_RXFIFO_INNER
|
||||||
|
#undef FDCAN_NOTIFY_FLAG_RXFIFO
|
||||||
|
#undef FDCANx_NOTIFY_FLAGS
|
||||||
|
#undef FDCANX_MSG_PENDING_CB_INNER
|
||||||
|
#undef FDCANX_MSG_PENDING_CB
|
||||||
|
|
||||||
|
return BSP_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
FDCAN_HandleTypeDef *BSP_FDCAN_GetHandle(BSP_FDCAN_t fdcan) {
|
||||||
|
if (fdcan >= BSP_FDCAN_NUM) return NULL;
|
||||||
|
switch (fdcan) {
|
||||||
|
/* AUTO GENERATED BSP_FDCAN_GET_HANDLE BEGIN */
|
||||||
|
case BSP_FDCAN_1: return &hfdcan1;
|
||||||
|
case BSP_FDCAN_2: return &hfdcan2;
|
||||||
|
case BSP_FDCAN_3: return &hfdcan3;
|
||||||
|
/* AUTO GENERATED BSP_FDCAN_GET_HANDLE END */
|
||||||
|
default: return NULL;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_RegisterCallback(BSP_FDCAN_t fdcan, BSP_FDCAN_Callback_t type, void (*callback)(void)) {
|
||||||
|
if (!inited) return BSP_ERR_INITED;
|
||||||
|
if (callback == NULL) return BSP_ERR_NULL;
|
||||||
|
if (fdcan >= BSP_FDCAN_NUM) return BSP_ERR;
|
||||||
|
if (type >= HAL_FDCAN_CB_NUM) return BSP_ERR;
|
||||||
|
FDCAN_Callback[fdcan][type] = callback;
|
||||||
|
return BSP_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_Transmit(BSP_FDCAN_t fdcan, BSP_FDCAN_Format_t format, uint32_t id, uint8_t *data, uint8_t dlc) {
|
||||||
|
if (!inited) return BSP_ERR_INITED;
|
||||||
|
if (fdcan >= BSP_FDCAN_NUM) return BSP_ERR;
|
||||||
|
if (data == NULL && format != BSP_FDCAN_FORMAT_STD_REMOTE && format != BSP_FDCAN_FORMAT_EXT_REMOTE) return BSP_ERR_NULL;
|
||||||
|
if (dlc > BSP_FDCAN_MAX_DLC) return BSP_ERR;
|
||||||
|
FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(fdcan);
|
||||||
|
if (hfdcan == NULL) return BSP_ERR_NULL;
|
||||||
|
|
||||||
|
BSP_FDCAN_TxMessage_t tx_msg = {0};
|
||||||
|
switch (format) {
|
||||||
|
case BSP_FDCAN_FORMAT_STD_DATA:
|
||||||
|
tx_msg.header.Identifier = id;
|
||||||
|
tx_msg.header.IdType = FDCAN_STANDARD_ID;
|
||||||
|
tx_msg.header.TxFrameType = FDCAN_DATA_FRAME;
|
||||||
|
break;
|
||||||
|
case BSP_FDCAN_FORMAT_EXT_DATA:
|
||||||
|
tx_msg.header.Identifier = id;
|
||||||
|
tx_msg.header.IdType = FDCAN_EXTENDED_ID;
|
||||||
|
tx_msg.header.TxFrameType = FDCAN_DATA_FRAME;
|
||||||
|
break;
|
||||||
|
case BSP_FDCAN_FORMAT_STD_REMOTE:
|
||||||
|
tx_msg.header.Identifier = id;
|
||||||
|
tx_msg.header.IdType = FDCAN_STANDARD_ID;
|
||||||
|
tx_msg.header.TxFrameType = FDCAN_REMOTE_FRAME;
|
||||||
|
break;
|
||||||
|
case BSP_FDCAN_FORMAT_EXT_REMOTE:
|
||||||
|
tx_msg.header.Identifier = id;
|
||||||
|
tx_msg.header.IdType = FDCAN_EXTENDED_ID;
|
||||||
|
tx_msg.header.TxFrameType = FDCAN_REMOTE_FRAME;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return BSP_ERR;
|
||||||
|
}
|
||||||
|
switch (hfdcan->Init.FrameFormat) {
|
||||||
|
case FDCAN_FRAME_FD_BRS:
|
||||||
|
tx_msg.header.BitRateSwitch = FDCAN_BRS_ON;
|
||||||
|
tx_msg.header.FDFormat = FDCAN_FD_CAN;
|
||||||
|
break;
|
||||||
|
case FDCAN_FRAME_FD_NO_BRS:
|
||||||
|
tx_msg.header.BitRateSwitch = FDCAN_BRS_OFF;
|
||||||
|
tx_msg.header.FDFormat = FDCAN_FD_CAN;
|
||||||
|
break;
|
||||||
|
case FDCAN_FRAME_CLASSIC:
|
||||||
|
default:
|
||||||
|
tx_msg.header.BitRateSwitch = FDCAN_BRS_OFF;
|
||||||
|
tx_msg.header.FDFormat = FDCAN_CLASSIC_CAN;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
tx_msg.header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
|
||||||
|
tx_msg.header.TxEventFifoControl = FDCAN_STORE_TX_EVENTS;
|
||||||
|
tx_msg.header.MessageMarker = 0x01;
|
||||||
|
tx_msg.header.DataLength = BSP_FDCAN_EncodeDLC(dlc);
|
||||||
|
|
||||||
|
memset(tx_msg.data, 0, dlc);
|
||||||
|
if (data != NULL && dlc > 0) {memcpy(tx_msg.data, data, dlc);}
|
||||||
|
|
||||||
|
if (HAL_FDCAN_GetTxFifoFreeLevel(hfdcan) > 0) {
|
||||||
|
if (HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &tx_msg.header, tx_msg.data) == HAL_OK) return BSP_OK;
|
||||||
|
}
|
||||||
|
if (BSP_FDCAN_TxQueuePush(fdcan, &tx_msg)) return BSP_OK;
|
||||||
|
return BSP_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_TransmitStdDataFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_StdDataFrame_t *frame) {
|
||||||
|
if (frame == NULL) return BSP_ERR_NULL;
|
||||||
|
return BSP_FDCAN_Transmit(fdcan, BSP_FDCAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_TransmitExtDataFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_ExtDataFrame_t *frame) {
|
||||||
|
if (frame == NULL) return BSP_ERR_NULL;
|
||||||
|
return BSP_FDCAN_Transmit(fdcan, BSP_FDCAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_TransmitRemoteFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_RemoteFrame_t *frame) {
|
||||||
|
if (frame == NULL) return BSP_ERR_NULL;
|
||||||
|
BSP_FDCAN_Format_t format = frame->is_extended ? BSP_FDCAN_FORMAT_EXT_REMOTE : BSP_FDCAN_FORMAT_STD_REMOTE;
|
||||||
|
return BSP_FDCAN_Transmit(fdcan, format, frame->id, NULL, frame->dlc);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_RegisterId(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size) {
|
||||||
|
if (!inited) return BSP_ERR_INITED;
|
||||||
|
return BSP_FDCAN_CreateIdQueue(fdcan, can_id, queue_size);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_GetMessage(BSP_FDCAN_t fdcan, uint32_t can_id, BSP_FDCAN_Message_t *msg, uint32_t timeout) {
|
||||||
|
if (!inited) return BSP_ERR_INITED;
|
||||||
|
if (msg == NULL) return BSP_ERR_NULL;
|
||||||
|
if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT;
|
||||||
|
osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id);
|
||||||
|
osMutexRelease(queue_mutex);
|
||||||
|
if (queue == NULL) return BSP_ERR_NO_DEV;
|
||||||
|
osStatus_t res = osMessageQueueGet(queue, msg, NULL, timeout);
|
||||||
|
return (res == osOK) ? BSP_OK : BSP_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32_t BSP_FDCAN_GetQueueCount(BSP_FDCAN_t fdcan, uint32_t can_id) {
|
||||||
|
if (!inited) return -1;
|
||||||
|
if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return -1;
|
||||||
|
osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id);
|
||||||
|
osMutexRelease(queue_mutex);
|
||||||
|
if (queue == NULL) return -1;
|
||||||
|
return (int32_t)osMessageQueueGetCount(queue);
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_FlushQueue(BSP_FDCAN_t fdcan, uint32_t can_id) {
|
||||||
|
if (!inited) return BSP_ERR_INITED;
|
||||||
|
if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT;
|
||||||
|
osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id);
|
||||||
|
osMutexRelease(queue_mutex);
|
||||||
|
if (queue == NULL) return BSP_ERR_NO_DEV;
|
||||||
|
BSP_FDCAN_Message_t tmp;
|
||||||
|
while (osMessageQueueGet(queue, &tmp, NULL, BSP_FDCAN_TIMEOUT_IMMEDIATE) == osOK) { }
|
||||||
|
return BSP_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t BSP_FDCAN_RegisterIdParser(BSP_FDCAN_IdParser_t parser) {
|
||||||
|
if (!inited) return BSP_ERR_INITED;
|
||||||
|
if (parser == NULL) return BSP_ERR_NULL;
|
||||||
|
id_parser = parser;
|
||||||
|
return BSP_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type) {
|
||||||
|
if (id_parser != NULL) return id_parser(original_id, frame_type);
|
||||||
|
return BSP_FDCAN_DefaultIdParser(original_id, frame_type);
|
||||||
|
}
|
||||||
|
/* */
|
||||||
|
|
||||||
137
bsp/fdcan/fdcan.h
Normal file
137
bsp/fdcan/fdcan.h
Normal file
@ -0,0 +1,137 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include "bsp/bsp.h"
|
||||||
|
#include "bsp/mm.h"
|
||||||
|
#include <cmsis_os.h>
|
||||||
|
|
||||||
|
/* USER INCLUDE BEGIN */
|
||||||
|
#include <fdcan.h>
|
||||||
|
/* USER INCLUDE END */
|
||||||
|
|
||||||
|
/* Exported constants ------------------------------------------------------- */
|
||||||
|
#define BSP_FDCAN_MAX_DLC 64
|
||||||
|
#define BSP_FDCAN_DEFAULT_QUEUE_SIZE 10
|
||||||
|
#define BSP_FDCAN_TIMEOUT_IMMEDIATE 0
|
||||||
|
#define BSP_FDCAN_TIMEOUT_FOREVER osWaitForever
|
||||||
|
#define BSP_FDCAN_TX_QUEUE_SIZE 32
|
||||||
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
|
//FDCANX实例使能
|
||||||
|
/* AUTO GENERATED FDCAN_EN BEGIN */
|
||||||
|
#define FDCAN1_EN
|
||||||
|
#define FDCAN2_EN
|
||||||
|
#define FDCAN3_EN
|
||||||
|
/* AUTO GENERATED FDCAN_EN END */
|
||||||
|
|
||||||
|
// FDCANX接收FIFO选择（0=FIFO0, 1=FIFO1）
|
||||||
|
/* AUTO GENERATED FDCAN_RX_FIFO BEGIN */
|
||||||
|
#ifdef FDCAN1_EN
|
||||||
|
#define FDCAN1_RX_FIFO 0
|
||||||
|
#endif
|
||||||
|
#ifdef FDCAN2_EN
|
||||||
|
#define FDCAN2_RX_FIFO 1
|
||||||
|
#endif
|
||||||
|
#ifdef FDCAN3_EN
|
||||||
|
#define FDCAN3_RX_FIFO 1
|
||||||
|
#endif
|
||||||
|
/* AUTO GENERATED FDCAN_RX_FIFO END */
|
||||||
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
typedef enum {
|
||||||
|
/* AUTO GENERATED BSP_FDCAN_NAME BEGIN */
|
||||||
|
BSP_FDCAN_1,
|
||||||
|
BSP_FDCAN_2,
|
||||||
|
BSP_FDCAN_3,
|
||||||
|
/* AUTO GENERATED BSP_FDCAN_NAME END */
|
||||||
|
BSP_FDCAN_NUM,
|
||||||
|
BSP_FDCAN_ERR,
|
||||||
|
} BSP_FDCAN_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
HAL_FDCAN_TX_EVENT_FIFO_CB,
|
||||||
|
HAL_FDCAN_TX_BUFFER_COMPLETE_CB,
|
||||||
|
HAL_FDCAN_TX_BUFFER_ABORT_CB,
|
||||||
|
HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB,
|
||||||
|
HAL_FDCAN_RX_FIFO0_FULL_CB,
|
||||||
|
HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB,
|
||||||
|
HAL_FDCAN_RX_FIFO1_FULL_CB,
|
||||||
|
HAL_FDCAN_ERROR_CB,
|
||||||
|
HAL_FDCAN_CB_NUM,
|
||||||
|
} BSP_FDCAN_Callback_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
BSP_FDCAN_FORMAT_STD_DATA,
|
||||||
|
BSP_FDCAN_FORMAT_EXT_DATA,
|
||||||
|
BSP_FDCAN_FORMAT_STD_REMOTE,
|
||||||
|
BSP_FDCAN_FORMAT_EXT_REMOTE,
|
||||||
|
} BSP_FDCAN_Format_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
BSP_FDCAN_FRAME_STD_DATA,
|
||||||
|
BSP_FDCAN_FRAME_EXT_DATA,
|
||||||
|
BSP_FDCAN_FRAME_STD_REMOTE,
|
||||||
|
BSP_FDCAN_FRAME_EXT_REMOTE,
|
||||||
|
} BSP_FDCAN_FrameType_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
BSP_FDCAN_FrameType_t frame_type;
|
||||||
|
uint32_t original_id;
|
||||||
|
uint32_t parsed_id;
|
||||||
|
uint8_t dlc;
|
||||||
|
uint8_t data[BSP_FDCAN_MAX_DLC];
|
||||||
|
uint32_t timestamp;
|
||||||
|
} BSP_FDCAN_Message_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint32_t id;
|
||||||
|
uint8_t dlc;
|
||||||
|
uint8_t data[BSP_FDCAN_MAX_DLC];
|
||||||
|
} BSP_FDCAN_StdDataFrame_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint32_t id;
|
||||||
|
uint8_t dlc;
|
||||||
|
uint8_t data[BSP_FDCAN_MAX_DLC];
|
||||||
|
} BSP_FDCAN_ExtDataFrame_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint32_t id;
|
||||||
|
uint8_t dlc;
|
||||||
|
bool is_extended;
|
||||||
|
} BSP_FDCAN_RemoteFrame_t;
|
||||||
|
|
||||||
|
typedef uint32_t (*BSP_FDCAN_IdParser_t)(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type);
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
FDCAN_TxHeaderTypeDef header; /* HAL FDCAN header type */
|
||||||
|
uint8_t data[BSP_FDCAN_MAX_DLC];
|
||||||
|
} BSP_FDCAN_TxMessage_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
BSP_FDCAN_TxMessage_t buffer[BSP_FDCAN_TX_QUEUE_SIZE];
|
||||||
|
volatile uint32_t head;
|
||||||
|
volatile uint32_t tail;
|
||||||
|
} BSP_FDCAN_TxQueue_t;
|
||||||
|
|
||||||
|
/* Exported functions prototypes -------------------------------------------- */
|
||||||
|
int8_t BSP_FDCAN_Init(void);
|
||||||
|
FDCAN_HandleTypeDef *BSP_FDCAN_GetHandle(BSP_FDCAN_t can);
|
||||||
|
int8_t BSP_FDCAN_RegisterCallback(BSP_FDCAN_t can, BSP_FDCAN_Callback_t type, void (*callback)(void));
|
||||||
|
int8_t BSP_FDCAN_Transmit(BSP_FDCAN_t can, BSP_FDCAN_Format_t format, uint32_t id, uint8_t *data, uint8_t dlc);
|
||||||
|
int8_t BSP_FDCAN_TransmitStdDataFrame(BSP_FDCAN_t can, BSP_FDCAN_StdDataFrame_t *frame);
|
||||||
|
int8_t BSP_FDCAN_TransmitExtDataFrame(BSP_FDCAN_t can, BSP_FDCAN_ExtDataFrame_t *frame);
|
||||||
|
int8_t BSP_FDCAN_TransmitRemoteFrame(BSP_FDCAN_t can, BSP_FDCAN_RemoteFrame_t *frame);
|
||||||
|
int8_t BSP_FDCAN_RegisterId(BSP_FDCAN_t can, uint32_t can_id, uint8_t queue_size);
|
||||||
|
int8_t BSP_FDCAN_GetMessage(BSP_FDCAN_t can, uint32_t can_id, BSP_FDCAN_Message_t *msg, uint32_t timeout);
|
||||||
|
int32_t BSP_FDCAN_GetQueueCount(BSP_FDCAN_t can, uint32_t can_id);
|
||||||
|
int8_t BSP_FDCAN_FlushQueue(BSP_FDCAN_t can, uint32_t can_id);
|
||||||
|
int8_t BSP_FDCAN_RegisterIdParser(BSP_FDCAN_IdParser_t parser);
|
||||||
|
uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type);
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
77
bsp/flash/CHANGELOG.md
Normal file
77
bsp/flash/CHANGELOG.md
Normal file
@ -0,0 +1,77 @@
|
|||||||
|
# Flash BSP 更新日志
|
||||||
|
|
||||||
|
## v2.0 - 2026-01-01
|
||||||
|
|
||||||
|
### 新增功能
|
||||||
|
✨ **多系列MCU支持**
|
||||||
|
- 新增 STM32F1 系列支持（Page模式）
|
||||||
|
- 新增 STM32H7 系列支持（Sector模式）
|
||||||
|
- 保持 STM32F4 系列支持（Sector模式）
|
||||||
|
|
||||||
|
### STM32F1系列详情
|
||||||
|
- **Flash组织**: Page模式（页）
|
||||||
|
- **页大小**:
|
||||||
|
- 小/中容量（≤128KB）: 1KB/页
|
||||||
|
- 大容量/互联型（>128KB）: 2KB/页
|
||||||
|
- **容量支持**: 16KB - 1MB
|
||||||
|
- **容量代码**: 4/6/8/B/C/D/E/F/G
|
||||||
|
- **生成宏**: `ADDR_FLASH_PAGE_X`
|
||||||
|
|
||||||
|
### STM32H7系列详情
|
||||||
|
- **Flash组织**: Sector模式（扇区）
|
||||||
|
- **扇区大小**: 固定128KB
|
||||||
|
- **容量支持**: 128KB - 2MB
|
||||||
|
- **容量代码**: B/G/I
|
||||||
|
- **Bank支持**:
|
||||||
|
- 单Bank: 1MB (8个Sector)
|
||||||
|
- 双Bank: 2MB (16个Sector)
|
||||||
|
- **生成宏**: `ADDR_FLASH_SECTOR_X`
|
||||||
|
|
||||||
|
### 技术改进
|
||||||
|
- 重构 `get_flash_config_from_mcu()` 函数为多系列架构
|
||||||
|
- 新增 `_get_stm32f1_flash_config()` - F1系列专用配置
|
||||||
|
- 新增 `_get_stm32f4_flash_config()` - F4系列专用配置
|
||||||
|
- 新增 `_get_stm32h7_flash_config()` - H7系列专用配置
|
||||||
|
- 配置中新增 `type` 字段区分 'page' 和 'sector' 模式
|
||||||
|
- 界面自动识别并显示Page或Sector模式
|
||||||
|
- 代码生成支持Page和Sector两种宏定义
|
||||||
|
|
||||||
|
### 示例支持的芯片型号
|
||||||
|
**STM32F1:**
|
||||||
|
- STM32F103C8T6 → 64KB (64 pages × 1KB)
|
||||||
|
- STM32F103RCT6 → 256KB (128 pages × 2KB)
|
||||||
|
- STM32F103ZET6 → 512KB (256 pages × 2KB)
|
||||||
|
|
||||||
|
**STM32F4:**
|
||||||
|
- STM32F407VGT6 → 1MB (Sector 0-11)
|
||||||
|
- STM32F407IGH6 → 2MB (Sector 0-23, 双Bank)
|
||||||
|
- STM32F405RGT6 → 1MB (Sector 0-11)
|
||||||
|
|
||||||
|
**STM32H7:**
|
||||||
|
- STM32H750VBT6 → 128KB (1 sector)
|
||||||
|
- STM32H743VGT6 → 1MB (8 sectors)
|
||||||
|
- STM32H743VIT6 → 2MB (16 sectors, 双Bank)
|
||||||
|
|
||||||
|
### 配置文件变化
|
||||||
|
```yaml
|
||||||
|
# 新增字段
|
||||||
|
flash:
|
||||||
|
type: page # 或 sector
|
||||||
|
page_size: 2 # 仅F1系列有此字段
|
||||||
|
```
|
||||||
|
|
||||||
|
### 文档更新
|
||||||
|
- 更新 README.md 包含三个系列的完整说明
|
||||||
|
- 新增各系列的Flash布局图
|
||||||
|
- 新增各系列的使用示例
|
||||||
|
- 更新注意事项包含擦除时间和寿命信息
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## v1.0 - 初始版本
|
||||||
|
|
||||||
|
### 初始功能
|
||||||
|
- STM32F4 系列支持
|
||||||
|
- 自动识别芯片型号
|
||||||
|
- 单Bank/双Bank配置
|
||||||
|
- 基础API（擦除、读、写）
|
||||||
346
bsp/flash/README.md
Normal file
346
bsp/flash/README.md
Normal file
@ -0,0 +1,346 @@
|
|||||||
|
# Flash BSP 自动配置说明
|
||||||
|
|
||||||
|
## 功能特性
|
||||||
|
|
||||||
|
Flash BSP模块能够自动识别STM32芯片型号并生成对应的Flash配置代码。
|
||||||
|
|
||||||
|
### 支持的芯片系列
|
||||||
|
|
||||||
|
#### STM32F1 系列
|
||||||
|
- 使用**Page**组织方式（而非Sector）
|
||||||
|
- 自动检测Flash容量（16KB - 1MB）
|
||||||
|
- 小/中容量设备：1KB/页
|
||||||
|
- 大容量/互联型设备：2KB/页
|
||||||
|
|
||||||
|
#### STM32F4 系列
|
||||||
|
- 使用**Sector**组织方式
|
||||||
|
- 自动检测Flash容量（256KB/512KB/1MB/2MB）
|
||||||
|
- 自动配置单Bank或双Bank模式
|
||||||
|
- 不同大小的Sector（16KB/64KB/128KB）
|
||||||
|
|
||||||
|
#### STM32H7 系列
|
||||||
|
- 使用**Sector**组织方式
|
||||||
|
- 每个Sector固定128KB
|
||||||
|
- 自动检测Flash容量（128KB/1MB/2MB）
|
||||||
|
- 自动配置单Bank或双Bank模式
|
||||||
|
|
||||||
|
### Flash容量识别规则
|
||||||
|
|
||||||
|
根据STM32命名规则中的第9位字符识别Flash容量：
|
||||||
|
|
||||||
|
**STM32F1系列:**
|
||||||
|
- **4**: 16KB (16 pages × 1KB)
|
||||||
|
- **6**: 32KB (32 pages × 1KB)
|
||||||
|
- **8**: 64KB (64 pages × 1KB)
|
||||||
|
- **B**: 128KB (128 pages × 1KB)
|
||||||
|
- **C**: 256KB (128 pages × 2KB)
|
||||||
|
- **D**: 384KB (192 pages × 2KB)
|
||||||
|
- **E**: 512KB (256 pages × 2KB)
|
||||||
|
- **F**: 768KB (384 pages × 2KB, 互联型)
|
||||||
|
- **G**: 1MB (512 pages × 2KB, 互联型)
|
||||||
|
|
||||||
|
**STM32F4系列:**
|
||||||
|
- **C**: 256KB (单Bank, Sector 0-7)
|
||||||
|
- **E**: 512KB (单Bank, Sector 0-9)
|
||||||
|
- **G**: 1MB (单Bank, Sector 0-11)
|
||||||
|
- **I**: 2MB (双Bank, Sector 0-23)
|
||||||
|
|
||||||
|
**STM32H7系列:**
|
||||||
|
- **B**: 128KB (1个Sector, 单Bank)
|
||||||
|
- **G**: 1MB (8个Sector, 单Bank)
|
||||||
|
- **I**: 2MB (16个Sector, 双Bank)
|
||||||
|
|
||||||
|
例如：
|
||||||
|
- `STM32F103C8T6` → 64KB Flash (64 pages × 1KB)
|
||||||
|
- `STM32F103RCT6` → 256KB Flash (128 pages × 2KB)
|
||||||
|
- `STM32F103ZET6` → 512KB Flash (256 pages × 2KB)
|
||||||
|
- `STM32F407VGT6` → 1MB Flash (Sector 0-11)
|
||||||
|
- `STM32F407IGH6` → 2MB Flash (Sector 0-23, 双Bank)
|
||||||
|
- `STM32F405RGT6` → 1MB Flash (Sector 0-11)
|
||||||
|
- `STM32H743VIT6` → 2MB Flash (16 sectors × 128KB, 双Bank)
|
||||||
|
- `STM32H750VBT6` → 128KB Flash (1 sector × 128KB)
|
||||||
|
|
||||||
|
## Flash布局
|
||||||
|
|
||||||
|
### STM32F1 Page模式 (16KB - 1MB)
|
||||||
|
```
|
||||||
|
小/中容量 (≤128KB): 每页1KB
|
||||||
|
Page 0: 0x08000000 - 0x080003FF (1KB)
|
||||||
|
Page 1: 0x08000400 - 0x080007FF (1KB)
|
||||||
|
...
|
||||||
|
|
||||||
|
大容量/互联型 (>128KB): 每页2KB
|
||||||
|
Page 0: 0x08000000 - 0x080007FF (2KB)
|
||||||
|
Page 1: 0x08000800 - 0x08000FFF (2KB)
|
||||||
|
...
|
||||||
|
```
|
||||||
|
|
||||||
|
### STM32F4 单Bank模式 (256KB - 1MB)
|
||||||
|
```
|
||||||
|
Sector 0-3: 16KB each (0x08000000 - 0x0800FFFF)
|
||||||
|
Sector 4: 64KB (0x08010000 - 0x0801FFFF)
|
||||||
|
Sector 5-11: 128KB each (0x08020000 - 0x080FFFFF)
|
||||||
|
```
|
||||||
|
|
||||||
|
### STM32F4 双Bank模式 (2MB)
|
||||||
|
```
|
||||||
|
Bank 1:
|
||||||
|
Sector 0-3: 16KB each (0x08000000 - 0x0800FFFF)
|
||||||
|
Sector 4: 64KB (0x08010000 - 0x0801FFFF)
|
||||||
|
Sector 5-11: 128KB each (0x08020000 - 0x080FFFFF)
|
||||||
|
|
||||||
|
Bank 2:
|
||||||
|
Sector 12-15: 16KB each (0x08100000 - 0x0810FFFF)
|
||||||
|
Sector 16: 64KB (0x08110000 - 0x0811FFFF)
|
||||||
|
Sector 17-23: 128KB each (0x08120000 - 0x081FFFFF)
|
||||||
|
```
|
||||||
|
|
||||||
|
### STM32H7 Sector模式
|
||||||
|
```
|
||||||
|
单Bank (1MB):
|
||||||
|
Sector 0-7: 128KB each (0x08000000 - 0x080FFFFF)
|
||||||
|
|
||||||
|
双Bank (2MB):
|
||||||
|
Bank 1:
|
||||||
|
Sector 0-7: 128KB each (0x08000000 - 0x080FFFFF)
|
||||||
|
Bank 2:
|
||||||
|
Sector 8-15: 128KB each (0x08100000 - 0x081FFFFF)
|
||||||
|
```
|
||||||
|
|
||||||
|
## 使用方法
|
||||||
|
|
||||||
|
### 1. 在BSP配置界面启用Flash
|
||||||
|
在代码生成界面的BSP标签中，勾选"生成 Flash 代码"选项。
|
||||||
|
|
||||||
|
### 2. 自动检测
|
||||||
|
系统会自动：
|
||||||
|
- 读取项目中的`.ioc`文件
|
||||||
|
- 提取MCU型号信息
|
||||||
|
- 计算Flash扇区配置
|
||||||
|
- 生成对应的宏定义
|
||||||
|
|
||||||
|
### 3. 生成的代码示例
|
||||||
|
|
||||||
|
**STM32F1系列** (以STM32F103RCT6为例 - 256KB):
|
||||||
|
```c
|
||||||
|
// flash.h
|
||||||
|
#define ADDR_FLASH_PAGE_0 ((uint32_t)0x08000000)
|
||||||
|
/* Base address of Page 0, 2 Kbytes */
|
||||||
|
#define ADDR_FLASH_PAGE_1 ((uint32_t)0x08000800)
|
||||||
|
/* Base address of Page 1, 2 Kbytes */
|
||||||
|
...
|
||||||
|
#define ADDR_FLASH_PAGE_127 ((uint32_t)0x0803F800)
|
||||||
|
/* Base address of Page 127, 2 Kbytes */
|
||||||
|
#define ADDR_FLASH_END ((uint32_t)0x08040000)
|
||||||
|
|
||||||
|
// flash.c
|
||||||
|
#define BSP_FLASH_MAX_PAGE 127
|
||||||
|
if (page >= 0 && page <= 127) {
|
||||||
|
// 擦除代码...
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
**STM32F4系列** (以STM32F407IGH6为例 - 2MB):
|
||||||
|
```c
|
||||||
|
// flash.h
|
||||||
|
#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000)
|
||||||
|
/* Base address of Sector 0, 16 Kbytes */
|
||||||
|
...
|
||||||
|
#define ADDR_FLASH_SECTOR_23 ((uint32_t)0x081E0000)
|
||||||
|
/* Base address of Sector 23, 128 Kbytes */
|
||||||
|
|
||||||
|
#define ADDR_FLASH_END ((uint32_t)0x08200000)
|
||||||
|
/* End address for flash */
|
||||||
|
```
|
||||||
|
|
||||||
|
**flash.c**:
|
||||||
|
```c
|
||||||
|
#define BSP_FLASH_MAX_SECTOR 23
|
||||||
|
|
||||||
|
void BSP_Flash_EraseSector(uint32_t sector) {
|
||||||
|
if (sector > 0 && sector <= 23) {
|
||||||
|
// 擦除代码...
|
||||||
|
}
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
**STM32H7系列** (以STM32H743VIT6为例 - 2MB):
|
||||||
|
```c
|
||||||
|
// flash.h
|
||||||
|
#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000)
|
||||||
|
/* Base address of Sector 0, 128 Kbytes */
|
||||||
|
...
|
||||||
|
#define ADDR_FLASH_SECTOR_15 ((uint32_t)0x081E0000)
|
||||||
|
/* Base address of Sector 15, 128 Kbytes */
|
||||||
|
|
||||||
|
#define ADDR_FLASH_END ((uint32_t)0x08200000)
|
||||||
|
|
||||||
|
// flash.c
|
||||||
|
#define BSP_FLASH_MAX_SECTOR 15
|
||||||
|
if (sector > 0 && sector <= 15) {
|
||||||
|
// 擦除代码...
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## API接口
|
||||||
|
|
||||||
|
### BSP_Flash_EraseSector (F4/H7) / BSP_Flash_ErasePage (F1)
|
||||||
|
擦除指定扇区或页
|
||||||
|
```c
|
||||||
|
// F4/H7系列
|
||||||
|
void BSP_Flash_EraseSector(uint32_t sector);
|
||||||
|
// F1系列
|
||||||
|
void BSP_Flash_ErasePage(uint32_t page);
|
||||||
|
```
|
||||||
|
- **参数**:
|
||||||
|
- sector/page - 扇区号或页号
|
||||||
|
- F1: 0 到 (页数-1)
|
||||||
|
- F4: 0-11 或 0-23（根据芯片型号）
|
||||||
|
- H7: 0-7 或 0-15（根据芯片型号）
|
||||||
|
|
||||||
|
### BSP_Flash_WriteBytes
|
||||||
|
写入数据到Flash
|
||||||
|
```c
|
||||||
|
void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len);
|
||||||
|
```
|
||||||
|
- **参数**:
|
||||||
|
- address - Flash地址
|
||||||
|
- buf - 数据缓冲区
|
||||||
|
- len - 数据长度
|
||||||
|
|
||||||
|
### BSP_Flash_ReadBytes
|
||||||
|
从Flash读取数据
|
||||||
|
```c
|
||||||
|
void BSP_Flash_ReadBytes(uint32_t address, void *buf, size_t len);
|
||||||
|
```
|
||||||
|
- **参数**:
|
||||||
|
- address - Flash地址
|
||||||
|
- buf - 接收缓冲区
|
||||||
|
- len - 读取长度
|
||||||
|
|
||||||
|
## 使用示例
|
||||||
|
|
||||||
|
### STM32F1系列示例
|
||||||
|
```c
|
||||||
|
#include "bsp/flash.h"
|
||||||
|
|
||||||
|
void save_config_f1(void) {
|
||||||
|
// 擦除Page 127 (最后一页，通常用于存储用户数据)
|
||||||
|
BSP_Flash_ErasePage(127);
|
||||||
|
|
||||||
|
// 写入配置数据
|
||||||
|
uint8_t config[100] = {/* 配置数据 */};
|
||||||
|
BSP_Flash_WriteBytes(ADDR_FLASH_PAGE_127, config, sizeof(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
void load_config_f1(void) {
|
||||||
|
// 读取配置数据
|
||||||
|
uint8_t config[100];
|
||||||
|
BSP_Flash_ReadBytes(ADDR_FLASH_PAGE_127, config, sizeof(config));
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
### STM32F4系列示例
|
||||||
|
```c
|
||||||
|
#include "bsp/flash.h"
|
||||||
|
|
||||||
|
void save_config_f4(void) {
|
||||||
|
// 擦除Sector 11 (通常用于存储用户数据)
|
||||||
|
BSP_Flash_EraseSector(11);
|
||||||
|
|
||||||
|
// 写入配置数据
|
||||||
|
uint8_t config[100] = {/* 配置数据 */};
|
||||||
|
BSP_Flash_WriteBytes(ADDR_FLASH_SECTOR_11, config, sizeof(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
void load_config_f4(void) {
|
||||||
|
// 读取配置数据
|
||||||
|
uint8_t config[100];
|
||||||
|
BSP_Flash_ReadBytes(ADDR_FLASH_SECTOR_11, config, sizeof(config));
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
### STM32H7系列示例
|
||||||
|
|
||||||
|
### STM32H7系列示例
|
||||||
|
```c
|
||||||
|
#include "bsp/flash.h"
|
||||||
|
|
||||||
|
void save_config(void) {
|
||||||
|
// 擦除Sector 11 (通常用于存储用户数据)
|
||||||
|
BSP_Flash_EraseSector(11);
|
||||||
|
|
||||||
|
// 写入配置数据
|
||||||
|
uint8_t config[100] = {/* 配置数据 */};
|
||||||
|
BSP_Flash_WriteBytes(ADDR_FLASH_SECTOR_11, config, sizeof(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
void load_config(void) {
|
||||||
|
// 读取配置数据
|
||||||
|
uint8_t config[100];
|
||||||
|
BSP_Flash_ReadBytes(ADDR_FLASH_SECTOR_11, config, sizeof(config));
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## 注意事项
|
||||||
|
|
||||||
|
1. **擦除时间**: Flash擦除需要一定时间，注意不要在中断中执行
|
||||||
|
- F1 Page擦除: ~20ms
|
||||||
|
- F4 Sector擦除: 16KB~100ms, 64KB~300ms, 128KB~500ms
|
||||||
|
- H7 Sector擦除: ~200ms
|
||||||
|
2. **写入前擦除**:
|
||||||
|
- F1: 必须先擦除整页才能写入
|
||||||
|
- F4/H7: 必须先擦除整个扇区才能写入
|
||||||
|
3. **区域选择**: 避免擦除包含程序代码的扇区/页
|
||||||
|
- F1: 通常最后几页用于存储数据
|
||||||
|
- F4: Sector 11 或 23 常用于存储数据
|
||||||
|
- H7: Sector 7 或 15 常用于存储数据
|
||||||
|
4. **写入对齐**: 建议按字节写入，HAL库会处理对齐
|
||||||
|
5. **断电保护**: 写入过程中断电可能导致数据丢失
|
||||||
|
6. **擦写次数限制**:
|
||||||
|
- F1: 典型10,000次
|
||||||
|
- F4/H7: 典型10,000-100,000次
|
||||||
|
|
||||||
|
## 配置文件
|
||||||
|
|
||||||
|
配置信息保存在 `bsp_config.yaml`:
|
||||||
|
|
||||||
|
**STM32F1:**
|
||||||
|
```yaml
|
||||||
|
flash:
|
||||||
|
enabled: true
|
||||||
|
mcu_name: STM32F103RCT6
|
||||||
|
dual_bank: false
|
||||||
|
sectors: 128 # 实际是128个页
|
||||||
|
type: page
|
||||||
|
page_size: 2
|
||||||
|
```
|
||||||
|
|
||||||
|
**STM32F4:**
|
||||||
|
```yaml
|
||||||
|
flash:
|
||||||
|
enabled: true
|
||||||
|
mcu_name: STM32F407IGHx
|
||||||
|
dual_bank: true
|
||||||
|
sectors: 24
|
||||||
|
type: sector
|
||||||
|
```
|
||||||
|
|
||||||
|
**STM32H7:**
|
||||||
|
```yaml
|
||||||
|
flash:
|
||||||
|
enabled: true
|
||||||
|
mcu_name: STM32H743VIT6
|
||||||
|
dual_bank: true
|
||||||
|
sectors: 16
|
||||||
|
type: sector
|
||||||
|
```
|
||||||
|
|
||||||
|
## 扩展支持
|
||||||
|
|
||||||
|
当前支持的系列：
|
||||||
|
- ✅ STM32F1 (Page模式)
|
||||||
|
- ✅ STM32F4 (Sector模式)
|
||||||
|
- ✅ STM32H7 (Sector模式)
|
||||||
|
|
||||||
|
如需支持其他STM32系列（如F2/F3/L4/G4等），可在 `analyzing_ioc.py` 的 `get_flash_config_from_mcu()` 函数中添加相应的配置规则。
|
||||||
55
bsp/flash/flash.c
Normal file
55
bsp/flash/flash.c
Normal file
@ -0,0 +1,55 @@
|
|||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "bsp/flash.h"
|
||||||
|
|
||||||
|
#include <main.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
/* USER CODE BEGIN FLASH_MAX_SECTOR */
|
||||||
|
/* AUTO GENERATED FLASH_MAX_SECTOR */
|
||||||
|
/* USER CODE END FLASH_MAX_SECTOR */
|
||||||
|
|
||||||
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
/* Private function -------------------------------------------------------- */
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
|
||||||
|
void BSP_Flash_EraseSector(uint32_t sector) {
|
||||||
|
FLASH_EraseInitTypeDef flash_erase;
|
||||||
|
uint32_t sector_error;
|
||||||
|
|
||||||
|
/* USER CODE BEGIN FLASH_ERASE_CHECK */
|
||||||
|
/* AUTO GENERATED FLASH_ERASE_CHECK */
|
||||||
|
/* USER CODE END FLASH_ERASE_CHECK */
|
||||||
|
flash_erase.Sector = sector;
|
||||||
|
flash_erase.TypeErase = FLASH_TYPEERASE_SECTORS;
|
||||||
|
flash_erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;
|
||||||
|
flash_erase.NbSectors = 1;
|
||||||
|
|
||||||
|
HAL_FLASH_Unlock();
|
||||||
|
while (FLASH_WaitForLastOperation(50) != HAL_OK)
|
||||||
|
;
|
||||||
|
HAL_FLASHEx_Erase(&flash_erase, §or_error);
|
||||||
|
HAL_FLASH_Lock();
|
||||||
|
}
|
||||||
|
/* USER CODE BEGIN FLASH_ERASE_END */
|
||||||
|
/* USER CODE END FLASH_ERASE_END */
|
||||||
|
}
|
||||||
|
|
||||||
|
void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) {
|
||||||
|
HAL_FLASH_Unlock();
|
||||||
|
while (len > 0) {
|
||||||
|
while (FLASH_WaitForLastOperation(50) != HAL_OK)
|
||||||
|
;
|
||||||
|
HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, address, *buf);
|
||||||
|
address++;
|
||||||
|
buf++;
|
||||||
|
len--;
|
||||||
|
}
|
||||||
|
HAL_FLASH_Lock();
|
||||||
|
}
|
||||||
|
|
||||||
|
void BSP_Flash_ReadBytes(uint32_t address, void *buf, size_t len) {
|
||||||
|
memcpy(buf, (void *)address, len);
|
||||||
|
}
|
||||||
31
bsp/flash/flash.h
Normal file
31
bsp/flash/flash.h
Normal file
@ -0,0 +1,31 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ------------------------------------------------------------------ */
|
||||||
|
#include <main.h>
|
||||||
|
|
||||||
|
#include "bsp/bsp.h"
|
||||||
|
|
||||||
|
/* Exported constants -------------------------------------------------------- */
|
||||||
|
/* Base address of the Flash sectors */
|
||||||
|
/* USER CODE BEGIN FLASH_SECTOR_DEFINES */
|
||||||
|
/* AUTO GENERATED FLASH_SECTORS */
|
||||||
|
/* USER CODE END FLASH_SECTOR_DEFINES */
|
||||||
|
|
||||||
|
/* USER CODE BEGIN FLASH_END_ADDRESS */
|
||||||
|
/* AUTO GENERATED FLASH_END_ADDRESS */
|
||||||
|
/* USER CODE END FLASH_END_ADDRESS */
|
||||||
|
|
||||||
|
/* Exported macro ------------------------------------------------------------ */
|
||||||
|
/* Exported types ------------------------------------------------------------ */
|
||||||
|
/* Exported functions prototypes --------------------------------------------- */
|
||||||
|
void BSP_Flash_EraseSector(uint32_t sector);
|
||||||
|
void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len);
|
||||||
|
void BSP_Flash_ReadBytes(uint32_t address, void *buf, size_t len);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
@ -1,4 +1,4 @@
|
|||||||
bsp,can,dwt,gpio,i2c,mm,spi,uart,pwm,time
|
bsp,can,fdcan,dwt,gpio,i2c,mm,spi,uart,pwm,time,flash
|
||||||
component,ahrs,capacity,cmd,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math
|
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_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,oid,lcd_driver
|
device,dr16,bmi088,ist8310,motor,motor_rm,motor_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,oid,lcd_driver
|
||||||
module,config,
|
module,
|
||||||
|
@ -240,6 +240,22 @@ devices:
|
|||||||
description: "lcd驱动(SPI)"
|
description: "lcd驱动(SPI)"
|
||||||
dependencies:
|
dependencies:
|
||||||
bsp: ["gpio", "spi"]
|
bsp: ["gpio", "spi"]
|
||||||
|
bsp_requirements:
|
||||||
|
- type: "spi"
|
||||||
|
var_name: "BSP_SPI_LCD"
|
||||||
|
description: "用于LCD通信的SPI总线"
|
||||||
|
- type: "gpio"
|
||||||
|
var_name: "BSP_GPIO_LCD_CS"
|
||||||
|
description: "LCD片选引脚"
|
||||||
|
gpio_type: "output"
|
||||||
|
- type: "gpio"
|
||||||
|
var_name: "BSP_GPIO_LCD_DC"
|
||||||
|
description: "LCD数据/命令控制引脚"
|
||||||
|
gpio_type: "output"
|
||||||
|
- type: "gpio"
|
||||||
|
var_name: "BSP_GPIO_LCD_RST"
|
||||||
|
description: "LCD复位引脚"
|
||||||
|
gpio_type: "output"
|
||||||
thread_signals: []
|
thread_signals: []
|
||||||
files:
|
files:
|
||||||
header: "lcd.h"
|
header: "lcd.h"
|
||||||
|
|||||||
@ -13,10 +13,17 @@
|
|||||||
|
|
||||||
/* Exported variables ------------------------------------------------------- */
|
/* Exported variables ------------------------------------------------------- */
|
||||||
|
|
||||||
// 机器人参数配置
|
/**
|
||||||
|
* @brief 机器人参数配置
|
||||||
|
* @note 在此配置机器人参数
|
||||||
|
*/
|
||||||
Config_RobotParam_t robot_config = {
|
Config_RobotParam_t robot_config = {
|
||||||
|
/* USER CODE BEGIN robot_config */
|
||||||
|
.example_param = 0, // 示例参数初始化
|
||||||
|
|
||||||
|
// 在此添加您的配置参数初始化
|
||||||
|
|
||||||
|
/* USER CODE END robot_config */
|
||||||
};
|
};
|
||||||
|
|
||||||
/* Private function prototypes ---------------------------------------------- */
|
/* Private function prototypes ---------------------------------------------- */
|
||||||
|
|||||||
@ -9,9 +9,20 @@ extern "C" {
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 机器人参数配置结构体
|
||||||
|
* @note 在此添加您的配置参数
|
||||||
|
*/
|
||||||
typedef struct {
|
typedef struct {
|
||||||
|
// 示例配置项（可根据实际需求修改或删除）
|
||||||
|
uint8_t example_param; // 示例参数
|
||||||
|
|
||||||
|
/* USER CODE BEGIN Config_RobotParam */
|
||||||
|
// 在此添加您的配置参数
|
||||||
|
|
||||||
|
/* USER CODE END Config_RobotParam */
|
||||||
} Config_RobotParam_t;
|
} Config_RobotParam_t;
|
||||||
|
|
||||||
/* Exported functions prototypes -------------------------------------------- */
|
/* Exported functions prototypes -------------------------------------------- */
|
||||||
|
|||||||
4
module/describe.csv
Normal file
4
module/describe.csv
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
module_name,description
|
||||||
|
cmd,命令系统，用于机器人指令处理和行为控制
|
||||||
|
2_axis_gimbal,双轴云台控制模块，支持pitch和yaw轴控制
|
||||||
|
3+3shoot,双级三摩擦轮发射机构模块
|
||||||
|
658
module/shoot/3+3shoot/shoot.c
Normal file
658
module/shoot/3+3shoot/shoot.c
Normal file
@ -0,0 +1,658 @@
|
|||||||
|
/*
|
||||||
|
* far♂蛇模块
|
||||||
|
*/
|
||||||
|
|
||||||
|
/********************************* 使用示例 **********************************/
|
||||||
|
/*1.配置config参数以及Config_ShootInit函数参数*/
|
||||||
|
/*2.
|
||||||
|
COMP_AT9S_CMD_t shoot_ctrl_cmd_rc;
|
||||||
|
Shoot_t shoot;
|
||||||
|
Shoot_CMD_t shoot_cmd;
|
||||||
|
|
||||||
|
void Task(void *argument) {
|
||||||
|
|
||||||
|
Config_ShootInit();
|
||||||
|
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,SHOOT_CTRL_FREQ);
|
||||||
|
Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE); 关于模式选择：初始化一个模式
|
||||||
|
|
||||||
|
while (1) {
|
||||||
|
|
||||||
|
shoot_cmd.online =shoot_ctrl_cmd_rc.online;
|
||||||
|
shoot_cmd.ready =shoot_ctrl_cmd_rc.shoot.ready;
|
||||||
|
shoot_cmd.firecmd =shoot_ctrl_cmd_rc.shoot.firecmd;
|
||||||
|
|
||||||
|
shoot.mode =shoot_ctrl_cmd_rc.mode; 关于模式选择：或者用遥控器随时切换模式，二选一
|
||||||
|
|
||||||
|
Chassis_UpdateFeedback(&shoot);
|
||||||
|
Shoot_Control(&shoot,&shoot_cmd);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include "shoot.h"
|
||||||
|
#include "bsp/mm.h"
|
||||||
|
#include "bsp/time.h"
|
||||||
|
#include "component/filter.h"
|
||||||
|
#include "component/user_math.h"
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
#define MAX_FRIC_RPM 7000.0f
|
||||||
|
#define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率，待测试
|
||||||
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
static bool last_firecmd;
|
||||||
|
|
||||||
|
float maxTrigrpm=1500.0f;
|
||||||
|
/* Private function -------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 设置射击模式
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param mode 要设置的模式
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
s->mode=mode;
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 重置PID积分
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_ResetIntegral(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
PID_ResetIntegral(&s->pid.fric_follow[i]);
|
||||||
|
PID_ResetIntegral(&s->pid.fric_err[i]);
|
||||||
|
}
|
||||||
|
PID_ResetIntegral(&s->pid.trig);
|
||||||
|
PID_ResetIntegral(&s->pid.trig_omg);
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 重置计算模块
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_ResetCalu(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
PID_Reset(&s->pid.fric_follow[i]);
|
||||||
|
PID_Reset(&s->pid.fric_err[i]);
|
||||||
|
LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
|
||||||
|
LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
|
||||||
|
}
|
||||||
|
PID_Reset(&s->pid.trig);
|
||||||
|
PID_Reset(&s->pid.trig_omg);
|
||||||
|
LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
|
||||||
|
LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 重置输出
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_ResetOutput(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
s->output.out_follow[i]=0.0f;
|
||||||
|
s->output.out_err[i]=0.0f;
|
||||||
|
s->output.out_fric[i]=0.0f;
|
||||||
|
s->output.lpfout_fric[i]=0.0f;
|
||||||
|
}
|
||||||
|
s->output.outagl_trig=0.0f;
|
||||||
|
s->output.outomg_trig=0.0f;
|
||||||
|
s->output.outlpf_trig=0.0f;
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
//float last_angle=0.0f;
|
||||||
|
//float speed=0.0f;
|
||||||
|
//int8_t Shoot_CalufeedbackRPM(Shoot_t *s)
|
||||||
|
//{
|
||||||
|
// if (s == NULL) {
|
||||||
|
// return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
// }
|
||||||
|
//// static
|
||||||
|
// float err;
|
||||||
|
// err=CircleError(s->feedback.fric[0].rotor_abs_angle,last_angle,M_2PI);
|
||||||
|
// speed=err/s->dt/M_2PI*60.0f;
|
||||||
|
// last_angle=s->feedback.fric->rotor_abs_angle;
|
||||||
|
|
||||||
|
|
||||||
|
// return SHOOT_OK;
|
||||||
|
//}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 根据目标弹丸速度计算摩擦轮目标转速
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param target_speed 目标弹丸速度，单位m/s
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
switch(s->param->basic.projectileType)
|
||||||
|
{
|
||||||
|
case SHOOT_PROJECTILE_17MM:
|
||||||
|
s->target_variable.fric_rpm=5000.0f;
|
||||||
|
break;
|
||||||
|
case SHOOT_PROJECTILE_42MM:
|
||||||
|
s->target_variable.fric_rpm=4000.0f;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL || s->var_trig.num_toShoot == 0) {
|
||||||
|
return SHOOT_ERR_NULL;
|
||||||
|
}
|
||||||
|
float dt = s->timer.now - s->var_trig.time_lastShoot;
|
||||||
|
float dpos;
|
||||||
|
dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI);
|
||||||
|
if(dt >= 1.0f/s->param->basic.shot_freq && cmd->firecmd && dpos<=1.0f)
|
||||||
|
{
|
||||||
|
s->var_trig.time_lastShoot=s->timer.now;
|
||||||
|
CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI);
|
||||||
|
s->var_trig.num_toShoot--;
|
||||||
|
}
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static float Shoot_CaluCoupledWeight(Shoot_t *s, uint8_t fric_index)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
|
||||||
|
float Threshold;
|
||||||
|
switch (s->param->basic.projectileType) {
|
||||||
|
case SHOOT_PROJECTILE_17MM:
|
||||||
|
Threshold=50.0f;
|
||||||
|
break;
|
||||||
|
case SHOOT_PROJECTILE_42MM:
|
||||||
|
Threshold=400.0f;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
float err;
|
||||||
|
err=fabs((s->param->basic.ratio_multilevel[fric_index]
|
||||||
|
*s->target_variable.fric_rpm)
|
||||||
|
-s->feedback.fric[fric_index].rotor_speed);
|
||||||
|
if (err<Threshold)
|
||||||
|
{
|
||||||
|
s->var_fric.coupled_control_weights=1.0f-(err*err)/(Threshold*Threshold);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
s->var_fric.coupled_control_weights=0.0f;
|
||||||
|
}
|
||||||
|
return s->var_fric.coupled_control_weights;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 更新射击模块的电机反馈信息
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_UpdateFeedback(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i = 0; i < fric_num; i++) {
|
||||||
|
/* 更新摩擦轮电机反馈 */
|
||||||
|
MOTOR_RM_Update(&s->param->motor.fric[i].param);
|
||||||
|
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->motor.fric[i].param);
|
||||||
|
if(motor_fed!=NULL)
|
||||||
|
{
|
||||||
|
s->feedback.fric[i]=motor_fed->motor.feedback;
|
||||||
|
}
|
||||||
|
/* 滤波摩擦轮电机转速反馈 */
|
||||||
|
s->var_fric.fil_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
|
||||||
|
/* 归一化摩擦轮电机转速反馈 */
|
||||||
|
s->var_fric.normalized_fil_rpm[i] = s->var_fric.fil_rpm[i] / MAX_FRIC_RPM;
|
||||||
|
if(s->var_fric.normalized_fil_rpm[i]>1.0f)s->var_fric.normalized_fil_rpm[i]=1.0f;
|
||||||
|
if(s->var_fric.normalized_fil_rpm[i]<-1.0f)s->var_fric.normalized_fil_rpm[i]=-1.0f;
|
||||||
|
/* 计算平均摩擦轮电机转速反馈 */
|
||||||
|
s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1]+=s->var_fric.normalized_fil_rpm[i];
|
||||||
|
}
|
||||||
|
for (int i=1; i<MAX_NUM_MULTILEVEL; i++)
|
||||||
|
{
|
||||||
|
s->var_fric.normalized_fil_avgrpm[i]=s->var_fric.normalized_fil_avgrpm[i]/fric_num/MAX_NUM_MULTILEVEL;
|
||||||
|
}
|
||||||
|
/* 更新拨弹电机反馈 */
|
||||||
|
MOTOR_RM_Update(&s->param->motor.trig);
|
||||||
|
s->feedback.trig = *MOTOR_RM_GetMotor(&s->param->motor.trig);
|
||||||
|
s->var_trig.trig_agl=s->param->basic.extra_deceleration_ratio*s->feedback.trig.gearbox_total_angle;
|
||||||
|
while(s->var_trig.trig_agl<0)s->var_trig.trig_agl+=M_2PI;
|
||||||
|
while(s->var_trig.trig_agl>=M_2PI)s->var_trig.trig_agl-=M_2PI;
|
||||||
|
if (s->feedback.trig.motor.reverse) {
|
||||||
|
s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl;
|
||||||
|
}
|
||||||
|
s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed);
|
||||||
|
s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / maxTrigrpm;
|
||||||
|
if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f;
|
||||||
|
if(s->var_trig.trig_rpm<-1.0f)s->var_trig.trig_rpm=-1.0f;
|
||||||
|
|
||||||
|
s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 射击模块运行状态机
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/float a;
|
||||||
|
int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL || cmd == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
static float pos;
|
||||||
|
if(s->mode==SHOOT_MODE_SAFE){
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
MOTOR_RM_Relax(&s->param->motor.fric[i].param);
|
||||||
|
}
|
||||||
|
MOTOR_RM_Relax(&s->param->motor.trig);\
|
||||||
|
pos=s->target_variable.trig_angle=s->var_trig.trig_agl;
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
switch(s->running_state)
|
||||||
|
{
|
||||||
|
case SHOOT_STATE_IDLE:/*熄火等待*/
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{ /* 转速归零 */
|
||||||
|
PID_ResetIntegral(&s->pid.fric_follow[i]);
|
||||||
|
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->var_fric.normalized_fil_rpm[i],0,s->timer.dt);
|
||||||
|
s->output.out_fric[i]=s->output.out_follow[i];
|
||||||
|
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt);
|
||||||
|
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt);
|
||||||
|
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
|
||||||
|
|
||||||
|
/* 检查状态机 */
|
||||||
|
if(cmd->ready)
|
||||||
|
{
|
||||||
|
Shoot_ResetCalu(s);
|
||||||
|
Shoot_ResetIntegral(s);
|
||||||
|
Shoot_ResetOutput(s);
|
||||||
|
s->running_state=SHOOT_STATE_READY;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SHOOT_STATE_READY:/*准备射击*/
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
uint8_t level=s->param->motor.fric[i].level-1;
|
||||||
|
float target_rpm=s->param->basic.ratio_multilevel[level]
|
||||||
|
*s->target_variable.fric_rpm/MAX_FRIC_RPM;
|
||||||
|
/* 计算耦合控制权重 */
|
||||||
|
float w=Shoot_CaluCoupledWeight(s,i);
|
||||||
|
/* 计算跟随输出、计算修正输出 */
|
||||||
|
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
|
||||||
|
target_rpm,
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.out_err[i]=w*PID_Calc(&s->pid.fric_err[i],
|
||||||
|
s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1],
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
/* 按比例缩放并加和输出 */
|
||||||
|
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
|
||||||
|
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
|
||||||
|
/* 滤波 */
|
||||||
|
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
||||||
|
/* 设置输出 */
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
|
||||||
|
}
|
||||||
|
/* 设置拨弹电机输出 */
|
||||||
|
s->output.outagl_trig =PID_Calc(&s->pid.trig,
|
||||||
|
pos,
|
||||||
|
s->var_trig.trig_agl,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
|
||||||
|
s->output.outagl_trig,
|
||||||
|
s->var_trig.trig_rpm,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
|
||||||
|
|
||||||
|
/* 检查状态机 */
|
||||||
|
if(!cmd->ready)
|
||||||
|
{
|
||||||
|
Shoot_ResetCalu(s);
|
||||||
|
Shoot_ResetOutput(s);
|
||||||
|
s->running_state=SHOOT_STATE_IDLE;
|
||||||
|
}
|
||||||
|
else if(last_firecmd==false&&cmd->firecmd==true)
|
||||||
|
{
|
||||||
|
s->running_state=SHOOT_STATE_FIRE;
|
||||||
|
/* 根据模式设置待发射弹数 */
|
||||||
|
switch(s->mode)
|
||||||
|
{
|
||||||
|
case SHOOT_MODE_SINGLE:
|
||||||
|
s->var_trig.num_toShoot=1;
|
||||||
|
break;
|
||||||
|
case SHOOT_MODE_BURST:
|
||||||
|
s->var_trig.num_toShoot=s->param->basic.shot_burst_num;
|
||||||
|
break;
|
||||||
|
case SHOOT_MODE_CONTINUE:
|
||||||
|
s->var_trig.num_toShoot=6666;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
s->var_trig.num_toShoot=0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SHOOT_STATE_FIRE:/*射击*/
|
||||||
|
Shoot_CaluTargetAngle(s, cmd);
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
uint8_t level=s->param->motor.fric[i].level-1;
|
||||||
|
float target_rpm=s->param->basic.ratio_multilevel[level]
|
||||||
|
*s->target_variable.fric_rpm/MAX_FRIC_RPM;
|
||||||
|
/* 计算耦合控制权重 */
|
||||||
|
float w=Shoot_CaluCoupledWeight(s,i);
|
||||||
|
/* 计算跟随输出、计算修正输出 */
|
||||||
|
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
|
||||||
|
target_rpm,
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.out_err[i]=w*PID_Calc(&s->pid.fric_err[i],
|
||||||
|
s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1],
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
/* 按比例缩放并加和输出 */
|
||||||
|
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
|
||||||
|
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
|
||||||
|
/* 滤波 */
|
||||||
|
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
||||||
|
/* 设置输出 */
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
|
||||||
|
}
|
||||||
|
/* 设置拨弹电机输出 */
|
||||||
|
s->output.outagl_trig =PID_Calc(&s->pid.trig,
|
||||||
|
s->target_variable.trig_angle,
|
||||||
|
s->var_trig.trig_agl,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
|
||||||
|
s->output.outagl_trig,
|
||||||
|
s->var_trig.trig_rpm,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
|
||||||
|
|
||||||
|
/* 检查状态机 */
|
||||||
|
if(!cmd->firecmd)
|
||||||
|
{
|
||||||
|
s->running_state=SHOOT_STATE_READY;
|
||||||
|
pos=s->var_trig.trig_agl;
|
||||||
|
s->var_trig.num_toShoot=0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
s->running_state=SHOOT_STATE_IDLE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/* 输出 */
|
||||||
|
MOTOR_RM_Ctrl(&s->param->motor.fric[0].param);
|
||||||
|
if(s->param->basic.fric_num>4)
|
||||||
|
{
|
||||||
|
MOTOR_RM_Ctrl(&s->param->motor.fric[4].param);
|
||||||
|
}
|
||||||
|
MOTOR_RM_Ctrl(&s->param->motor.trig);
|
||||||
|
last_firecmd = cmd->firecmd;
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 射击模块堵塞检测状态机
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
if(s->param->jamDetection.enable){
|
||||||
|
switch (s->jamdetection.fsmState) {
|
||||||
|
case SHOOT_JAMFSM_STATE_NORMAL:/* 正常运行 */
|
||||||
|
/* 检测电流是否超过阈值 */
|
||||||
|
if (s->feedback.trig.feedback.torque_current/1000.0f > s->param->jamDetection.threshold) {
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_SUSPECTED;
|
||||||
|
s->jamdetection.lastTime = s->timer.now; /* 记录怀疑开始时间 */
|
||||||
|
}
|
||||||
|
/* 正常运行射击状态机 */
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
break;
|
||||||
|
case SHOOT_JAMFSM_STATE_SUSPECTED:/* 怀疑堵塞 */
|
||||||
|
/* 检测电流是否低于阈值 */
|
||||||
|
if (s->feedback.trig.feedback.torque_current/1000.0f < s->param->jamDetection.threshold) {
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
/* 检测高阈值状态是否超过设定怀疑时间 */
|
||||||
|
else if ((s->timer.now - s->jamdetection.lastTime) >= s->param->jamDetection.suspectedTime) {
|
||||||
|
s->jamdetection.detected =true;
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_CONFIRMED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
/* 正常运行射击状态机 */
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
break;
|
||||||
|
case SHOOT_JAMFSM_STATE_CONFIRMED:/* 确认堵塞 */
|
||||||
|
/* 清空待发射弹 */
|
||||||
|
s->var_trig.num_toShoot=0;
|
||||||
|
/* 修改拨弹盘目标角度 */
|
||||||
|
s->target_variable.trig_angle = s->var_trig.trig_agl-(M_2PI/s->param->basic.num_trig_tooth);
|
||||||
|
/* 切换状态 */
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_DEAL;
|
||||||
|
/* 记录处理开始时间 */
|
||||||
|
s->jamdetection.lastTime = s->timer.now;
|
||||||
|
case SHOOT_JAMFSM_STATE_DEAL:/* 堵塞处理 */
|
||||||
|
/* 正常运行射击状态机 */
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
/* 给予0.3秒响应时间并检测电流小于20A，认为堵塞已解除 */
|
||||||
|
if ((s->timer.now - s->jamdetection.lastTime)>=0.3f&&s->feedback.trig.feedback.torque_current/1000.0f < 20.0f) {
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
s->jamdetection.detected = false;
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
}
|
||||||
|
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
/**
|
||||||
|
* \brief 初始化射击模块
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param param 包含射击参数的结构体
|
||||||
|
* \param target_freq 控制循环频率，单位Hz
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
||||||
|
{
|
||||||
|
if (s == NULL || param == NULL || target_freq <= 0.0f) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = param->basic.fric_num;
|
||||||
|
s->param=param;
|
||||||
|
BSP_CAN_Init();
|
||||||
|
/* 初始化摩擦轮PID和滤波器 */
|
||||||
|
for(int i=0;i<fric_num;i++){
|
||||||
|
MOTOR_RM_Register(¶m->motor.fric[i].param);
|
||||||
|
PID_Init(&s->pid.fric_follow[i],
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.fric_follow);
|
||||||
|
PID_Init(&s->pid.fric_err[i],
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.fric_err);
|
||||||
|
LowPassFilter2p_Init(&s->filter.fric.in[i],
|
||||||
|
target_freq,
|
||||||
|
s->param->filter.fric.in);
|
||||||
|
LowPassFilter2p_Init(&s->filter.fric.out[i],
|
||||||
|
target_freq,
|
||||||
|
s->param->filter.fric.out);
|
||||||
|
}
|
||||||
|
/* 初始化拨弹PID和滤波器 */
|
||||||
|
MOTOR_RM_Register(¶m->motor.trig);
|
||||||
|
switch(s->param->motor.trig.module)
|
||||||
|
{
|
||||||
|
case MOTOR_M3508:
|
||||||
|
PID_Init(&s->pid.trig,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_3508);
|
||||||
|
PID_Init(&s->pid.trig_omg,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_omg_3508);
|
||||||
|
break;
|
||||||
|
case MOTOR_M2006:
|
||||||
|
PID_Init(&s->pid.trig,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_2006);
|
||||||
|
PID_Init(&s->pid.trig_omg,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_omg_2006);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return SHOOT_ERR_MOTOR;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
LowPassFilter2p_Init(&s->filter.trig.in,
|
||||||
|
target_freq,
|
||||||
|
s->param->filter.trig.in);
|
||||||
|
LowPassFilter2p_Init(&s->filter.trig.out,
|
||||||
|
target_freq,
|
||||||
|
s->param->filter.trig.out);
|
||||||
|
|
||||||
|
/* 归零变量 */
|
||||||
|
memset(&s->var_trig,0,sizeof(s->var_trig));
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 射击模块控制主函数
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL || cmd == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
s->timer.now = BSP_TIME_Get_us() / 1000000.0f;
|
||||||
|
s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f;
|
||||||
|
s->timer.lask_wakeup = BSP_TIME_Get_us();
|
||||||
|
Shoot_CaluTargetRPM(s,233);
|
||||||
|
|
||||||
|
Shoot_JamDetectionFSM(s, cmd);
|
||||||
|
// Shoot_CalufeedbackRPM(s);
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
243
module/shoot/3+3shoot/shoot.h
Normal file
243
module/shoot/3+3shoot/shoot.h
Normal file
@ -0,0 +1,243 @@
|
|||||||
|
/*
|
||||||
|
* far♂蛇模块
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stddef.h>
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include "main.h"
|
||||||
|
#include "component/pid.h"
|
||||||
|
#include "device/motor_rm.h"
|
||||||
|
/* Exported constants ------------------------------------------------------- */
|
||||||
|
#define MAX_FRIC_NUM 6
|
||||||
|
#define MAX_NUM_MULTILEVEL 2 /* 多级发射级数 */
|
||||||
|
|
||||||
|
#define SHOOT_OK (0) /* 运行正常 */
|
||||||
|
#define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */
|
||||||
|
#define SHOOT_ERR_ERR (-2) /* 运行时发现了其他错误 */
|
||||||
|
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的Mode */
|
||||||
|
#define SHOOT_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
|
||||||
|
#define SHOOT_ERR_MALLOC (-5) /* 内存分配失败 */
|
||||||
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_JAMFSM_STATE_NORMAL = 0,/* 常规状态 */
|
||||||
|
SHOOT_JAMFSM_STATE_SUSPECTED, /* 怀疑状态 */
|
||||||
|
SHOOT_JAMFSM_STATE_CONFIRMED, /* 确认状态 */
|
||||||
|
SHOOT_JAMFSM_STATE_DEAL /* 处理状态 */
|
||||||
|
}Shoot_JamDetectionFSM_State_t;
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_STATE_IDLE = 0,/* 熄火 */
|
||||||
|
SHOOT_STATE_READY, /* 准备射击 */
|
||||||
|
SHOOT_STATE_FIRE /* 射击 */
|
||||||
|
}Shoot_Running_State_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_MODE_SAFE = 0,/* 安全模式 */
|
||||||
|
SHOOT_MODE_SINGLE, /* 单发模式 */
|
||||||
|
SHOOT_MODE_BURST, /* 多发模式 */
|
||||||
|
SHOOT_MODE_CONTINUE,/* 连发模式 */
|
||||||
|
SHOOT_MODE_NUM
|
||||||
|
}Shoot_Mode_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_PROJECTILE_17MM,
|
||||||
|
SHOOT_PROJECTILE_42MM,
|
||||||
|
}Shoot_Projectile_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
MOTOR_RM_Param_t param;
|
||||||
|
uint8_t level; /* 电机属于第几级发射；1起始 */
|
||||||
|
}Shoot_MOTOR_RM_Param_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
MOTOR_Feedback_t fric[MAX_FRIC_NUM];/* 摩擦轮电机反馈 */
|
||||||
|
MOTOR_RM_t trig; /* 拨弹电机反馈 */
|
||||||
|
|
||||||
|
}Shoot_Feedback_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
float fil_rpm[MAX_FRIC_NUM]; /* 滤波后的摩擦轮原始转速 */
|
||||||
|
float normalized_fil_rpm[MAX_FRIC_NUM]; /* 归一化摩擦轮转速 */
|
||||||
|
float normalized_fil_avgrpm[MAX_NUM_MULTILEVEL]; /* 归一化摩擦轮平均转速 */
|
||||||
|
float coupled_control_weights; /* 耦合控制权重 */
|
||||||
|
}Shoot_VARSForFricCtrl_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
float time_lastShoot;/* 上次射击时间 */
|
||||||
|
uint16_t num_toShoot;/* 剩余待发射弹数 */
|
||||||
|
uint16_t num_shooted;/* 已发射弹数 */
|
||||||
|
|
||||||
|
float trig_agl; /*计算所有减速比后的拨弹盘的角度*/
|
||||||
|
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
|
||||||
|
float trig_rpm; /* 归一化拨弹电机转速 */
|
||||||
|
}Shoot_VARSForTrigCtrl_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
bool detected; /* 卡弹检测结果 */
|
||||||
|
float lastTime;/* 用于记录怀疑状态或处理状态的开始时间 */
|
||||||
|
Shoot_JamDetectionFSM_State_t fsmState; /* 卡弹检测状态机 */
|
||||||
|
}Shoot_JamDetection_t;
|
||||||
|
typedef struct {
|
||||||
|
float out_follow[MAX_FRIC_NUM];
|
||||||
|
float out_err[MAX_FRIC_NUM];
|
||||||
|
float out_fric[MAX_FRIC_NUM];
|
||||||
|
float lpfout_fric[MAX_FRIC_NUM];
|
||||||
|
|
||||||
|
float outagl_trig;
|
||||||
|
float outomg_trig;
|
||||||
|
float outlpf_trig;
|
||||||
|
}Shoot_Output_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
Shoot_Mode_t mode;/* 射击模式 */
|
||||||
|
bool ready; /* 准备射击 */
|
||||||
|
bool firecmd; /* 射击 */
|
||||||
|
}Shoot_CMD_t;
|
||||||
|
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
||||||
|
typedef struct {
|
||||||
|
struct{
|
||||||
|
Shoot_Projectile_t projectileType; /* 发射弹丸类型 */;
|
||||||
|
size_t fric_num; /* 摩擦轮电机数量 */
|
||||||
|
float ratio_multilevel[MAX_NUM_MULTILEVEL]; /* 多级发射各级速度比例 */
|
||||||
|
float extra_deceleration_ratio; /*电机出轴到拨盘的额外减速比；没有写1*/
|
||||||
|
size_t num_trig_tooth; /* 拨弹盘每圈弹丸数量 */
|
||||||
|
float shot_freq; /* 射击频率，单位Hz */
|
||||||
|
size_t shot_burst_num; /* 多发模式一次射击的数量 */
|
||||||
|
}basic;/* 发射基础参数 */
|
||||||
|
struct {
|
||||||
|
bool enable; /* 是否启用卡弹检测 */
|
||||||
|
float threshold; /* 卡弹检测阈值，单位A (dji2006建议设置为120A，dji3508建议设置为235A,根据实际测试调整)*/
|
||||||
|
float suspectedTime;/* 卡弹怀疑时间，单位秒 */
|
||||||
|
}jamDetection;/* 卡弹检测参数 */
|
||||||
|
struct {
|
||||||
|
Shoot_MOTOR_RM_Param_t fric[MAX_FRIC_NUM];
|
||||||
|
MOTOR_RM_Param_t trig;
|
||||||
|
}motor; /* 电机参数 */
|
||||||
|
struct{
|
||||||
|
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
|
||||||
|
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
|
||||||
|
KPID_Params_t trig_2006; /* 拨弹电机PID控制参数 */
|
||||||
|
KPID_Params_t trig_omg_2006;/* 拨弹电机PID控制参数 */
|
||||||
|
KPID_Params_t trig_3508; /* 拨弹电机PID控制参数 */
|
||||||
|
KPID_Params_t trig_omg_3508;/* 拨弹电机PID控制参数 */
|
||||||
|
}pid; /* PID参数 */
|
||||||
|
struct {
|
||||||
|
struct{
|
||||||
|
float in; /* 反馈值滤波器截止频率 */
|
||||||
|
float out; /* 输出值滤波器截止频率 */
|
||||||
|
}fric;
|
||||||
|
struct{
|
||||||
|
float in; /* 反馈值滤波器截止频率 */
|
||||||
|
float out; /* 输出值滤波器截止频率 */
|
||||||
|
}trig;
|
||||||
|
} filter;/* 滤波器截止频率参数 */
|
||||||
|
} Shoot_Params_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
float now; /* 当前时间，单位秒 */
|
||||||
|
uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */
|
||||||
|
float dt; /* 两次唤醒间隔时间，单位秒 */
|
||||||
|
}Shoot_Timer_t;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
|
||||||
|
* 包含了初始化参数，中间变量，输出变量
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
Shoot_Timer_t timer; /* 计时器 */
|
||||||
|
Shoot_Params_t *param; /* 发射参数 */
|
||||||
|
/* 模块通用 */
|
||||||
|
Shoot_Mode_t mode; /* 射击模式 */
|
||||||
|
/* 反馈信息 */
|
||||||
|
Shoot_Feedback_t feedback;
|
||||||
|
/* 控制信息*/
|
||||||
|
Shoot_Running_State_t running_state; /* 运行状态机 */
|
||||||
|
Shoot_JamDetection_t jamdetection; /* 卡弹检测控制信息 */
|
||||||
|
Shoot_VARSForFricCtrl_t var_fric; /* 摩擦轮控制信息 */
|
||||||
|
Shoot_VARSForTrigCtrl_t var_trig; /* 角度计算控制信息 */
|
||||||
|
Shoot_Output_t output; /* 输出信息 */
|
||||||
|
/* 目标控制量 */
|
||||||
|
struct {
|
||||||
|
float fric_rpm; /* 目标摩擦轮转速 */
|
||||||
|
float trig_angle;/* 目标拨弹位置 */
|
||||||
|
}target_variable;
|
||||||
|
|
||||||
|
/* 反馈控制用的PID */
|
||||||
|
struct {
|
||||||
|
KPID_t fric_follow[MAX_FRIC_NUM];/* 摩擦轮PID主结构体 */
|
||||||
|
KPID_t fric_err[MAX_FRIC_NUM]; /* 摩擦轮PID主结构体 */
|
||||||
|
KPID_t trig; /* 拨弹PID主结构体 */
|
||||||
|
KPID_t trig_omg; /* 拨弹PID主结构体 */
|
||||||
|
} pid;
|
||||||
|
|
||||||
|
/* 滤波器 */
|
||||||
|
struct {
|
||||||
|
struct{
|
||||||
|
LowPassFilter2p_t in[MAX_FRIC_NUM]; /* 反馈值滤波器 */
|
||||||
|
LowPassFilter2p_t out[MAX_FRIC_NUM];/* 输出值滤波器 */
|
||||||
|
}fric;
|
||||||
|
struct{
|
||||||
|
LowPassFilter2p_t in; /* 反馈值滤波器 */
|
||||||
|
LowPassFilter2p_t out;/* 输出值滤波器 */
|
||||||
|
}trig;
|
||||||
|
} filter;
|
||||||
|
|
||||||
|
float errtosee; /*调试用*/
|
||||||
|
|
||||||
|
} Shoot_t;
|
||||||
|
|
||||||
|
/* Exported functions prototypes -------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 初始化发射
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param param 包含发射参数的结构体指针
|
||||||
|
* \param target_freq 任务预期的运行频率
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 设置发射模式
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param mode 包含发射模式的枚举
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 更新反馈
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_UpdateFeedback(Shoot_t *s);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 初始化发射
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param cmd 包含发射命令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user