cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(FOC): 重构齿槽转矩校准状态机为独立函数

Browse Source 
- 将原本在 calibrate_cogging_tick 中的大函数拆分为多个职责单一的函数
- 新增 start_cogging_calibration、cogging_settling_tick、cogging_sampling_tick、
  cogging_stopping_tick、cogging_done_tick 等状态处理函数
- 添加 process_cogging_run_data、start_next_cogging_run、build_cogging_tables、
  print_cogging_table、print_cogging_c_array 等辅助函数
- 移动全局 FOC 控制器单例注释到更合适位置
- 添加齿槽转矩校准参数常量定义和详细注释
- 添加防御性编程处理默认状态分支，避免状态机卡死
- 提高代码可读性和维护性

feat(tasks): 添加任务头文件到项目配置

- 在 Robot_V2.cproject.yml 中添加 tasks.h 和 tasks.cpp 文件引用

docs(ADC): 添加ADC转换常量说明注释

- 在 ADC.cpp 中添加关于ADC换算常量的详细注释
- 说明全局采样器单例的初始化和使用方式
This commit is contained in:
cbb
2026-05-24 01:21:41 +08:00
parent 4c4e47e9e3
commit e646544f7f
23 changed files with 1705 additions and 647 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+170 -150
View File
@@ -20,6 +20,9 @@
#include <cstring>
#include <gd32c10x.h>
// 全局 FOC 控制器单例：系统只有一个电机控制对象。
// 初始化顺序由 main::hardware_init() 明确调用 FOC.init() 完成；
// 运行期主要由主循环控制任务访问，CAN 命令只更新目标/参数。
FOC_Controller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
@@ -62,6 +65,14 @@ inline float FOC_Controller::get_cogging_compensation(float angle_el) {
// 启用方法：config.h 中 FOC_DEBUG 改为 1，通过 CAN 命令触发（RESET_ID bit4）
// 校准完成后把 C Array 硬编码到 init() 中，再改回 FOC_DEBUG 0
#if FOC_DEBUG
// 齿槽转矩校准参数
constexpr u32 COG_CALIB_SETTLE_TICKS = 16000; // 800ms / 50us
constexpr u32 COG_CALIB_STOP_TICKS = 10000; // 500ms / 50us
constexpr u32 COG_CALIB_TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
constexpr float COG_CALIB_SLOW_VEL = 1.0f; // 开环速度 rad/s
constexpr u32 COG_CALIB_SAMPLE_DIV = 20; // 1ms / 50us
constexpr u8 COG_CALIB_TOTAL_RUNS = 6; // 3正+3反
// 齿槽转矩校准静态数据
float FOC_Controller::cog_calib_run_data[6][COGGING_TABLE_SIZE] = {};
float FOC_Controller::cog_calib_accum[COGGING_TABLE_SIZE] = {};
@@ -70,172 +81,181 @@ u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {};
/**
* @brief 齿槽转矩校准 - 非阻塞状态机
* 正转3次 + 反转3次，分开存储补偿表。
* 只打印最终平均数据和 C Array。
*/
void FOC_Controller::calibrate_cogging_tick(void) {
/** @brief SETTLE_TICKS 常量定义 */
constexpr u32 SETTLE_TICKS = 16000; // 800ms / 50us
/** @brief STOP_TICKS 常量定义 */
constexpr u32 STOP_TICKS = 10000; // 500ms / 50us
/** @brief TARGET_SAMPLES 常量定义 */
constexpr u32 TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
/** @brief SLOW_VEL 常量定义 */
constexpr float SLOW_VEL = 1.0f; // 开环速度 rad/s
/** @brief SAMPLE_DIV 常量定义 */
constexpr u32 SAMPLE_DIV = 20; // 1ms / 50us
/** @brief TOTAL_RUNS 常量定义 */
constexpr u8 TOTAL_RUNS = 6; // 3正+3反
// 入口只做状态派发，避免单函数堆叠所有校准分支。
switch (cog_calib_state) {
case CogCalibState::IDLE:
if (!cogging_calib_request) return;
cogging_calib_request = false;
cog_calib_run = 0;
cog_calib_saved_mode = control_mode;
cog_calib_saved_vel = target_velocity;
cogging_comp_enabled = false;
memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));
printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");
control_mode = 6;
target_velocity = SLOW_VEL;
cog_calib_settle_tick = 0;
cog_calib_state = CogCalibState::SETTLING;
start_cogging_calibration();
return;
case CogCalibState::SETTLING:
cog_calib_settle_tick++;
if (cog_calib_settle_tick >= SETTLE_TICKS) {
cog_calib_state = CogCalibState::SAMPLING;
cog_calib_sample_count = 0;
memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
memset(cog_calib_count, 0, sizeof(cog_calib_count));
}
cogging_settling_tick();
return;
case CogCalibState::SAMPLING:
// 采样分频
static u32 sample_div = 0;
sample_div++;
if (sample_div < SAMPLE_DIV) return;
sample_div = 0;
{
float real_angle = KTH7823.getcurrent_sensorangle();
float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
float iq = rt_current;
if (fabsf(iq) < 5000.0f) {
u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
cog_calib_accum[idx] += iq;
cog_calib_count[idx]++;
}
cog_calib_sample_count++;
}
if (cog_calib_sample_count >= TARGET_SAMPLES) {
target_velocity = 0;
cog_calib_state = CogCalibState::STOPPING;
cog_calib_stop_tick = 0;
}
cogging_sampling_tick();
return;
case CogCalibState::STOPPING:
cog_calib_stop_tick++;
if (cog_calib_stop_tick >= STOP_TICKS) {
// 处理本轮数据：去直流
float dc = 0;
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cog_calib_run_data[cog_calib_run][i] =
(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
dc += cog_calib_run_data[cog_calib_run][i];
}
dc /= (float)COGGING_TABLE_SIZE;
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cog_calib_run_data[cog_calib_run][i] -= dc;
}
printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
cog_calib_run + 1, TOTAL_RUNS, dc);
cog_calib_run++;
if (cog_calib_run < TOTAL_RUNS) {
// run 0~2 正转，run 3~5 反转
control_mode = 6;
target_velocity = (cog_calib_run < 3) ? SLOW_VEL : -SLOW_VEL;
cog_calib_state = CogCalibState::SETTLING;
cog_calib_settle_tick = 0;
} else {
cog_calib_state = CogCalibState::DONE;
}
}
cogging_stopping_tick();
return;
case CogCalibState::DONE:
// 正转平均 (run 0~2)
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cogging_table[i] = (cog_calib_run_data[0][i] +
cog_calib_run_data[1][i] +
cog_calib_run_data[2][i]) / 3.0f;
}
// 反转平均 (run 3~5)
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cogging_table_rev[i] = (cog_calib_run_data[3][i] +
cog_calib_run_data[4][i] +
cog_calib_run_data[5][i]) / 3.0f;
}
cogging_comp_enabled = true;
control_mode = cog_calib_saved_mode;
target_velocity = cog_calib_saved_vel;
// 打印正转平均
printf("\r\n[COG] === FWD Average ===\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table[i]);
fwdgt_counter_reload();
}
// 打印反转平均
printf("\r\n[COG] === REV Average ===\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table_rev[i]);
fwdgt_counter_reload();
}
// 打印正转 C Array
printf("\r\n[COG] --- FWD C Array ---\r\n");
printf("static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
if (i % 8 == 0) printf(" ");
printf("%.2ff", cogging_table[i]);
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
fwdgt_counter_reload();
}
printf("};\r\n");
// 打印反转 C Array
printf("\r\n[COG] --- REV C Array ---\r\n");
printf("static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
if (i % 8 == 0) printf(" ");
printf("%.2ff", cogging_table_rev[i]);
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
fwdgt_counter_reload();
}
printf("};\r\n");
printf("\r\n[COG] === Done. ===\r\n");
cogging_done_tick();
return;
default:
// 防御性处理：枚举值异常时回到空闲态，避免状态机卡死。
cog_calib_state = CogCalibState::IDLE;
return;
}
}
void FOC_Controller::start_cogging_calibration(void) {
// IDLE 状态下没有请求则保持空闲，不扰动正常控制路径。
if (!cogging_calib_request) return;
cogging_calib_request = false;
cog_calib_run = 0;
// 保存原控制状态，校准完成后恢复现场。
cog_calib_saved_mode = control_mode;
cog_calib_saved_vel = target_velocity;
cogging_comp_enabled = false;
memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));
printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");
control_mode = 6;
target_velocity = COG_CALIB_SLOW_VEL;
cog_calib_settle_tick = 0;
cog_calib_state = CogCalibState::SETTLING;
}
void FOC_Controller::cogging_settling_tick(void) {
// 给低速开环运行留出稳定时间，再开始采样。
cog_calib_settle_tick++;
if (cog_calib_settle_tick < COG_CALIB_SETTLE_TICKS) return;
cog_calib_state = CogCalibState::SAMPLING;
cog_calib_sample_count = 0;
memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
memset(cog_calib_count, 0, sizeof(cog_calib_count));
}
void FOC_Controller::cogging_sampling_tick(void) {
static u32 sample_div = 0;
// 每 1ms 采一次，降低校准数据相关处理对实时控制的影响。
sample_div++;
if (sample_div < COG_CALIB_SAMPLE_DIV) return;
sample_div = 0;
float real_angle = KTH7823.getcurrent_sensorangle();
float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
float iq = rt_current;
if (fabsf(iq) < 5000.0f) {
u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
cog_calib_accum[idx] += iq;
cog_calib_count[idx]++;
}
cog_calib_sample_count++;
if (cog_calib_sample_count >= COG_CALIB_TARGET_SAMPLES) {
target_velocity = 0;
cog_calib_state = CogCalibState::STOPPING;
cog_calib_stop_tick = 0;
}
}
void FOC_Controller::cogging_stopping_tick(void) {
// 每轮采样结束后先停稳，再计算本轮去直流数据。
cog_calib_stop_tick++;
if (cog_calib_stop_tick < COG_CALIB_STOP_TICKS) return;
float dc = process_cogging_run_data();
printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
cog_calib_run + 1, COG_CALIB_TOTAL_RUNS, dc);
cog_calib_run++;
if (cog_calib_run < COG_CALIB_TOTAL_RUNS) {
start_next_cogging_run();
} else {
cog_calib_state = CogCalibState::DONE;
}
}
float FOC_Controller::process_cogging_run_data(void) {
// 先求每个角度桶的平均电流，再统一去除直流偏置。
float dc = 0;
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cog_calib_run_data[cog_calib_run][i] =
(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
dc += cog_calib_run_data[cog_calib_run][i];
}
dc /= (float)COGGING_TABLE_SIZE;
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cog_calib_run_data[cog_calib_run][i] -= dc;
}
return dc;
}
void FOC_Controller::start_next_cogging_run(void) {
control_mode = 6;
target_velocity = (cog_calib_run < 3) ? COG_CALIB_SLOW_VEL : -COG_CALIB_SLOW_VEL;
cog_calib_state = CogCalibState::SETTLING;
cog_calib_settle_tick = 0;
}
void FOC_Controller::cogging_done_tick(void) {
// 生成最终表后恢复用户进入校准前的控制模式和目标速度。
build_cogging_tables();
cogging_comp_enabled = true;
control_mode = cog_calib_saved_mode;
target_velocity = cog_calib_saved_vel;
print_cogging_table("\r\n[COG] === FWD Average ===\r\n", cogging_table);
print_cogging_table("\r\n[COG] === REV Average ===\r\n", cogging_table_rev);
print_cogging_c_array("\r\n[COG] --- FWD C Array ---\r\n",
"COGGING_TABLE_FWD", cogging_table);
print_cogging_c_array("\r\n[COG] --- REV C Array ---\r\n",
"COGGING_TABLE_REV", cogging_table_rev);
printf("\r\n[COG] === Done. ===\r\n");
cog_calib_state = CogCalibState::IDLE;
}
void FOC_Controller::build_cogging_tables(void) {
// 前 3 轮为正转，后 3 轮为反转，分别求平均表。
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cogging_table[i] = (cog_calib_run_data[0][i] +
cog_calib_run_data[1][i] +
cog_calib_run_data[2][i]) / 3.0f;
cogging_table_rev[i] = (cog_calib_run_data[3][i] +
cog_calib_run_data[4][i] +
cog_calib_run_data[5][i]) / 3.0f;
}
}
void FOC_Controller::print_cogging_table(const char *title, float *table) {
printf("%s", title);
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
printf("[COG] %.2f, %.2f\r\n", angle_deg, table[i]);
fwdgt_counter_reload();
}
}
void FOC_Controller::print_cogging_c_array(const char *title,
const char *arrayName,
float *table) {
printf("%s", title);
printf("static const float %s[COGGING_TABLE_SIZE] = {\r\n", arrayName);
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
if (i % 8 == 0) printf(" ");
printf("%.2ff", table[i]);
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
fwdgt_counter_reload();
}
printf("};\r\n");
}
#endif // FOC_DEBUG
// 初始变量及函数定义
Robot_V2/Algorithm/FOC.h
+57 -1
View File
@@ -167,10 +167,66 @@ class FOC_Controller {
#if FOC_DEBUG
volatile bool cogging_calib_request = false;
/**
* @brief calibrate_cogging_tick 函数
* @brief 齿槽校准状态机入口，每个周期按当前状态派发到对应处理函数
* @return void
*/
void calibrate_cogging_tick(void);
/**
* @brief 处理齿槽校准空闲态，收到请求后保存现场并进入稳定等待态
* @return void
*/
void start_cogging_calibration(void);
/**
* @brief 处理齿槽校准稳定等待态，等待电机进入稳定低速运行
* @return void
*/
void cogging_settling_tick(void);
/**
* @brief 处理齿槽校准采样态，按分频周期累计每个电角度桶的电流
* @return void
*/
void cogging_sampling_tick(void);
/**
* @brief 处理齿槽校准停止态，等待停稳后完成本轮数据归一化
* @return void
*/
void cogging_stopping_tick(void);
/**
* @brief 处理齿槽校准完成态，生成补偿表并打印校准结果
* @return void
*/
void cogging_done_tick(void);
/**
* @brief 计算当前校准轮次的平均电流并去除直流偏置
* @return float 当前轮次直流偏置，单位 mA
*/
float process_cogging_run_data(void);
/**
* @brief 切换到下一轮齿槽校准运行方向并重新进入稳定等待态
* @return void
*/
void start_next_cogging_run(void);
/**
* @brief 根据 3 次正转和 3 次反转采样结果生成最终补偿表
* @return void
*/
void build_cogging_tables(void);
/**
* @brief 打印齿槽补偿表的人类可读角度/电流列表
* @param[in] title 表格标题字符串
* @param[in] table 待打印的补偿表
* @return void
*/
void print_cogging_table(const char *title, float *table);
/**
* @brief 打印可直接复制回源码的齿槽补偿 C 数组
* @param[in] title 输出标题字符串
* @param[in] arrayName C 数组名
* @param[in] table 待打印的补偿表
* @return void
*/
void print_cogging_c_array(const char *title, const char *arrayName,
float *table);
/**
* @brief CogCalibState 枚举定义
*/
Robot_V2/Robot_V2.cproject.yml
+2
View File
@@ -4,6 +4,8 @@ project:
files:
- file: main.h
- file: main.cpp
- file: tasks.h
- file: tasks.cpp
- group: sys
files:
- file: sys/systick.cpp
Robot_V2/device/ADC.cpp
+5
View File
@@ -16,6 +16,8 @@
#include <cstring>
#include <gd32c10x_dma.h>
// ADC 换算常量均为编译期常量，非可变全局状态。
// 用于 ADC 原始值到电压、温度、电流的固定比例换算。
/** @brief ADC_SCALE 常量定义 */
constexpr float ADC_SCALE = 3.3f / 4096.0f;
/** @brief TEMP_FACTOR 常量定义 */
@@ -27,6 +29,9 @@ constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controller FOC;
// 全局采样器单例：系统只有一组 ADC 采样缓存，由 hardware_init() 调用 sampler.init() 初始化。
// regular DMA 写 adc_value[]；injected ADC 中断写 adc_current_value[] 并置位 adc_current_frame_ready。
sampler sampler; // 采样器初始化
u16 adc_value[3] = {0}; // regular DMA: MCU温度 / DRV温度 / VBUS
static volatile u16 adc_current_value[3] = {0}; // injected 同步采样: U / V / W
Robot_V2/device/Can.cpp
+206 -209
View File
@@ -33,6 +33,7 @@
#define CAN_DEBUG_PRINT(fmt, ...) ((void)0)
#endif
// CAN0 固定外设号，编译期常量，非可变全局状态。
/** @brief CANX 常量定义 */
constexpr u16 CANX = CAN0;
extern FOC_Controller FOC;
@@ -44,7 +45,9 @@ extern FlashEEPROM flashEEP;
extern VersionManager Version; // 声明外部版本管理器实例
extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式计时）
// 添加 DMA 相关变量
// CAN RX DMA 与收发队列模块状态：
// - DMA 元数据/busy 标志由 CAN RX 中断与 DMA 完成中断协同维护；
// - rx/tx 队列 head/tail/count 同时被主循环和中断访问，队列操作通过 CAN_EnterCritical()/CAN_ExitCritical() 保护。
dma_parameter_struct dma_rx_parameter;
uint8_t can_rx_buffer[64]; // CAN 接收缓冲区（支持CAN FD最大64字节）
static uint32_t can_rx_dma_words[16]; // CAN FD接收DMA按32位搬运，最多64字节
@@ -81,6 +84,8 @@ static volatile u8 tx_queue_count = 0;
// CAN FD DLC 9~15对应的非线性payload长度表
static constexpr u8 CANFD_DLC_LENGTH_TABLE[7] = {12, 16, 20, 24, 32, 48, 64};
// 全局 CAN 发送器单例：系统只有一个 CAN0 通信对象。
// 初始化顺序由 main::hardware_init() 调用 CAN0_transmiter.init() 保证。
CAN_transmiter CAN0_transmiter; // CAN 初始化
// ============================================================================
@@ -701,8 +706,10 @@ u8 CAN_transmiter::init() {
}
/**
* @brief CAN_EnterCritical 函数
* @return uint32_t 返回值
* @brief 进入 CAN 临界区并保存原 PRIMASK 状态
* @return uint32_t 进入临界区前的 PRIMASK，用于退出时恢复原中断状态
* @note 退出时使用 CAN_ExitCritical() 调用 __set_PRIMASK() 恢复，
* 因此这里不会直接与 __enable_irq() 成对出现。
*/
static uint32_t CAN_EnterCritical(void) {
uint32_t primask = __get_PRIMASK();
@@ -711,9 +718,10 @@ static uint32_t CAN_EnterCritical(void) {
}
/**
* @brief CAN_ExitCritical 函数
* @param[in] primask 参数
* @brief 退出 CAN 临界区并恢复进入前的中断屏蔽状态
* @param[in] primask CAN_EnterCritical() 返回的原 PRIMASK 状态
* @return void
* @note 不直接调用 __enable_irq()，避免误开启外层已经关闭的中断。
*/
static void CAN_ExitCritical(uint32_t primask) { __set_PRIMASK(primask); }
@@ -1074,228 +1082,217 @@ u8 CAN_transmiter::CANFD_all_Send() {
* 1: 有新消息
*/
void CAN_transmiter::communication_check() {
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// 检查是否在静默模式中
if (!is_in_silent_mode()) {
update();
CANFD_all_Send();
} else {
check_silent_mode(); // 检查静默是否结束
}
can0_send_flag = RESET;
}
// 先处理周期发送，再抽空接收队列，保持原来的调度顺序。
handle_periodic_send_tick();
// 处理所有接收到的 CANFD 消息（从底层中断队列中取出）
can_rx_queue_item_t rx_item;
while (CAN_RX_QueuePop(&rx_item)) {
can_receive_message_struct current_message;
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &current_message);
current_message.rx_efid = rx_item.id;
current_message.rx_dlen = rx_item.length;
memcpy(current_message.rx_data, rx_item.data, rx_item.length);
handle_received_queue_item(rx_item.id, rx_item.length, rx_item.data);
}
}
// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
u32 base_id = current_message.rx_efid & 0xFFFF;
void CAN_transmiter::handle_periodic_send_tick() {
// 无发送请求时直接返回，避免每周期进入发送逻辑。
if (SET != can0_send_flag) return;
// 提取发送方的节点 ID
u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF);
if (!is_in_silent_mode()) {
update();
CANFD_all_Send();
} else {
check_silent_mode();
}
can0_send_flag = RESET;
}
// 特殊处理 base_id=0x0000（BOOT 命令）：即使是其他节点的广播消息也要处理
if (base_id == CANFD_BOOT_ID) {
// 判断是广播消息还是其他节点的命令
if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) {
// 广播消息或其他节点的命令 → 进入静默模式（停发 10 秒）
CAN_DEBUG_PRINT(
"Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n",
sender_node_id);
enter_silent_mode();
} else {
// 自己节点的命令 → 执行 BOOT 功能
CAN_DEBUG_PRINT(
"Received base_id=0x0000 from self, jumping to BOOT\r\n");
motor_sleep();
flashEEP.save_system_config(false);
fmc_unlock();
OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置 APP 为不激活
fmc_lock();
jumpToBOOT();
}
continue; // BOOT 命令处理完跳过后续逻辑
}
void CAN_transmiter::handle_received_queue_item(u32 id, u8 length, const u8 *data) {
// 将轻量队列项还原为原 CAN 消息结构，后续沿用原解析逻辑。
can_receive_message_struct current_message;
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &current_message);
current_message.rx_efid = id;
current_message.rx_dlen = length;
memcpy(current_message.rx_data, data, length);
// 设置节点 ID 命令 (基础 ID: 0x0400)
// 注意：此命令在 is_message_for_me 检查之前处理，所有节点都会响应
// 这样即使节点ID已经改变，仍然可以用旧节点ID或广播ID来修改
else if (base_id == CANFD_SET_NODE_ID) {
// 打印接收到的原始数据
CAN_DEBUG_PRINT(
"SET_NODE_ID: rx_dlen=%d, data[0]=%02X data[1]=%02X\r\n",
current_message.rx_dlen, current_message.rx_data[0],
current_message.rx_data[1]);
// 解析新节点 ID - 支持 1 字节或 2 字节格式
u16 new_node_id;
if (current_message.rx_dlen >= 2) {
new_node_id = unpack_bytes_to_u16(current_message.rx_data, 0);
} else {
new_node_id = current_message.rx_data[0]; // 只发送 1 字节时直接使用
}
u16 old_node_id = node_id; // 保存旧节点 ID 用于构建响应 ID
CAN_DEBUG_PRINT(
"SET_NODE_ID: recv new_id=%d (0x%04X), old_id=%d\r\n",
u32 base_id = current_message.rx_efid & 0xFFFF;
u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF);
// BOOT 和 SET_NODE_ID 需要在节点过滤前处理。
if (handle_special_message(current_message, base_id, sender_node_id)) return;
if (!is_message_for_me(current_message.rx_efid)) return;
handle_node_message(current_message, base_id);
}
bool CAN_transmiter::handle_special_message(can_receive_message_struct &current_message,
u32 base_id, u16 sender_node_id) {
if (base_id == CANFD_BOOT_ID) {
handle_boot_message(sender_node_id);
return true;
}
if (base_id == CANFD_SET_NODE_ID) {
handle_set_node_id_message(current_message);
return true;
}
return false;
}
void CAN_transmiter::handle_boot_message(u16 sender_node_id) {
// 其他节点或广播 BOOT 命令只让本节点静默，避免抢占总线。
if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) {
CAN_DEBUG_PRINT(
"Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n",
sender_node_id);
enter_silent_mode();
return;
}
CAN_DEBUG_PRINT("Received base_id=0x0000 from self, jumping to BOOT\r\n");
motor_sleep();
flashEEP.save_system_config(false);
fmc_unlock();
OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL);
fmc_lock();
jumpToBOOT();
}
void CAN_transmiter::handle_set_node_id_message(can_receive_message_struct &current_message) {
// 支持 1 字节和 2 字节节点 ID 命令，兼容旧上位机。
CAN_DEBUG_PRINT(
"SET_NODE_ID: rx_dlen=%d, data[0]=%02X data[1]=%02X\r\n",
current_message.rx_dlen, current_message.rx_data[0],
current_message.rx_data[1]);
u16 new_node_id = (current_message.rx_dlen >= 2) ?
unpack_bytes_to_u16(current_message.rx_data, 0) : current_message.rx_data[0];
u16 old_node_id = node_id;
CAN_DEBUG_PRINT("SET_NODE_ID: recv new_id=%d (0x%04X), old_id=%d\r\n",
new_node_id, new_node_id, old_node_id);
if (new_node_id <= MAX_NODE_ID && new_node_id != BROADCAST_NODE_ID) {
// 先发送响应（使用旧节点ID构建CAN ID）
// 响应数据格式：[status:1byte][new_node_id:2byte] = 3 字节
transmit_data[0] = 0x01; // 成功
transmit_data[1] = (u8)(new_node_id & 0xFF); // 新 ID 低字节
transmit_data[2] =
(u8)((new_node_id >> 8) & 0xFF); // 新 ID 高字节
transmit_data[1] = (u8)(new_node_id & 0xFF);
transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF);
// 响应帧使用旧节点 ID 构造，确保上位机能收到确认。
node_id = old_node_id;
// 使用旧节点 ID 发送响应
node_id = old_node_id;
u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
if (new_node_id > MAX_NODE_ID || new_node_id == BROADCAST_NODE_ID) {
CAN_DEBUG_PRINT("SET_NODE_ID: invalid new_id=%d (max=%d)\r\n",
new_node_id, MAX_NODE_ID);
transmit_data[0] = 0x00;
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, transmit_data);
return;
}
// 然后保存新节点 ID 到 Flash
u8 save_status = save_node_id_to_flash(new_node_id);
transmit_data[0] = 0x01;
u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
u8 save_status = save_node_id_to_flash(new_node_id);
CAN_DEBUG_PRINT(
"SET_NODE_ID: save_status=%d, send_result=%d\r\n",
save_status, send_result);
CAN_DEBUG_PRINT("SET_NODE_ID: save_status=%d, send_result=%d\r\n",
save_status, send_result);
if (save_status == FLASH_EEPROM_OK) {
CAN_DEBUG_PRINT(
"SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n",
old_node_id, new_node_id);
} else {
CAN_DEBUG_PRINT(
"SET_NODE_ID: Flash write FAILED, status=%d\r\n",
save_status);
// 发送错误响应
transmit_data[0] = 0x00; // 失败
node_id = old_node_id;
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
}
} else {
// 发送错误响应 - 使用旧节点 ID 构建响应 ID
CAN_DEBUG_PRINT("SET_NODE_ID: invalid new_id=%d (max=%d)\r\n",
new_node_id, MAX_NODE_ID);
transmit_data[0] = 0x00; // 失败
transmit_data[1] = (u8)(new_node_id & 0xFF);
transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF);
node_id = old_node_id; // 临时恢复旧 ID 用于构建响应
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
// 保持原 ID 不变
}
continue; // SET_NODE_ID 命令处理完跳过后续逻辑
}
if (save_status == FLASH_EEPROM_OK) {
CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n",
old_node_id, new_node_id);
return;
}
// 其他 base_id 的消息，检查是否是发给本节点的
if (!is_message_for_me(current_message.rx_efid)) {
continue; // 不是发给本节点的消息，跳过
}
CAN_DEBUG_PRINT("SET_NODE_ID: Flash write FAILED, status=%d\r\n", save_status);
transmit_data[0] = 0x00;
node_id = old_node_id;
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, transmit_data);
}
if (base_id == CANFD_CMD_ID) {
// 控制模式
FOC.control_mode = current_message.rx_data[0];
// 目标电压限制
FOC.limit_voltage =
float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
// 目标电流
target_current_union.f =
unpack_bytes_to_float(current_message.rx_data, 3).f;
FOC.current_loop.set_limit(target_current_union.f);
FOC.vel_inMITloop.set_limit(target_current_union.f);
// 目标速度
target_velocity_union.f =
unpack_bytes_to_float(current_message.rx_data, 7).f;
FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
FOC.ang_inMITloop.set_limit(target_velocity_union.f);
// 目标角度
target_angle_union.f =
unpack_bytes_to_float(current_message.rx_data, 11).f;
// 赋值
FOC.target_angle = target_angle_union.f;
FOC.target_velocity = target_velocity_union.f;
FOC.target_current = target_current_union.f;
void CAN_transmiter::handle_node_message(can_receive_message_struct &current_message,
u32 base_id) {
// 这里只处理已经确认发给本节点的普通协议消息。
if (base_id == CANFD_CMD_ID) {
handle_control_command(current_message);
} else if (base_id == CANFD_RESET_ID) {
handle_reset_command(current_message);
} else if (base_id == CANFD_VERSION_ID) {
Version.sendVersionInfoViaCan();
} else if (base_id == CANFD_OTA_ACK_ID) {
handle_ota_ack_message();
} else if (base_id == CANFD_GET_NODE_ID) {
pack_u16_to_bytes(transmit_data, 0, get_node_id());
CANFD_AsyncSend(build_can_id(CANFD_GET_NODE_ID), 2, transmit_data);
} else {
CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n",
current_message.rx_efid, node_id);
}
}
// 解析 PID 参数
float voltage = FOC.limit_voltage;
// PID 参数
u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
void CAN_transmiter::handle_control_command(can_receive_message_struct &current_message) {
// 控制帧布局：模式、电压限制、目标电流/速度/角度、三组 P 参数。
FOC.control_mode = current_message.rx_data[0];
FOC.limit_voltage = float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
target_current_union.f = unpack_bytes_to_float(current_message.rx_data, 3).f;
FOC.current_loop.set_limit(target_current_union.f);
FOC.vel_inMITloop.set_limit(target_current_union.f);
target_velocity_union.f = unpack_bytes_to_float(current_message.rx_data, 7).f;
FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
FOC.ang_inMITloop.set_limit(target_velocity_union.f);
target_angle_union.f = unpack_bytes_to_float(current_message.rx_data, 11).f;
FOC.target_angle = target_angle_union.f;
FOC.target_velocity = target_velocity_union.f;
FOC.target_current = target_current_union.f;
P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
float voltage = FOC.limit_voltage;
u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
} else if (base_id == CANFD_RESET_ID) {
u8 first_byte = current_message.rx_data[0];
u8 eigth_byte = current_message.rx_data[7];
// 电机重置
if (first_byte & 0x01) {
motor_reset();
testfloat_union.f = KTH7823.getcurrent_sensorangle();
}
// EEPROM 重置
if (first_byte >> 1 & 0x01) {
// flashEEP.format();
}
// 系统参数保存
if (first_byte >> 2 & 0x01) {
flashEEP.save_system_config(false);
}
// 角度传感器重置
if (first_byte >> 3 & 0x01) {
KTH7823.angle_set(0);
Angle_kalman.init(0); // 角度滤波
Vel_kalman.init(0); // 角度滤波
}
// 齿槽转矩校准 (first_byte bit4) — DEBUG，平时不启用
#if FOC_DEBUG
if (first_byte >> 4 & 0x01) {
FOC.cogging_calib_request = true;
}
#endif
// 休眠
if (eigth_byte & 0x80 >> 7) {
app_prepare_deep_sleep(true);
// CAN_Sleep();
}
} else if (base_id == CANFD_VERSION_ID) {
// 处理版本号请求，发送当前系统版本信息
Version.sendVersionInfoViaCan();
} else if (base_id == CANFD_OTA_ACK_ID) {
volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
u32 app_enable = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR;
u32 app_check = *flash_ptr;
transmit_data[0] = 0x0A;
transmit_data[1] = 0xAC;
delay_1ms(1);
if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2,
transmit_data);
}
}
// 查询节点 ID 命令 (基础 ID: 0x0402)
else if (base_id == CANFD_GET_NODE_ID) {
pack_u16_to_bytes(transmit_data, 0, get_node_id());
CANFD_AsyncSend(build_can_id(CANFD_GET_NODE_ID), 2, transmit_data);
} else {
CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n",
current_message.rx_efid, node_id);
}
P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
}
void CAN_transmiter::handle_reset_command(can_receive_message_struct &current_message) {
// RESET 帧按 bit 位触发独立动作，保持与原协议兼容。
u8 first_byte = current_message.rx_data[0];
u8 eigth_byte = current_message.rx_data[7];
if (first_byte & 0x01) {
motor_reset();
testfloat_union.f = KTH7823.getcurrent_sensorangle();
}
if (first_byte >> 1 & 0x01) {
// flashEEP.format();
}
if (first_byte >> 2 & 0x01) {
flashEEP.save_system_config(false);
}
if (first_byte >> 3 & 0x01) {
KTH7823.angle_set(0);
Angle_kalman.init(0);
Vel_kalman.init(0);
}
#if FOC_DEBUG
if (first_byte >> 4 & 0x01) {
FOC.cogging_calib_request = true;
}
#endif
if (eigth_byte & 0x80 >> 7) {
app_prepare_deep_sleep(true);
}
}
void CAN_transmiter::handle_ota_ack_message() {
// 只有当前 APP 标记为可用且校验通过时才回 ACK。
volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
u32 app_enable = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR;
u32 app_check = *flash_ptr;
transmit_data[0] = 0x0A;
transmit_data[1] = 0xAC;
delay_1ms(1);
if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2, transmit_data);
}
}
Robot_V2/device/Can.h
+59
View File
@@ -235,6 +235,65 @@ class CAN_transmiter {
* @return void
*/
void check_silent_mode(); // 检查静默模式是否结束
/**
* @brief 处理周期性发送请求，静默模式下只检查是否退出静默
* @return void
*/
void handle_periodic_send_tick();
/**
* @brief 将底层接收队列项转换为 CAN 消息并按协议分发
* @param[in] id 接收到的扩展 CAN ID
* @param[in] length 接收数据长度
* @param[in] data 接收数据指针
* @return void
*/
void handle_received_queue_item(u32 id, u8 length, const u8 *data);
/**
* @brief 处理不受本节点过滤限制的特殊消息
* @param[in,out] current_message 当前 CAN 消息
* @param[in] base_id 低 16 位基础 ID
* @param[in] sender_node_id 发送方节点 ID
* @return bool true 表示消息已处理，false 表示继续按普通节点消息处理
*/
bool handle_special_message(can_receive_message_struct &current_message,
u32 base_id, u16 sender_node_id);
/**
* @brief 处理 BOOT 命令或其他节点 BOOT 广播导致的静默逻辑
* @param[in] sender_node_id 发送方节点 ID
* @return void
*/
void handle_boot_message(u16 sender_node_id);
/**
* @brief 处理节点 ID 设置命令，发送应答并保存新 ID 到 Flash
* @param[in,out] current_message 当前 CAN 消息
* @return void
*/
void handle_set_node_id_message(can_receive_message_struct &current_message);
/**
* @brief 处理已经确认发给本节点的普通协议消息
* @param[in,out] current_message 当前 CAN 消息
* @param[in] base_id 低 16 位基础 ID
* @return void
*/
void handle_node_message(can_receive_message_struct &current_message,
u32 base_id);
/**
* @brief 解析控制命令并更新 FOC 目标值和 PID 参数
* @param[in,out] current_message 当前 CAN 消息
* @return void
*/
void handle_control_command(can_receive_message_struct &current_message);
/**
* @brief 解析复位命令各 bit 位并执行对应动作
* @param[in,out] current_message 当前 CAN 消息
* @return void
*/
void handle_reset_command(can_receive_message_struct &current_message);
/**
* @brief 处理 OTA ACK 查询，APP 有效时回发确认帧
* @return void
*/
void handle_ota_ack_message();
public:
// 节点 ID 相关辅助函数 (使用高 16 位存储节点 ID)
Robot_V2/device/Timer.cpp
+1 -1
View File
@@ -15,7 +15,7 @@
#include "gd32c10x_timer.h"
#include "initial.h"
volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1
volatile u32 total_us100_counter = 0; // TIMER2 ISR 内部 100us 调度索引
extern u16 adc_value[3];
extern FOC_Controller FOC;
extern TaskScheduler scheduler;
Robot_V2/device/ang_sensor.cpp
+3 -1
View File
@@ -34,10 +34,12 @@
/** @brief DRV_SPI_CS_DISABLE 宏定义 */
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
// DMA 传输完成标志
// SPI DMA 完成标志：由 DMA ISR 置位，spi_rw_half_word_dma() 发起传输前清零并等待。
volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
// 全局角度传感器单例：系统只有一个 KTH7823 角度传感器。
// 初始化顺序由 main::hardware_init() 调用 KTH7823.init() 保证。
angle_sensor KTH7823; // 角度传感器初始化
extern FlashEEPROM flashEEP;
extern EncoderAngleKalman Angle_kalman;
Robot_V2/device/motor.cpp
+2
View File
@@ -35,7 +35,9 @@ void motor_init(void) {
*/
void motor_reset(void) {
motor_sleep();
#if FOC_DEBUG
printf("motor reseting...\r\n");
#endif
motor_resume();
}
Robot_V2/main.cpp
+6 -109
View File
@@ -5,6 +5,7 @@
*/
#include "main.h"
#include "tasks.h"
#include "Can.h"
#include "FOC.h"
#include "Filter.h"
@@ -30,29 +31,16 @@ extern sampler sampler; // 采样器
extern angle_sensor KTH7823; // 角度传感器
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern EncoderAngleKalman Angle_kalman; // 角度滤波
extern ThermalController thermalController; // 热量控制器
extern FOC_Controller FOC; // FOC 控制
extern FlashEEPROM flashEEP; // EEPROM 类
extern VersionManager Version;
// 全局版本管理器实例
// 主任务调度共享位图：TIMER2 ISR 置位任务，main 循环短关中断后取快照并清位。
TaskScheduler scheduler = {0};
u32 motor_runtime_50us = 0; // 电机驱动计算轮次
u32 cnt_test = 0; // 负载检测
// 电机 50us 控制周期计数：主循环电机任务递增，观测器/计时逻辑读取。
u32 motor_runtime_50us = 0;
// 负载检测/通信心跳计数：主循环递增，CAN 周期发送任务清零。
u32 cnt_test = 0;
// 函数指针数组，包含各种控制模式对应的函数
static FOCFunctionPtr control_functions[] = {
&FOC_Controller::drv_hold, // 0: 占空比置
&FOC_Controller::angle_control, // 1: 位置闭环控制
&FOC_Controller::velocity_control, // 2: 速度闭环控制
&FOC_Controller::
angle_control_with_velocity_limit, // 3:
// 角度闭环控制，带速度限制
&FOC_Controller::MIT_Mode, // 4: MIT 模式
&FOC_Controller::current_control, // 5: 电流闭环控制
&FOC_Controller::velocity_open_loop, // 6: 速度开环控制
&FOC_Controller::PWM_out, // 0: 占空比置
};
/**
* @brief 应用程序入口函数
* @return int 返回值
@@ -112,94 +100,3 @@ u8 hardware_init(void) {
return init_status;
}
/**
* @brief 根据任务标志调度周期性业务任务
* @param[in] currentTasks 参数
* @return void
*/
void do_task(u32 currentTasks) {
busy_task_enter(); // DWT: 记录忙碌起始周期
cnt_test++;
// 按照优先级和重要性处理任务
if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
sampler.update_current();
}
if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值
KTH7823.update();
FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
}
if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度观测值
FOC.rt_velocity = Angle_kalman.getVelocity();
}
if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务 (最高优先级)
motor_runtime_50us++;
// 快速电压检测（每100us执行一次，响应时间 < 200us）
static u8 vcheck_div = 0;
if (++vcheck_div >= 2u) {
vcheck_div = 0;
FOC.fast_voltage_check();
}
// 静态变量用于跟踪上一次的控制模式
static int last_control_mode = -1;
// 边界检查，防止数组越界
if (FOC.control_mode >= 0 &&
FOC.control_mode < 8) { // 更新为 8 个控制模式
// 检查控制模式是否发生变化
if (FOC.control_mode != last_control_mode) {
// 重置所有 PID 控制器的状态，避免积分项累积
FOC.vel_loop.reset();
FOC.angle_loop.reset();
FOC.current_loop.reset();
FOC.vel_inVelAngloop.reset();
FOC.ang_inVelAngloop.reset();
FOC.ang_inMITloop.reset();
FOC.vel_inMITloop.reset();
FOC.current_inMITloop.reset();
// 更新上次控制模式
last_control_mode = FOC.control_mode;
}
if (!FOC.is_shut_down) {
// 调用对应的控制函数
(FOC.*control_functions[FOC.control_mode])();
} else {
// 调用对应的控制函数
FOC.drv_stop();
}
}
}
// 齿槽转矩校准 tick（DEBUG，平时不启用）
#if FOC_DEBUG
FOC.calibrate_cogging_tick();
#endif
if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新 mcu 温度传感器值
sampler.update_mcu_temp();
}
if (currentTasks & (1 << TASK_DRV_TEMP_UPDATE)) { // 更新电机温度传感器值
sampler.update_drv_temp();
}
if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制
thermalController.update();
}
if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测
angsens_MG_check();
}
if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新 vbus 电压值
power_manage();
}
if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN 通信
CAN0_transmiter.test_u16 = CAN0_transmiter.get_node_id(); // 负载检测
cnt_test = 0;
CAN0_transmiter.can0_send_flag = SET;
CAN0_transmiter.communication_check();
fwdgt_counter_reload(); // 看门狗复位
}
busy_task_exit(); // DWT: 累加忙碌周期
}
Robot_V2/main.h
-7
View File
@@ -36,13 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define MAIN_H
#include <cstdint>
/**
* @brief 根据任务标志调度周期性业务任务
* @param[in] currentTasks 参数
* @return void
*/
void do_task(u32 currentTasks);
/**
* @brief 初始化应用层使用的硬件与软件模块
* @return u8 返回值
Robot_V2/sys/config.h
+1 -164
View File
@@ -12,71 +12,41 @@
// 如果使用 CMSIS DSP 库，需要定义 ARM_MATH_CM4
#define ARM_MATH_CM4
/** @brief EEP_DEBUG 宏定义 */
#define EEP_DEBUG 0
/** @brief OTA_DEBUG 宏定义 */
#define OTA_DEBUG 0
/** @brief SLEEP_DEBUG 宏定义 */
#define SLEEP_DEBUG 0
/** @brief ANG_SENS_DEBUG 宏定义 */
#define ANG_SENS_DEBUG 0
/** @brief CAN_DEBUG 宏定义 */
#define CAN_DEBUG 0
/** @brief MAIN_DEBUG 宏定义 */
#define MAIN_DEBUG 0
/** @brief FOC_DEBUG 宏定义 */
#define FOC_DEBUG 0
// 版本号定义 - 可根据实际项目进行调整
/** @brief SOFTWARE_VERSION_MAJOR 常量定义 */
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
/** @brief SOFTWARE_VERSION_MINOR 常量定义 */
constexpr u16 SOFTWARE_VERSION_MINOR = 5;
/** @brief SOFTWARE_VERSION_PATCH 常量定义 */
constexpr u16 SOFTWARE_VERSION_PATCH = 19;
/** @brief SOFTWARE_VERSION_BUILD 常量定义 */
constexpr u16 SOFTWARE_VERSION_PATCH = 24;
constexpr u16 SOFTWARE_VERSION_BUILD = 2;
/** @brief HARDWARE_VERSION_MAJOR 常量定义 */
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
/** @brief HARDWARE_VERSION_MINOR 常量定义 */
constexpr u16 HARDWARE_VERSION_MINOR = 12;
/** @brief HARDWARE_VERSION_PATCH 常量定义 */
constexpr u16 HARDWARE_VERSION_PATCH = 01;
/** @brief HARDWARE_VERSION_BUILD 常量定义 */
constexpr u16 HARDWARE_VERSION_BUILD = 0;
/** @brief BOOT_VERSION_MAJOR 常量定义 */
constexpr u16 BOOT_VERSION_MAJOR = 26;
/** @brief BOOT_VERSION_MINOR 常量定义 */
constexpr u16 BOOT_VERSION_MINOR = 1;
/** @brief BOOT_VERSION_PATCH 常量定义 */
constexpr u16 BOOT_VERSION_PATCH = 12;
/** @brief BOOT_VERSION_BUILD 常量定义 */
constexpr u16 BOOT_VERSION_BUILD = 0;
// Flash 分区配置
/** @brief BOOTLOADER_SIZE 常量定义 */
constexpr u32 BOOTLOADER_SIZE = 0x6000; // 24KB
/** @brief BOOTLOADER_START_ADDR 常量定义 */
constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT 起始地址
/** @brief APPLATEST_START_ADDR 常量定义 */
constexpr u32 APPLATEST_START_ADDR = 0x08012000; // APP 起始地址
/** @brief APPBACK_START_ADDR 常量定义 */
constexpr u32 APPBACK_START_ADDR = 0x08006000; // APPBACK 起始地址
/** @brief APP_MAX_SIZE 常量定义 */
constexpr u32 APP_MAX_SIZE = 0xC000; // 48KB
/** @brief FMC_PAGE_SIZE 常量定义 */
constexpr u32 FMC_PAGE_SIZE = 0x400; // 1KB
/** @brief SLEEP_CONFIG_START 常量定义 */
constexpr u32 SLEEP_CONFIG_START = 0x0801E400; // 睡眠配置起始地址
/** @brief SLEEP_CONFIG_END 常量定义 */
constexpr u32 SLEEP_CONFIG_END = 0x0801E800; // 睡眠配置结束地址
/** @brief FLASH_EMPTY 常量定义 */
constexpr u32 FLASH_EMPTY = 0xFFFFFFFF; // 空数据
/** @brief SLEEP_ENABLE 常量定义 */
constexpr u32 SLEEP_ENABLE = 0x11111111; // 睡眠使能
/** @brief SLEEP_DISABLE 常量定义 */
constexpr u32 SLEEP_DISABLE = 0x00000000; // 睡眠禁止
// 节点 ID 配置 (用于多电机 CAN 总线隔离)
@@ -88,19 +58,12 @@ constexpr u32 SLEEP_DISABLE = 0x00000000; // 睡眠禁止
// - 采用双重备份机制 (主备份 + 备用备份)
// - 使用魔数 (NODE_ID_MAGIC) 验证数据有效性，防止位翻转导致错读
// - 使用写入计数器区分最新数据
/** @brief NODE_ID_PAGE_ADDR 常量定义 */
constexpr u32 NODE_ID_PAGE_ADDR = 0x0801E800; // 节点 ID 存储页 (1KB，独立区域)
/** @brief NODE_ID_PRIMARY_ADDR 常量定义 */
constexpr u32 NODE_ID_PRIMARY_ADDR = 0x0801E800; // 主存储地址
/** @brief NODE_ID_BACKUP_ADDR 常量定义 */
constexpr u32 NODE_ID_BACKUP_ADDR = 0x0801E840; // 备份存储地址 (偏移64字节)
/** @brief NODE_ID_MAGIC 常量定义 */
constexpr u32 NODE_ID_MAGIC = 0x4E4F4445; // 魔数 'NODE' 用于验证数据有效性
/** @brief DEFAULT_NODE_ID 常量定义 */
constexpr u16 DEFAULT_NODE_ID = 1; // 默认节点 ID
/** @brief BROADCAST_NODE_ID 常量定义 */
constexpr u16 BROADCAST_NODE_ID = 0x1FFF; // 广播 ID (所有节点响应)
/** @brief MAX_NODE_ID 常量定义 */
constexpr u16 MAX_NODE_ID = 0x1FFE; // 最大有效节点 ID (0~0x1FFE)
// 节点 ID 存储结构 (存储在Flash中的格式)
@@ -109,296 +72,174 @@ constexpr u16 MAX_NODE_ID = 0x1FFE; // 最大有效节点 ID (0~0x1FFE)
// 地址偏移 0x06: 保留 (u16，填充为0xFFFF)
// 地址偏移 0x08: 写入计数器 (u32，用于区分最新数据)
// 地址偏移 0x0C: CRC32 校验 (u32)
/** @brief NODE_ID_STRUCT_SIZE 常量定义 */
constexpr u8 NODE_ID_STRUCT_SIZE = 0x10; // 结构体大小 16 字节
/** @brief NODE_ID_MAGIC_OFFSET 常量定义 */
constexpr u8 NODE_ID_MAGIC_OFFSET = 0x00; // 魔数偏移
/** @brief NODE_ID_VALUE_OFFSET 常量定义 */
constexpr u8 NODE_ID_VALUE_OFFSET = 0x04; // 节点ID值偏移
/** @brief NODE_ID_COUNTER_OFFSET 常量定义 */
constexpr u8 NODE_ID_COUNTER_OFFSET = 0x08; // 写入计数器偏移
/** @brief NODE_ID_CRC_OFFSET 常量定义 */
constexpr u8 NODE_ID_CRC_OFFSET = 0x0C; // CRC偏移
// BOOT 相关
/** @brief OTA_CONFIG_NUM 常量定义 */
constexpr u32 OTA_CONFIG_NUM = 6;
/** @brief n_APP_LATEST_AVAL_ADDR 常量定义 */
constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801E000; // 最新 APP 可用地址
/** @brief n_APP_BACKUP_AVAL_ADDR 常量定义 */
constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801E004; // 备份 APP 可用地址
/** @brief n_APP_LATEST_EN_ADDR 常量定义 */
constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801E008; // 最新 APP 激活
/** @brief n_APP_BACKUP_EN_ADDR 常量定义 */
constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801E00C; // 备份 APP 激活
/** @brief n_APP_LATEST_CHECK_ADDR 常量定义 */
constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801E010; // 最新 APP 校验
/** @brief n_APP_BACKUP_CHECK_ADDR 常量定义 */
constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801E014; // 备份 APP 校验
/** @brief OTA_NOT_AVAL 常量定义 */
constexpr u32 OTA_NOT_AVAL = 0xFFFFFFFF;
/** @brief OTA_AVAL 常量定义 */
constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用
// 参数配置
/** @brief PP 常量定义 */
constexpr u8 PP = 11;
/** @brief DIR 常量定义 */
constexpr float DIR = 1.0f; // 默认方向：-1.0f 反转，1.0f 正转
/** @brief REDUCTION_RATIO 常量定义 */
constexpr u16 REDUCTION_RATIO = 25;
/** @brief CURRENT_OFFSET 常量定义 */
constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移
/** @brief HOLD_PWM_DUTY 常量定义 */
constexpr float HOLD_PWM_DUTY = 0.1f;
/** @brief ANGLESENS_OFFSET 常量定义 */
constexpr u16 ANGLESENS_OFFSET = 0;
/** @brief ANGLESENS_CALIBRATION_ANGLE 常量定义 */
constexpr float ANGLESENS_CALIBRATION_ANGLE = 83.0f;
/** @brief KE_FACTOR 常量定义 */
constexpr float KE_FACTOR = 0.0f;
// 角度/速度 α-β 观测器参数
/** @brief ANGLE_KALMAN_INITIAL_STATE 常量定义 */
constexpr float ANGLE_KALMAN_INITIAL_STATE = 0.0f;
/** @brief ANGLE_KALMAN_INITIAL_COVARIANCE 常量定义 */
constexpr float ANGLE_KALMAN_INITIAL_COVARIANCE = 0.0f;
/** @brief ANGLE_KALMAN_PROCESS_NOISE_Q 常量定义 */
constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q = 0.0f;
/** @brief ANGLE_KALMAN_MEASUREMENT_NOISE_R 常量定义 */
constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R = 0.0f;
/** @brief ANGLE_OBSERVER_ALPHA 常量定义 */
constexpr float ANGLE_OBSERVER_ALPHA = 0.75f; // 角度校正增益，越大越跟测量
/** @brief ANGLE_OBSERVER_BETA 常量定义 */
constexpr float ANGLE_OBSERVER_BETA = 0.002f; // 速度校正增益，原0.001微调至0.002
// 旧速度 Kalman 参数保留用于接口兼容；主速度输出现在直接来自 α-β 观测器
/** @brief VELOCITY_KALMAN_INITIAL_STATE 常量定义 */
constexpr float VELOCITY_KALMAN_INITIAL_STATE = 0.0f;
/** @brief VELOCITY_KALMAN_INITIAL_COVARIANCE 常量定义 */
constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-3f;
/** @brief VELOCITY_KALMAN_PROCESS_NOISE_Q 常量定义 */
constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
5e-4f; // 略增大，减少拖尾
/** @brief VELOCITY_KALMAN_MEASUREMENT_NOISE_R 常量定义 */
constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
0.05f; // 明显小于旧值，避免压得太死
// 电流卡尔曼滤波器参数
/** @brief CURRENT_KALMAN_INITIAL_STATE 常量定义 */
constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
/** @brief CURRENT_KALMAN_INITIAL_COVARIANCE 常量定义 */
constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f; // 增大初始协方差
/** @brief CURRENT_KALMAN_PROCESS_NOISE_Q 常量定义 */
constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声
/** @brief CURRENT_KALMAN_MEASUREMENT_NOISE_R 常量定义 */
constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声
// PID 配置
/** @brief DEFAULT_current_loop_p 常量定义 */
constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流 P
/** @brief current_loop_Ki 常量定义 */
constexpr float current_loop_Ki = 0.55f; // 电流 P
/** @brief current_loop_Kd 常量定义 */
constexpr float current_loop_Kd = 3e-4f; // 电流 P
/** @brief DEFAULT_vel_loop_p 常量定义 */
constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度 P
/** @brief vel_loop_Ku 常量定义 */
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
/** @brief vel_loop_Tu 常量定义 */
constexpr float vel_loop_Tu = 17.0f * 1e-3f;
/** @brief vel_loop_Ki 常量定义 */
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
/** @brief vel_loop_Kd 常量定义 */
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
/** @brief DEFAULT_vel_angloop_p 常量定义 */
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度 - 角度 P
/** @brief vel_angloop_Ki 常量定义 */
constexpr float vel_angloop_Ki = 5.0f; // 速度 速度 - 角度 I
/** @brief vel_angloop_Kd 常量定义 */
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度 - 角度 D
/** @brief DEFAULT_angle_angloop_p 常量定义 */
constexpr float DEFAULT_angle_angloop_p =
150.0f / 100 * 12; // 角度 速度 - 角度 P
/** @brief ang_angloop_Ki 常量定义 */
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度 - 角度 I
/** @brief ang_angloop_Kd 常量定义 */
constexpr float ang_angloop_Kd = 8e-04f; // 角度 速度 - 角度 D
/** @brief DEFAULT_angle_loop_p 常量定义 */
constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度 P
/** @brief DEFAULT_angle_MIT_p 常量定义 */
constexpr float DEFAULT_angle_MIT_p = 100.0f / 100 * 12; // 角度 MIT P
/** @brief angle_MIT_Ki 常量定义 */
constexpr float angle_MIT_Ki = 0.0f;
/** @brief angle_MIT_Kd 常量定义 */
constexpr float angle_MIT_Kd = 8e-04f;
/** @brief DEFAULT_vel_MIT_p 常量定义 */
constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
/** @brief vel_MIT_Ki 常量定义 */
constexpr float vel_MIT_Ki = 15.0f;
/** @brief vel_MIT_Kd 常量定义 */
constexpr float vel_MIT_Kd = 1e-04f;
/** @brief DEFAULT_current_MIT_p 常量定义 */
constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
/** @brief current_MIT_Ki 常量定义 */
constexpr float current_MIT_Ki = 0.1f;
/** @brief current_MIT_Kd 常量定义 */
constexpr float current_MIT_Kd = 3e-4f;
// 常量定义
/** @brief SIZE_FLOAT 常量定义 */
constexpr u8 SIZE_FLOAT = sizeof(float);
/** @brief SIZE_U32 常量定义 */
constexpr u8 SIZE_U32 = sizeof(uint32_t);
/** @brief ADC_MAX 常量定义 */
constexpr float ADC_MAX = 65536.0f;
/** @brief PI 常量定义 */
constexpr float PI = 3.14159265358979323846f;
/** @brief INV_PI_3 常量定义 */
constexpr float INV_PI_3 = 3.0f / PI;
/** @brief PI_3 常量定义 */
constexpr float PI_3 = PI / 3.0f;
/** @brief _1_SQRT3 常量定义 */
constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
/** @brief _3PI_2 常量定义 */
constexpr float _3PI_2 = 4.71238898038f;
/** @brief SQRT3 常量定义 */
constexpr float SQRT3 = 1.7320508075688772f;
/** @brief SQRT3DV2 常量定义 */
constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
/** @brief COS_PI_3 常量定义 */
constexpr float COS_PI_3 = 0.5f;
/** @brief ANGLE_FACTOR 常量定义 */
constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
/** @brief DEG2RAD 常量定义 */
constexpr float DEG2RAD = 1.0f / 180.0f * PI;
/** @brief RAD2DEG 常量定义 */
constexpr float RAD2DEG = 180.0f / PI; // 修正 RAD2DEG 定义，应该是 180/π
/** @brief DEFAULT_VALUE 常量定义 */
constexpr u8 DEFAULT_VALUE = 0;
/** @brief CONFIG_SAVED_NUM 常量定义 */
constexpr u8 CONFIG_SAVED_NUM = 6;
// 三角函数快速计算 LUT
/** @brief LUT_SIZE 常量定义 */
constexpr u16 LUT_SIZE = 1024;
/** @brief LUT_MASK 常量定义 */
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
/** @brief LUT_SCALE 常量定义 */
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
// CAN 相关
// 注意：以下为 CAN 基础 ID（只保留低 16 位），实际发送/接收时会自动叠加节点 ID
// 到高 16 位 格式：[节点 ID(16 位)][基础 ID(16 位)] 例如：节点 ID=1，发送命令
// ID = (1 << 16) | CANFD_CMD_ID = 0x00010200
/** @brief DEV_CAN0_ID 常量定义 */
constexpr u16 DEV_CAN0_ID = 0x0000;
/** @brief DEV_CAN0_MASK 常量定义 */
constexpr u16 DEV_CAN0_MASK = 0x0000;
/** @brief DEV_CAN_LOOPBACK_ID 常量定义 */
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
// 反馈基础 ID
/** @brief CANFD_STATUS_ID 常量定义 */
constexpr u16 CANFD_STATUS_ID = 0x0100; // 状态反馈基础 ID
/** @brief CANFD_CONFIG_ID 常量定义 */
constexpr u16 CANFD_CONFIG_ID = 0x0101; // 配置信息基础 ID
/** @brief CANFD_VERSION_ID 常量定义 */
constexpr u16 CANFD_VERSION_ID = 0x00FF; // 版本信息基础 ID
// 命令基础 ID
/** @brief CANFD_CMD_ID 常量定义 */
constexpr u16 CANFD_CMD_ID = 0x0200; // 控制命令基础 ID
/** @brief CANFD_RESET_ID 常量定义 */
constexpr u16 CANFD_RESET_ID = 0x0301; // 重置命令基础 ID
// 节点 ID 管理基础 ID
/** @brief CANFD_SET_NODE_ID 常量定义 */
constexpr u16 CANFD_SET_NODE_ID = 0x0400; // 设置节点 ID 基础 ID
/** @brief CANFD_GET_NODE_ID 常量定义 */
constexpr u16 CANFD_GET_NODE_ID = 0x0401; // 查询节点 ID 基础 ID
// OTA 相关基础 ID 不受到node ID影响
/** @brief CANFD_BOOT_ID 常量定义 */
constexpr u16 CANFD_BOOT_ID = 0x0000; // BOOT 模式基础 ID (进入 BOOT 使用此 ID)
/** @brief CANFD_OTA_REQ_ID 常量定义 */
constexpr u16 CANFD_OTA_REQ_ID = 0x0100; // OTA 请求基础 ID
/** @brief CANFD_OTA_DATA_ID 常量定义 */
constexpr u16 CANFD_OTA_DATA_ID = 0x0101; // OTA 数据基础 ID
/** @brief CANFD_OTA_ACK_ID 常量定义 */
constexpr u16 CANFD_OTA_ACK_ID = 0x0102; // OTA 确认基础 ID
/** @brief CANFD_OTA_WAIT_ID 常量定义 */
constexpr u16 CANFD_OTA_WAIT_ID = 0x0103; // OTA 等待基础 ID
// ADC
/** @brief ADC_FILTER_SIZE 常量定义 */
constexpr u8 ADC_FILTER_SIZE = 6;
/** @brief BLOCK_SIZE 常量定义 */
constexpr u8 BLOCK_SIZE = ADC_FILTER_SIZE * SIZE_FLOAT;
/** @brief ERROR_ADC_VAL 常量定义 */
constexpr float ERROR_ADC_VAL = 255.0f;
/** @brief MAX_TEMP 常量定义 */
constexpr float MAX_TEMP = 150.0f;
/** @brief MIN_TEMP 常量定义 */
constexpr float MIN_TEMP = -40.0f;
/** @brief MAX_CURRENT 常量定义 */
constexpr float MAX_CURRENT = 40.0f;
/** @brief MIN_CURRENT 常量定义 */
constexpr float MIN_CURRENT = -40.0f;
/** @brief MAX_VCC 常量定义 */
constexpr float MAX_VCC = 65.0f;
/** @brief MIN_VCC 常量定义 */
constexpr float MIN_VCC = -0.1f;
// ang_sensor
/** @brief VEL_FILTER_SIZE 常量定义 */
constexpr u16 VEL_FILTER_SIZE = 4;
/** @brief DEFAULT_COMMAND 常量定义 */
constexpr u16 DEFAULT_COMMAND = 0x0000;
/** @brief FAULT_RECEIVE 常量定义 */
constexpr u16 FAULT_RECEIVE = 0xFFFF;
/** @brief ANGLE_WRAP_THRESHOLD 常量定义 */
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
/** @brief ANGLE_WRAP_VALUE 常量定义 */
constexpr float ANGLE_WRAP_VALUE = 360.0f;
/** @brief RAD_FACTOR 常量定义 */
constexpr float RAD_FACTOR = 1.0f / ADC_MAX * 2.0f * PI;
/** @brief ANGLESENS_FACTOR 常量定义 */
constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
// TIMER2-任务调度
/** @brief TIMER2_PRESCALER 常量定义 */
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
/** @brief TIMER2_PERIOD 常量定义 */
constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us
/** @brief MOTOR_PERIOD 常量定义 */
constexpr int MOTOR_PERIOD = 1; // 控制每50us执行一次，无分频
/** @brief TIME_SLICE 常量定义 */
constexpr float TIME_SLICE =
(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
/** @brief TIMER2_FREQUENCY 常量定义 */
constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率
/** @brief MOTOR_FREQUENCY 常量定义 */
constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
// TIMER1-PWM
/** @brief TIMER1_PRESCALER 常量定义 */
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为 120MHz
/** @brief TIMER1_PERIOD 常量定义 */
constexpr int TIMER1_PERIOD = 2999; // 120MHz / (0+1) / (2999+1) = 40kHz计数, 中心对齐实际PWM 20kHz
/** @brief TIMER1_DEFAULT_PULSE 常量定义 */
constexpr int TIMER1_DEFAULT_PULSE = 0;
/** @brief TIMER1_CH3_COMPARE_VALUE 常量定义 */
constexpr int TIMER1_CH3_COMPARE_VALUE =
(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM 周期 41% 位置触发ADC采样
// 齿槽转矩补偿
/** @brief COGGING_TABLE_SIZE 常量定义 */
constexpr u16 COGGING_TABLE_SIZE = 128; // 每电周期采样点数
/** @brief COGGING_COMP_SCALE 常量定义 */
constexpr float COGGING_COMP_SCALE = 0.005f; // 补偿换算系数 V/mA，40mA × 0.005 = 0.2V
// 电压检测参数（基于跌幅检测，不依赖固定额定电压）
@@ -409,13 +250,9 @@ constexpr float COGGING_COMP_SCALE = 0.005f; // 补偿换算系数 V/mA，40mA
// 慢速管理层（power_manage，~10ms周期）：
// - 执行Flash保存
// - 状态恢复判断（电压恢复到基准电压的90%以上）
/** @brief VBUS_DROP_THRESHOLD 常量定义 */
constexpr float VBUS_DROP_THRESHOLD = 5.0f; // 电压跌幅阈值 (V)
/** @brief VBUS_ABSOLUTE_MIN 常量定义 */
constexpr float VBUS_ABSOLUTE_MIN = 12.0f; // 绝对最低电压 (V)
/** @brief VBUS_DEBOUNCE_COUNT 常量定义 */
constexpr u32 VBUS_DEBOUNCE_COUNT = 5; // 快速检测：防抖计数 (5次×100us = 500us)
/** @brief VBUS_RECOVERY_RATIO 常量定义 */
constexpr float VBUS_RECOVERY_RATIO = 0.9f; // 恢复比例（基准电压的90%）
Robot_V2/sys/health.cpp
+2 -1
View File
@@ -16,6 +16,7 @@ extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controller FOC;
extern FlashEEPROM flashEEP;
// 全局热管理控制器单例：由 main 任务周期调用 update()，用于维护过热保护状态。
ThermalController thermalController; // 热量控制器初始化
/** @brief max 宏定义 */
@@ -24,7 +25,7 @@ ThermalController thermalController; // 热量控制器初始化
/** @brief PROTECT_TIME 常量定义 */
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
// 电压管理状态机
// 电压管理状态机：仅 power_manage() 慢速任务访问，文件内封装。
static VoltageState voltage_state = VoltageState::NORMAL;
static float last_vbus_slow = 0.0f; // 上次电压值（慢速任务用）
/**
Robot_V2/sys/initial.cpp
+37 -2
View File
@@ -36,6 +36,7 @@
#endif
extern CAN_transmiter CAN0_transmiter; // CAN 初始化
// 全局版本管理器单例：构造函数只复制 constexpr 版本号常量，不依赖外设初始化。
VersionManager Version;
VersionManager::VersionManager() {
@@ -120,14 +121,48 @@ void VersionManager::setBootVersion(u16 major, u16 minor, u16 patch,
bootVersion.build = build;
}
static void appendVersionChar(char *&out, size_t &remaining, char ch) {
if (remaining > 1U) {
*out++ = ch;
--remaining;
*out = '\0';
}
}
static void appendVersionU16(char *&out, size_t &remaining, u16 value) {
char digits[5]; // u16 最大值 65535，最多 5 位十进制数字
u8 count = 0;
do {
digits[count++] = static_cast<char>('0' + (value % 10U));
value = static_cast<u16>(value / 10U);
} while (value > 0U && count < sizeof(digits));
while (count > 0U) {
appendVersionChar(out, remaining, digits[--count]);
}
}
char *VersionManager::versionToString(const VersionInfo &version, char *buffer,
size_t bufSize) const {
if (buffer == nullptr) {
return nullptr;
}
if (bufSize == 0U) {
return buffer;
}
snprintf(buffer, bufSize, "%d.%d.%d.%d", version.major, version.minor,
version.patch, version.build);
char *out = buffer;
size_t remaining = bufSize;
*out = '\0';
appendVersionU16(out, remaining, version.major);
appendVersionChar(out, remaining, '.');
appendVersionU16(out, remaining, version.minor);
appendVersionChar(out, remaining, '.');
appendVersionU16(out, remaining, version.patch);
appendVersionChar(out, remaining, '.');
appendVersionU16(out, remaining, version.build);
return buffer;
}
Robot_V2/sys/systick.cpp
+7
View File
@@ -50,6 +50,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define SLEEP_DEBUG_PRINT(fmt, ...) ((void)0)
#endif
// SysTick 模块内部状态，仅在本文件访问：
// - delay / systick_ms 由 SysTick 中断维护，用 volatile 保证主循环可见性；
// - DWT 统计变量用于 CPU 占用率采样；
// - static 限制文件作用域，外部只能通过 get_systick_ms()/get_cpu_usage() 读取。
volatile static uint32_t delay;
volatile static uint32_t systick_ms = 0; // 系统运行时间（毫秒）
@@ -253,6 +257,7 @@ void app_prepare_deep_sleep(bool save_data) {
fwdgt_config(0xFFF, FWDGT_PSC_DIV256); // 设置最大超时时间
fwdgt_write_disable();
uint32_t primask = __get_PRIMASK();
__disable_irq();
// 遍历最后一个数字，后一个地址写 +1
for (u32 address = SLEEP_CONFIG_END - SIZE_U32;
@@ -271,6 +276,7 @@ void app_prepare_deep_sleep(bool save_data) {
if (fmc_status != FMC_READY) { // 擦除失败报错
fmc_lock();
flashEEP.EEPROMStatus = FLASH_EEPROM_ERASE_ERROR;
__set_PRIMASK(primask); // 失败返回前恢复进入前的中断状态
return;
}
address = SLEEP_CONFIG_START - SIZE_U32; // 重新开始写入
@@ -289,6 +295,7 @@ void app_prepare_deep_sleep(bool save_data) {
flashEEP.EEPROMStatus = FLASH_EEPROM_WRITE_ERROR;
SLEEP_DEBUG_PRINT("app wrote %08x at %08x ERROR\r\n", SLEEP_ENABLE,
address);
__set_PRIMASK(primask); // 失败返回前恢复进入前的中断状态
return;
}
break;
Robot_V2/tasks.cpp
+172
View File
@@ -0,0 +1,172 @@
/**
* @file tasks.cpp
* @brief 周期任务调度与任务执行实现
*/
#include "tasks.h"
#include "Can.h"
#include "FOC.h"
#include "Filter.h"
#include "adc.h"
#include "ang_sensor.h"
#include "health.h"
#include "systick.h"
extern CAN_transmiter CAN0_transmiter;
extern sampler sampler;
extern angle_sensor KTH7823;
extern EncoderAngleKalman Angle_kalman;
extern ThermalController thermalController;
extern FOC_Controller FOC;
extern u32 motor_runtime_50us;
extern u32 cnt_test;
// 函数指针数组，包含各种控制模式对应的函数
static FOCFunctionPtr control_functions[] = {
&FOC_Controller::drv_hold, // 0: 占空比置
&FOC_Controller::angle_control, // 1: 位置闭环控制
&FOC_Controller::velocity_control, // 2: 速度闭环控制
&FOC_Controller::
angle_control_with_velocity_limit, // 3:
// 角度闭环控制，带速度限制
&FOC_Controller::MIT_Mode, // 4: MIT 模式
&FOC_Controller::current_control, // 5: 电流闭环控制
&FOC_Controller::velocity_open_loop, // 6: 速度开环控制
&FOC_Controller::PWM_out, // 7: 占空比输出
};
/**
* @brief 判断任务标志中是否包含指定任务
* @param[in] currentTasks 当前任务位图快照
* @param[in] task 待检查任务枚举
* @return bool true 表示该任务需要执行
*/
static bool has_task(u32 currentTasks, SystemTask task) {
return (currentTasks & (1u << task)) != 0U;
}
/**
* @brief 控制模式变化时复位所有控制环，避免积分项跨模式继承
* @return void
*/
static void reset_control_loops_on_mode_change(void) {
static int last_control_mode = -1;
if (FOC.control_mode == last_control_mode) return;
FOC.vel_loop.reset();
FOC.angle_loop.reset();
FOC.current_loop.reset();
FOC.vel_inVelAngloop.reset();
FOC.ang_inVelAngloop.reset();
FOC.ang_inMITloop.reset();
FOC.vel_inMITloop.reset();
FOC.current_inMITloop.reset();
last_control_mode = FOC.control_mode;
}
/**
* @brief 执行 50us 电机控制任务，包含快速电压检测和控制函数派发
* @return void
*/
static void run_motor_control_task(void) {
motor_runtime_50us++;
static u8 vcheck_div = 0;
// 快速电压检测每 100us 执行一次，避免每 50us 都读取处理。
if (++vcheck_div >= 2u) {
vcheck_div = 0;
FOC.fast_voltage_check();
}
if (FOC.control_mode >= 8) return;
// 模式合法时再派发控制函数，防止数组越界。
reset_control_loops_on_mode_change();
if (!FOC.is_shut_down) {
(FOC.*control_functions[FOC.control_mode])();
} else {
FOC.drv_stop();
}
}
/**
* @brief 执行高频采样和电机控制相关任务
* @param[in] currentTasks 当前任务位图快照
* @return void
*/
static void run_fast_sample_tasks(u32 currentTasks) {
if (has_task(currentTasks, TASK_CURRENT_UPDATE)) {
sampler.update_current();
}
if (has_task(currentTasks, TASK_ANGLE_UPDATE)) {
KTH7823.update();
FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
}
if (has_task(currentTasks, TASK_VELOCITY_UPDATE)) {
FOC.rt_velocity = Angle_kalman.getVelocity();
}
if (has_task(currentTasks, TASK_MOTOR_CONTROL)) {
run_motor_control_task();
}
}
/**
* @brief 执行温度、热管理、磁检测和母线电压等低频维护任务
* @param[in] currentTasks 当前任务位图快照
* @return void
*/
static void run_housekeeping_tasks(u32 currentTasks) {
if (has_task(currentTasks, TASK_MCU_TEMP_UPDATE)) {
sampler.update_mcu_temp();
}
if (has_task(currentTasks, TASK_DRV_TEMP_UPDATE)) {
sampler.update_drv_temp();
}
if (has_task(currentTasks, TASK_THERMAL_CONTROL)) {
thermalController.update();
}
if (has_task(currentTasks, TASK_MAGNETIC_CHECK)) {
angsens_MG_check();
}
if (has_task(currentTasks, TASK_VBUS_UPDATE)) {
power_manage();
}
}
/**
* @brief 执行 CAN 周期发送任务并喂狗
* @return void
*/
static void run_can_task(void) {
CAN0_transmiter.test_u16 = CAN0_transmiter.get_node_id();
cnt_test = 0;
CAN0_transmiter.can0_send_flag = SET;
CAN0_transmiter.communication_check();
fwdgt_counter_reload();
}
/**
* @brief 根据任务标志调度周期性业务任务
* @param[in] currentTasks 当前任务位图快照
* @return void
*/
void do_task(u32 currentTasks) {
busy_task_enter(); // DWT: 记录忙碌起始周期
cnt_test++;
// 高频任务先执行，保持电机控制优先级。
run_fast_sample_tasks(currentTasks);
// 齿槽转矩校准 tick（DEBUG，平时不启用）
#if FOC_DEBUG
FOC.calibrate_cogging_tick();
#endif
run_housekeeping_tasks(currentTasks);
// CAN 发送放在最后，避免通信处理影响前面的实时控制任务。
if (has_task(currentTasks, TASK_CAN_TRANSMIT)) {
run_can_task();
}
busy_task_exit(); // DWT: 累加忙碌周期
}
Robot_V2/tasks.h
+21
View File
@@ -0,0 +1,21 @@
#ifndef TASKS_H
#define TASKS_H
#include "initial.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 根据任务标志调度周期性业务任务
* @param[in] currentTasks 当前任务位图快照
* @return void
*/
void do_task(u32 currentTasks);
#ifdef __cplusplus
}
#endif
#endif /* TASKS_H */