Falling_jasmine/FDR-Core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-03-18 22:03:16 +08:00
parent 6b4a459fef
commit 1eaf422130
8 changed files with 90 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Core/Inc/f4_can_app.h
View File

@@ -21,7 +21,7 @@ typedef enum
{
SYSTEM_BOOTING = 0, /* 上电启动，尚未收到第一帧合法速度命令。 */
SYSTEM_OPERATIONAL, /* 正常工作，允许按目标速度运动。 */
SYSTEM_SAFE_FAULT /* 安全保护：目标速度已被强制清零。 */
SYSTEM_SAFE_FAULT /* 安全保护：目标速度强制清零，控制器内部状态清零，并禁止继续驱动。 */
} System_State_t;
/* ================== 健康等级定义 ================== */
@@ -101,13 +101,13 @@ extern volatile Robot_Control_t g_robot_ctrl;
* Byte6 : cmd_age_10ms
* Byte7 : status rolling counter
*
* Tx: 0x182 实际轮速帧（20ms）
* Tx: 0x182 实际轮速帧（轮询，约 60ms）
* Byte0~1 : FL 实际 RPM, int16 little-endian
* Byte2~3 : RL 实际 RPM
* Byte4~5 : FR 实际 RPM
* Byte6~7 : RR 实际 RPM
*
* Tx: 0x183 目标轮速帧（20ms）
* Tx: 0x183 目标轮速帧（轮询，约 60ms）
* Byte0~1 : FL 目标 RPM, int16 little-endian
* Byte2~3 : RL 目标 RPM
* Byte4~5 : FR 目标 RPM
@@ -123,7 +123,7 @@ extern volatile Robot_Control_t g_robot_ctrl;
* Byte6 : last accepted rolling counter
* Byte7 : [7:4] 连续 counter 错误计数(饱和到15), [3:0] 连续 CRC 错误计数(饱和到15)
*
* Tx: 0x200 里程计增量帧（20ms）
* Tx: 0x200 里程计增量帧（轮询，约 60ms）
* Byte0~1 : FL delta ticks, int16 little-endian
* Byte2~3 : RL delta ticks
* Byte4~5 : FR delta ticks
@@ -151,7 +151,7 @@ void CAN_Safety_Watchdog_Tick(void);
/*
* 10ms 周期调用：
* - 从最近一次完整一致的目标命令快照进行差速解算
* - 在 SAFE_FAULT / BOOTING 下强制目标轮速为 0
* - 在 SAFE_FAULT / BOOTING 下强制目标轮速为 0，并禁止继续输出驱动
*/
void Kinematics_Update_LADRC(void);

7
Core/Inc/ladrc.h
View File

@@ -43,10 +43,13 @@ void FourWheel_LADRC_Init(void);
/* 2. 设定目标转速：给四个轮子下发指令 */
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm);
/* 3. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 */
/* 3. 故障/停机时立即清零控制器内部状态并断驱 */
void FourWheel_LADRC_ResetAll(void);
/* 4. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 */
void FourWheel_LADRC_Control_Loop(void);
/* 4. 诊断/状态上报接口：读取每个轮子的目标转速和控制输出 */
/* 5. 诊断/状态上报接口：读取每个轮子的目标转速和控制输出 */
float FourWheel_Get_Target_RPM(Motor_ID_t id);
float FourWheel_Get_Control_Output(Motor_ID_t id);

4
Core/Src/can.c
View File

@@ -84,9 +84,9 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* CAN1 interrupt Init */
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0);
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 1, 0);
HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 2, 0);
HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);
/* USER CODE BEGIN CAN1_MspInit 1 */

55
Core/Src/f4_can_app.c
View File

@@ -34,6 +34,7 @@ volatile uint32_t g_dbg_valid_accept = 0U;
#define CMD_TIMEOUT_MS 150U
#define MAX_ACCEPTED_COUNTER_GAP 3U
#define TELEMETRY_COMM_DIAG_DIV 5U /* 20ms * 5 = 100ms */
#define CAN_RX_ISR_MAX_FRAMES_PER_CALL 2U
/* ================= 通信故障判定参数 ================= */
#define CRC_STORM_THRESHOLD 5U
@@ -63,8 +64,9 @@ volatile Robot_Control_t g_robot_ctrl = {
* 只用“合法且新鲜”的 0x100 控制帧喂运动看门狗。
* 这样即使上位机还在发心跳，但速度命令停了，底盘也会在超时后安全停车。
*/
static volatile uint16_t s_cmd_watchdog_ms = 0U;
static volatile uint32_t s_last_valid_cmd_tick_ms = 0U;
static volatile uint8_t s_cmd_age_10ms = 0U;
static volatile bool s_have_seen_valid_cmd = false;
/* 最近一次接受的 rolling counter，同步状态用于首帧/故障恢复。 */
static volatile uint8_t s_last_rx_counter = 0U;
@@ -300,11 +302,12 @@ static void CAN_EnterSafeFault(uint32_t reason_bits, bool desync_counter)
if (desync_counter)
{
s_cmd_watchdog_ms = 0U;
s_counter_synced = false;
}
__set_PRIMASK(primask);
FourWheel_LADRC_ResetAll();
}
static System_Health_t CAN_GetCurrentHealth(uint32_t diag_bits, System_State_t state)
@@ -354,6 +357,7 @@ static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc)
if (CAN_CRC8_SAE_J1850(rx_data, 7U) != rx_data[7])
{
CAN_AtomicIncU32(&s_crc_error_total);
s_consecutive_counter_errors = 0U;
CAN_AtomicIncU8Saturated(&s_consecutive_crc_errors, 0xFFU);
if (s_consecutive_crc_errors >= CRC_STORM_THRESHOLD)
@@ -383,6 +387,7 @@ static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc)
{
g_dbg_cnt_reject++;
CAN_AtomicIncU32(&s_counter_reject_total);
s_consecutive_crc_errors = 0U;
CAN_AtomicIncU8Saturated(&s_consecutive_counter_errors, 0xFFU);
if (s_consecutive_counter_errors >= COUNTER_STORM_THRESHOLD)
@@ -407,7 +412,8 @@ static void CAN_ProcessControlFrame(const uint8_t *rx_data, uint8_t dlc)
s_last_rx_counter = rx_cnt;
s_counter_synced = true;
s_cmd_watchdog_ms = CMD_TIMEOUT_MS;
s_last_valid_cmd_tick_ms = HAL_GetTick();
s_have_seen_valid_cmd = true;
s_cmd_age_10ms = 0U;
/* 合法新帧到来后，允许通信相关与执行器相关的自动恢复。 */
@@ -453,8 +459,9 @@ void CAN_App_Init(void)
g_robot_ctrl.rolling_cnt = 0U;
g_robot_ctrl.state = SYSTEM_BOOTING;
s_cmd_watchdog_ms = 0U;
s_last_valid_cmd_tick_ms = 0U;
s_cmd_age_10ms = 0U;
s_have_seen_valid_cmd = false;
s_last_rx_counter = 0U;
s_counter_synced = false;
@@ -493,12 +500,15 @@ void CAN_App_Init(void)
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
uint8_t frames_processed = 0U;
if (hcan != &hcan1)
{
return;
}
while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0U)
while ((frames_processed < CAN_RX_ISR_MAX_FRAMES_PER_CALL) &&
(HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0U))
{
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8];
@@ -508,6 +518,8 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
break;
}
frames_processed++;
if ((rx_header.IDE != CAN_ID_STD) || (rx_header.RTR != CAN_RTR_DATA))
{
continue;
@@ -560,34 +572,29 @@ void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan)
void CAN_Safety_Watchdog_Tick(void)
{
uint32_t primask;
uint32_t elapsed_ms = 0U;
uint32_t age_ticks = 0U;
if (s_cmd_age_10ms < 0xFFU)
if (!s_have_seen_valid_cmd)
{
primask = __get_PRIMASK();
__disable_irq();
if (s_cmd_age_10ms < 0xFFU)
{
s_cmd_age_10ms = (uint8_t)(s_cmd_age_10ms + 1U);
}
__set_PRIMASK(primask);
s_cmd_age_10ms = 0U;
return;
}
if (s_cmd_watchdog_ms > CONTROL_LOOP_PERIOD_MS)
elapsed_ms = HAL_GetTick() - s_last_valid_cmd_tick_ms;
age_ticks = elapsed_ms / CONTROL_LOOP_PERIOD_MS;
if (age_ticks > 0xFFU)
{
primask = __get_PRIMASK();
__disable_irq();
s_cmd_watchdog_ms = (uint16_t)(s_cmd_watchdog_ms - CONTROL_LOOP_PERIOD_MS);
__set_PRIMASK(primask);
age_ticks = 0xFFU;
}
else if (s_cmd_watchdog_ms > 0U)
s_cmd_age_10ms = (uint8_t)age_ticks;
if ((g_robot_ctrl.state == SYSTEM_OPERATIONAL) && (elapsed_ms >= CMD_TIMEOUT_MS))
{
CAN_EnterSafeFault(DIAG_COMM_TIMEOUT, true);
}
else
{
/* BOOTING / SAFE_FAULT 期间不重复处理。 */
}
}
void Kinematics_Update_LADRC(void)

45
Core/Src/ladrc.c
View File

@@ -1,4 +1,5 @@
#include "ladrc.h"
#include "f4_can_app.h"
/*
* 这个文件实现两层内容：
@@ -20,6 +21,9 @@
/* ================== 内部辅助函数 ================== */
static float LADRC_Abs(float x);
static float LADRC_Clamp(float x, float min_value, float max_value);
static void LADRC_ResetStates(LADRC_TypeDef *ladrc);
static float s_filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
static float LADRC_Abs(float x)
{
@@ -39,6 +43,20 @@ static float LADRC_Clamp(float x, float min_value, float max_value)
return x;
}
static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
{
if (ladrc == NULL)
{
return;
}
ladrc->v = 0.0f;
ladrc->y = 0.0f;
ladrc->z1 = 0.0f;
ladrc->z2 = 0.0f;
ladrc->u = 0.0f;
}
/**
* @brief 初始化单个 LADRC 控制器。
* @note 顺手做基础参数保护，避免 b0/h/out_max 为 0 或负值时出问题。
@@ -145,9 +163,23 @@ void FourWheel_LADRC_Init(void)
LADRC_DEFAULT_B0,
LADRC_CONTROL_DT_S,
LADRC_DEFAULT_OUT_MAX);
s_filtered_rpm[i] = 0.0f;
}
}
void FourWheel_LADRC_ResetAll(void)
{
int i;
for (i = 0; i < 4; ++i)
{
LADRC_ResetStates(&ladrc_motors[i]);
s_filtered_rpm[i] = 0.0f;
}
Motor_Brake_All();
}
/**
* @brief 设置四个轮子的目标 RPM。
*/
@@ -169,20 +201,25 @@ void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr
*/
void FourWheel_LADRC_Control_Loop(void)
{
static float filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
int i;
Motor_Update_RPM(LADRC_CONTROL_DT_S);
if (g_robot_ctrl.state != SYSTEM_OPERATIONAL)
{
FourWheel_LADRC_ResetAll();
return;
}
for (i = 0; i < 4; ++i)
{
float raw_rpm = Get_Motor_RPM((Motor_ID_t)i);
float pwm_out;
filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * filtered_rpm[i]
+ LADRC_RPM_FILTER_ALPHA * raw_rpm;
s_filtered_rpm[i] = (1.0f - LADRC_RPM_FILTER_ALPHA) * s_filtered_rpm[i]
+ LADRC_RPM_FILTER_ALPHA * raw_rpm;
pwm_out = LADRC_Calc(&ladrc_motors[i], filtered_rpm[i]);
pwm_out = LADRC_Calc(&ladrc_motors[i], s_filtered_rpm[i]);
Set_Motor_Output((Motor_ID_t)i, (int16_t)pwm_out);
}
}

2
Core/Src/tim.c
View File

@@ -557,7 +557,7 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
__HAL_RCC_TIM6_CLK_ENABLE();
/* TIM6 interrupt Init */
HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 2, 0);
HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
/* USER CODE BEGIN TIM6_MspInit 1 */