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

Compare commits

base: cbb/Motor_v2:94d5fda0b65b0027d8c49290b45fb63e9a503152
Branches Tags
cbb/Motor_v2:master
...
compare: cbb/Motor_v2:9955132e0ee416da5be397617def310b71e05dac
Branches Tags
cbb/Motor_v2:master
5 Commits
94d5fda0b6 ... 9955132e0e
Author SHA1 Message Date
cbb 9955132e0e feat(FOC): 添加速度目标突变检测和积分清零机制 
- 在velocity_control()函数中添加目标速度突变检测，当速度变化
  超过5.0f时自动清零积分项
- 在angle_control_with_velocity_limit()函数中添加相同的速度
  突变检测逻辑
- 在MIT_Mode()函数中实现速度环积分清零功能
- 添加reset_integral()方法到PID控制器用于清零积分项

refactor(Filter): 将角度卡尔曼滤波器替换为α-β观测器

- 重构EncoderAngleKalman类，将内部卡尔曼滤波器改为α-β观
  测器算法
- 保留原有的接口名称和参数以维持兼容性
- 添加ANGLE_OBSERVER_ALPHA和ANGLE_OBSERVER_BETA配置参数
- 修改速度获取逻辑，直接从观测器状态获取速度估计值

perf(systick): 实现基于DWT的CPU使用率监测

- 使用DWT周期计数器精确测量任务执行周期
- 添加busy_task_enter()和busy_task_exit()函数追踪忙碌周期
- 实现每秒统计CPU占用率并提供get_cpu_usage()接口
- 在CAN数据包中添加CPU使用率传输字段

fix(Can): 修复CANFD数据包中的整数打包问题

- 将16位整数正确拆分为两个字节进行传输
- 修正了test_u16数据的字节序打包方式

chore(config): 更新软件版本号并优化配置参数

- 将版本号从26.4.26更新到26.5.17
- 调整角度/速度α-β观测器的相关增益参数
 2026-05-17 23:40:53 +08:00
cbb e95673fc4e fix(algorithm): 修改FOC控制器中的角度计算和校准参数 
修正了FOC控制器中电气角度计算的符号错误（减法改为加法），
移除了不必要的空行，并优化了velocity_open_loop函数中的注释
格式。同时修正了角度控制中PID计算的目标角度差值顺序。

BREAKING CHANGE: 角度校准参数从硬编码值改为配置常量。
 2026-05-17 03:54:26 +08:00
cbb d6f784db60 fix(FOC): 修复SVPWM相电压计算中的角度单位转换错误 
SVPWM相电压计算中，arm_sin_cos_f32函数期望接收度数而非弧度，
需要将local_angle乘以RAD2DEG进行单位转换。

同时修正了开环速度控制中的电压计算公式，
将固定电压改为基于目标速度的约束电压，
并调整了电机控制频率配置。
 2026-05-16 21:22:14 +08:00
cbb c6e00e1a49 feat(ADC): 重构ADC采样系统以支持同步电流采集 
- 将ADC缓冲区从6通道减少到3通道，专门用于MCU温度、DRV温度和VBUS监测
- 实现了注入组ADC同步采样，通过TIMER1 TRGO触发三相电流同时采样
- 添加了PWM中心对齐模式以提供更好的采样窗口
- 在FOC控制器中增加了ADC错误检查和三相公共模偏置消除
- 优化了电流采样更新机制，使用中断驱动的数据搬运
- 调整了ADC通道配置，规则组用于后台温度/VBUS采样，注入组用于PWM同步电流采样
 2026-05-16 13:09:57 +08:00
cbb 12b2577d6b feat(can): 实现CANFD协议栈及DMA优化 
- 添加CANFD DMA接收功能，使用32位宽数据传输提升效率
- 实现CANFD收发队列管理，支持异步消息处理防止丢帧
- 重构CAN中断处理机制，采用FIFO0中断配合DMA完成数据接收
- 添加CANFD DLC长度转换表，规范化数据长度处理
- 实现临界区保护机制，确保多线程环境下的数据安全
- 优化发送队列机制，使用邮箱空闲中断进行消息调度
- 添加DMA错误处理，增强系统稳定性
- 移除原有8字节限制，支持CANFD最大64字节数据传输
 2026-05-15 23:04:04 +08:00
15 changed files with 697 additions and 380 deletions
Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+59 -22
View File
@@ -17,7 +17,7 @@ extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern CurrentKalman Current_kalman;
extern u16 adc_value[6]; // ADC DMA缓冲区，用于快速电压检测
extern u16 adc_value[3]; // regular DMA缓冲区：MCU温度 / DRV温度 / VBUS
// 初始变量及函数定义
#define _constrain(amt, low, high) \
@@ -52,8 +52,19 @@ void calibrate_zero_position(void) {
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
float current_c) {
if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
current_c == ERROR_ADC_VAL) {
return Current_kalman.getCurrent();
}
// 去除三相公共模偏置，使 Ia + Ib + Ic 更接近 0
const float current_avg = (current_a + current_b + current_c) / 3.0f;
current_a -= current_avg;
current_b -= current_avg;
current_c -= current_avg;
// 计算电气角度
float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
float angle_el = fmod(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// Clarke变换 (使用三相电流)
float I_alpha, I_beta;
@@ -193,9 +204,9 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
magnitude = voltage_power_supply * limit_percent;
}
// 使用arm_sin_cos_f32计算local_angle的sin和cos
// local_angle 是弧度，arm_sin_cos_f32 期望度数，需转换
float sin_l, cos_l;
arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
arm_sin_cos_f32(local_angle * RAD2DEG, &sin_l, &cos_l);
float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
float T2 = modulation_index * sin_l;
float T0 = 1.0f - T1 - T2;
@@ -238,7 +249,6 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
duty_c = 0.5f;
break;
}
// 设置PWM输出
Set_3Pwm(duty_a, duty_b, duty_c);
}
@@ -351,8 +361,8 @@ void FOC_Controller::update_power(float voltage) {
* - VBUS_DEBOUNCE_COUNT: 防抖计数 (默认5次)
*/
void FOC_Controller::fast_voltage_check(void) {
// 直接从DMA缓冲区读取最新ADC值（通道5是Vbus）
float current_vbus = adc_value[5] * (3.3f / 4096.0f) * 11.0f;
// 直接从DMA缓冲区读取最新ADC值（regular rank2 是 VBUS）
float current_vbus = adc_value[2] * (3.3f / 4096.0f) * 11.0f;
// 更新基准电压（峰值检测）
// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
@@ -398,18 +408,16 @@ void FOC_Controller::fast_voltage_check(void) {
* @return float: q轴电压
*/
float FOC_Controller::velocity_open_loop() {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
MOTOR_FREQUENCY / 2,
360.0f);
// 获取电机轴角度
float shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
shaft_angle =
fmod(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY,
360.0f);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = voltage_power_supply / 2.0f;
// 因为在开环控制中，Ud 始终为 0，只控制 Uq
const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f,
2.0f, voltage_power_supply * 0.5f);
setSVPWM_PhaseVoltage(Uq, 0.0f);
return limit_voltage;
}
@@ -422,7 +430,7 @@ float FOC_Controller::velocity_open_loop() {
*/
float FOC_Controller::angle_control() {
// PID 控制计算
float Uq = angle_loop(rt_angle - target_angle);
float Uq = angle_loop(target_angle - rt_angle);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -440,10 +448,19 @@ float FOC_Controller::angle_control() {
* @return float: 当前速度
*/
float FOC_Controller::velocity_control() {
// PID 控制计算，使用 DIR 参数调整速度误差的符号
// DIR = -1.0f 时：速度误差 = -(target_velocity - rt_velocity) = rt_velocity - target_velocity
// DIR = 1.0f 时：速度误差 = target_velocity - rt_velocity
float Uq = vel_loop((target_velocity - rt_velocity) * DIR * REDUCTION_RATIO);
// 速度误差计算
float velocity_error = (target_velocity - rt_velocity) * DIR * REDUCTION_RATIO;
// 目标速度突变检测：target 大幅变化时清积分
static float last_target_vel = 0.0f;
const float target_delta = target_velocity - last_target_vel;
if (fabsf(target_delta) > 5.0f) {
vel_loop.reset_integral();
}
last_target_vel = target_velocity;
// PID 控制计算
float Uq = vel_loop(velocity_error);
CAN0_transmiter.testfloat_union.f = Uq;
// 获取电机轴角度
@@ -499,7 +516,19 @@ float FOC_Controller::angle_control_with_velocity_limit() {
} else {
vel_inVelAngloop.set_P(vel_angloop_p);
}
float Uq = vel_inVelAngloop(target_velocity - rt_velocity)* DIR;
float velocity_error = target_velocity - rt_velocity;
// 目标速度突变检测：target 大幅变化时清积分
static float last_vang_target_vel = 0.0f;
if (fabsf(target_velocity - last_vang_target_vel) > 5.0f) {
vel_inVelAngloop.reset_integral();
}
last_vang_target_vel = target_velocity;
static float last_velocity_error = 0.0f;
last_velocity_error = velocity_error;
float Uq = vel_inVelAngloop(velocity_error)* DIR;
CAN0_transmiter.testfloat_union.f = Uq;
// // 实现力度控制：如果电流超过目标电流，则限制输出
@@ -541,6 +570,14 @@ float FOC_Controller::MIT_Mode() {
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = (target_velocity - rt_velocity)* DIR;
// 目标速度变化检测：target 大幅变化时清积分
static float last_mit_target_vel = 0.0f;
if (fabsf(target_velocity - last_mit_target_vel) > 5.0f) {
vel_inMITloop.reset_integral();
}
last_mit_target_vel = target_velocity;
vel_inMITloop.set_P(vel_MIT_p);
float target_current = vel_inMITloop(velocity_error);
Robot_V2/Algorithm/Filter.cpp
+54 -51
View File
@@ -95,79 +95,82 @@ void EncoderVelocityKalman::setMeasurementNoise(float r) {
// EncoderAngleKalman类的实现
/*!
\brief 构造函数，初始化编码器角度卡尔曼滤波器参数
\brief 构造函数，初始化编码器角度 α-β 观测器参数
\param[in] initial_angle 初始角度值 (degree)
\param[in] initial_error_covariance 初始误差协方差
\param[in] process_noise 过程噪声协方差
\param[in] measurement_noise 测量噪声协方差
\param[in] initial_error_covariance 保留参数，当前未使用
\param[in] process_noise 保留参数，当前未使用
\param[in] measurement_noise 保留参数，当前未使用
\retval none
*/
EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
float initial_error_covariance,
float process_noise,
float measurement_noise)
: kalman(initial_angle, initial_error_covariance, process_noise,
measurement_noise),
last_angle(initial_angle), initialized(true),
velocity_last_angle(initial_angle),
velocity_last_us50_counter(motor_runtime_50us) {}
float initial_error_covariance,
float process_noise,
float measurement_noise)
: angle_estimate(initial_angle), velocity_estimate(0.0f),
alpha_gain(ANGLE_OBSERVER_ALPHA), beta_gain(ANGLE_OBSERVER_BETA),
last_update_count(0U), initialized(false) {
(void)initial_error_covariance;
(void)process_noise;
(void)measurement_noise;
}
/*!
\brief 更新卡尔曼滤波器状态
\brief 更新 α-β 观测器状态
\param[in] measured_angle 测量的角度值 (degree)
\retval 估计的角度值 (degree)
\note 实现卡尔曼滤波的预测和更新步骤
*/
float EncoderAngleKalman::update(float measured_angle) {
// 更新last_angle以支持速度计算
last_angle = kalman.getState();
// 1. 预测步骤 - 速度估计
kalman.predict(last_angle);
// 2. 更新步骤 - 使用处理后的角度值更新
float filtered_angle = kalman.update(measured_angle);
return filtered_angle;
}
/*!
\brief 获取当前角度估计值
\retval 当前角度估计值 (degree)
*/
float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
/*!
\brief 获取当前角度估计值
\retval 当前角度估计值 (degree)
*/
float EncoderAngleKalman::getVelocity() const {
float angle_now = kalman.getState();
u32 time_counts = motor_runtime_50us - velocity_last_us50_counter;
float vel = 0.0f;
if (time_counts > 0) {
vel = (angle_now - velocity_last_angle) * TIMER2_FREQUENCY /
time_counts ;
const u32 now_counts = motor_runtime_50us;
if (!initialized) {
angle_estimate = measured_angle;
velocity_estimate = 0.0f;
last_update_count = now_counts;
initialized = true;
return angle_estimate;
}
velocity_last_angle = angle_now;
velocity_last_us50_counter = motor_runtime_50us;
u32 dt_counts = now_counts - last_update_count;
last_update_count = now_counts;
if (dt_counts == 0U) {
dt_counts = 1U;
}
const float dt = TIME_SLICE * static_cast<float>(dt_counts);
const float angle_pred = angle_estimate + velocity_estimate * dt;
const float velocity_pred = velocity_estimate;
const float error = measured_angle - angle_pred;
return vel;
angle_estimate = angle_pred + alpha_gain * error;
velocity_estimate = velocity_pred + (beta_gain / dt) * error;
return angle_estimate;
}
/*!
\brief 设置过程噪声协方差
\param[in] q 过程噪声协方差值
\retval none
\brief 获取当前角度估计值
\retval 当前角度估计值 (degree)
*/
void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
float EncoderAngleKalman::getAngle() const { return angle_estimate; }
/*!
\brief 获取当前速度估计值
\retval 当前速度估计值 (degree/s)
*/
float EncoderAngleKalman::getVelocity() const { return velocity_estimate; }
/*!
\brief 设置测量噪声协方差
\param[in] r 测量噪声协方差值
\brief 设置 α 增益（借用原接口名）
\param[in] q α 增益
\retval none
*/
void EncoderAngleKalman::setProcessNoise(float q) {
alpha_gain = q;
}
/*!
\brief 设置 β 增益（借用原接口名）
\param[in] r β 增益
\retval none
*/
void EncoderAngleKalman::setMeasurementNoise(float r) {
kalman.setMeasurementNoise(r);
beta_gain = r;
}
// CurrentKalman类的实现
Robot_V2/Algorithm/Filter.h
+11 -13
View File
@@ -166,12 +166,12 @@ class EncoderVelocityKalman {
*/
class EncoderAngleKalman {
private:
KalmanFilter<float> kalman;
float angle_estimate;
float velocity_estimate;
float alpha_gain;
float beta_gain;
u32 last_update_count;
bool initialized;
float last_angle;
// 添加成员变量来替代静态变量
mutable u32 velocity_last_us50_counter;
mutable float velocity_last_angle;
public:
EncoderAngleKalman(float initial_angle, float initial_error_covariance,
@@ -183,17 +183,15 @@ class EncoderAngleKalman {
float getVelocity() const;
void setProcessNoise(float q);
float getstate() const { return kalman.getState(); };
float getstate() const { return angle_estimate; };
void setMeasurementNoise(float r);
u8 init(float init_angle, float initial_error_covariance = 1.0f) {
kalman.x = init_angle;
kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
last_angle = init_angle; // 同时重置last_angle
// 重置速度计算相关的变量
velocity_last_angle = init_angle;
extern u32 motor_runtime_50us;
velocity_last_us50_counter = motor_runtime_50us;
(void)initial_error_covariance;
angle_estimate = init_angle;
velocity_estimate = 0.0f;
last_update_count = 0U;
initialized = false;
return 0;
}
};
Robot_V2/Algorithm/pid.cpp
+7
View File
@@ -88,3 +88,10 @@ void PIDController::reset() {
output_prev = 0.0f;
integral_prev = 0.0f;
}
/**
* @brief 仅清除PID积分项，保留输出斜坡和误差历史
*
* @return void
*/
void PIDController::reset_integral() { integral_prev = 0.0f; }
Robot_V2/Algorithm/pid.h
+1
View File
@@ -11,6 +11,7 @@ class PIDController {
void set_P(float pp);
void set_limit(float limit);
void reset(); // 重置PID控制器状态
void reset_integral(); // 仅清除积分项，保留输出斜坡状态
float P; //!< 比例增益(P环增益)
float I; //!< 积分增益（I环增益）
Robot_V2/device/ADC.cpp
+80 -52
View File
@@ -3,6 +3,7 @@
#include "FOC.h"
#include "Filter.h"
#include "config.h"
#include "initial.h"
#include "math.h"
#include "systick.h"
#include <cstdint>
@@ -17,7 +18,14 @@ constexpr float CURRENT_FACTOR =
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controller FOC;
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
u16 adc_value[3] = {0}; // regular DMA: MCU温度 / DRV温度 / VBUS
static volatile u16 adc_current_value[3] = {0}; // injected 同步采样: U / V / W
static volatile u8 adc_current_frame_ready = 0;
static inline float current_adc_to_ma(u16 raw_adc) {
return (raw_adc * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
}
/*!
\brief configure the DMA peripheral
\param[in] none
@@ -41,7 +49,7 @@ void dma_config(void) {
dma_data_parameter.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
dma_data_parameter.memory_width = DMA_MEMORY_WIDTH_16BIT;
dma_data_parameter.direction = DMA_PERIPHERAL_TO_MEMORY;
dma_data_parameter.number = 6;
dma_data_parameter.number = 3;
dma_data_parameter.priority = DMA_PRIORITY_HIGH;
dma_init(DMA0, DMA_CH0, &dma_data_parameter);
@@ -67,38 +75,48 @@ void adc_config(void) {
adc_deinit(ADC0);
/* ADC模式配置 */
adc_mode_config(ADC_MODE_FREE);
/* 使能ADC连续转换功能 */
/* regular 组保留连续扫描，用于温度/VBUS 后台采样 */
adc_special_function_config(ADC0, ADC_CONTINUOUS_MODE, ENABLE);
/* 使能ADC扫描模式 */
adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE);
/* injected 组用于 PWM 同步电流采样，关闭自动串联，独立由 TIMER1 触发 */
adc_special_function_config(ADC0, ADC_INSERTED_CHANNEL_AUTO, DISABLE);
/* ADC数据右对齐配置 */
adc_data_alignment_config(ADC0, ADC_DATAALIGN_RIGHT);
/* 使能ADC温度传感器和内部参考电压 */
adc_tempsensor_vrefint_enable();
/* 配置ADC通道长度 */
adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6);
adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 3);
/* 配置ADC规则通道 */
adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16,
ADC_SAMPLETIME_41POINT5);
adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 4, ADC_CHANNEL_5, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_6, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_6, ADC_SAMPLETIME_28POINT5);
/* ADC触发配置 */
/* regular 组继续软件触发连续转换：本组只服务低速量 */
adc_external_trigger_source_config(ADC0, ADC_REGULAR_CHANNEL,
ADC0_1_EXTTRIG_REGULAR_NONE);
adc_external_trigger_config(ADC0, ADC_REGULAR_CHANNEL, ENABLE);
/* injected 组改为 TIMER1 CH3 导出的 TRGO 触发，实现三相同一采样时刻锁存 */
adc_channel_length_config(ADC0, ADC_INSERTED_CHANNEL, 4);
adc_inserted_channel_config(ADC0, 0, ADC_CHANNEL_3, ADC_SAMPLETIME_55POINT5);
adc_inserted_channel_config(ADC0, 1, ADC_CHANNEL_3, ADC_SAMPLETIME_55POINT5);
adc_inserted_channel_config(ADC0, 2, ADC_CHANNEL_4, ADC_SAMPLETIME_55POINT5);
adc_inserted_channel_config(ADC0, 3, ADC_CHANNEL_5, ADC_SAMPLETIME_55POINT5);
adc_external_trigger_source_config(ADC0, ADC_INSERTED_CHANNEL,
ADC0_1_EXTTRIG_INSERTED_T1_TRGO);
adc_external_trigger_config(ADC0, ADC_INSERTED_CHANNEL, ENABLE);
/* 使能ADC接口 */
adc_enable(ADC0);
/* ADC校准 */
adc_calibration_enable(ADC0);
/* 使能ADC DMA功能 */
adc_dma_mode_enable(ADC0);
/* 使能ADC软件触发 */
/* injected 完成中断负责搬运完整三相样本 */
adc_interrupt_enable(ADC0, ADC_INT_EOIC);
/* 先启动 regular 组连续后台采样 */
adc_software_trigger_enable(ADC0, ADC_REGULAR_CHANNEL);
}
/**
@@ -107,20 +125,22 @@ void adc_config(void) {
u8 sampler::init() {
dma_config(); // DMA采样ADC初始化
adc_config(); // ADC初始化
current_sample_head = 0;
adc_current_frame_ready = 0;
delay_1ms(1);
const float current_init = current_adc_to_ma(
static_cast<u16>((-CURRENT_OFFSET / ADC_SCALE) + 0.5f));
// 初始采样
for (int i = 0; i < ADC_FILTER_SIZE + 1; i++) {
for (int i = 0; i < ADC_FILTER_SIZE; i++) {
mcu_temp_samples[i] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
currU_samples[i] =
(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currV_samples[i] =
(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currW_samples[i] =
(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currU_samples[i] = current_init;
currV_samples[i] = current_init;
currW_samples[i] = current_init;
vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
vbus_samples[i] = adc_value[2] * ADC_SCALE * VCC_FACTOR;
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[i] =
@@ -135,7 +155,7 @@ u8 sampler::init() {
* 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
*/
float sampler::get_mcu_temp() {
tempora_sample = medium_filter(mcu_temp_samples, ADC_FILTER_SIZE + 1);
tempora_sample = medium_filter(mcu_temp_samples, ADC_FILTER_SIZE);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
@@ -151,7 +171,7 @@ float sampler::get_mcu_temp() {
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
float sampler::get_drv_temp() {
tempora_sample = medium_filter(drv_temp_samples, ADC_FILTER_SIZE + 1);
tempora_sample = medium_filter(drv_temp_samples, ADC_FILTER_SIZE);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
@@ -167,24 +187,18 @@ float sampler::get_drv_temp() {
* 通过ADC采样计算各相电流值
*/
float sampler::get_current(char type) {
u8 idx = current_sample_index;
u8 idx = (current_sample_head == 0U) ? (ADC_FILTER_SIZE - 1U)
: (current_sample_head - 1U);
switch (type) {
case 'U': {
// return currU_samples[current_sample_index];
// tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
tempora_sample = currU_samples[idx];
break;
}
case 'V': {
// return currV_samples[current_sample_index];
// tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
tempora_sample = currV_samples[idx];
break;
}
case 'W': {
// return currW_samples[current_sample_index];
// tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
tempora_sample = currW_samples[idx];
break;
}
@@ -207,10 +221,10 @@ float sampler::get_current(char type) {
* 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
*/
float sampler::get_vbus_voltage() {
tempora_sample = medium_filter(vbus_samples, ADC_FILTER_SIZE + 1);
tempora_sample = medium_filter(vbus_samples, ADC_FILTER_SIZE);
// 检查电压读数是否在有效范围内
if (vbus_samples[ADC_FILTER_SIZE / 2] > MAX_VCC or
vbus_samples[ADC_FILTER_SIZE / 2] < MIN_VCC) {
const u8 mid = ADC_FILTER_SIZE / 2;
if (vbus_samples[mid] > MAX_VCC or vbus_samples[mid] < MIN_VCC) {
return ERROR_ADC_VAL;
}
return tempora_sample;
@@ -224,8 +238,8 @@ float sampler::get_vbus_voltage() {
*/
void sampler::update_mcu_temp() {
memmove(mcu_temp_samples, mcu_temp_samples + 1,
ADC_FILTER_SIZE * sizeof(float));
mcu_temp_samples[ADC_FILTER_SIZE] =
(ADC_FILTER_SIZE - 1) * sizeof(float));
mcu_temp_samples[ADC_FILTER_SIZE - 1] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
}
@@ -234,9 +248,9 @@ void sampler::update_mcu_temp() {
*/
void sampler::update_drv_temp() {
memmove(drv_temp_samples, drv_temp_samples + 1,
ADC_FILTER_SIZE * sizeof(float));
(ADC_FILTER_SIZE - 1) * sizeof(float));
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[ADC_FILTER_SIZE] =
drv_temp_samples[ADC_FILTER_SIZE - 1] =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
}
@@ -244,27 +258,41 @@ void sampler::update_drv_temp() {
* @brief 更新相电流
*/
void sampler::update_current() {
u8 idx = current_sample_index;
extern FOC_Controller FOC;
// 优化计算
const float scale = ADC_SCALE * CURRENT_FACTOR;
const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
currU_samples[idx] = adc_value[2] * scale + offset;
currV_samples[idx] = adc_value[3] * scale + offset;
currW_samples[idx] = adc_value[4] * scale + offset;
// 更新索引
current_sample_index++;
if (current_sample_index >= ADC_FILTER_SIZE - 1) {
current_sample_index = 0;
if (adc_current_frame_ready == 0U) {
return;
}
const u8 idx = current_sample_head;
currU_samples[idx] = current_adc_to_ma(adc_current_value[0]);
currV_samples[idx] = current_adc_to_ma(adc_current_value[1]);
currW_samples[idx] = current_adc_to_ma(adc_current_value[2]);
current_sample_head++;
if (current_sample_head >= ADC_FILTER_SIZE) {
current_sample_head = 0;
}
adc_current_frame_ready = 0U;
}
/**
* @brief 更新供电电压
*/
void sampler::update_vbus_voltage() {
memmove(vbus_samples, vbus_samples + 1, ADC_FILTER_SIZE * sizeof(float));
vbus_samples[ADC_FILTER_SIZE] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
memmove(vbus_samples, vbus_samples + 1, (ADC_FILTER_SIZE - 1) * sizeof(float));
vbus_samples[ADC_FILTER_SIZE - 1] = adc_value[2] * ADC_SCALE * VCC_FACTOR;
}
extern "C" void ADC0_1_IRQHandler(void) {
if (adc_interrupt_flag_get(ADC0, ADC_INT_FLAG_EOIC) != RESET) {
/* 丢弃 rank0 dummy，真实样本使用 rank1/2/3，对应 U/V/W 同一 PWM 时刻 */
(void)adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0);
adc_current_value[0] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_1);
adc_current_value[1] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_2);
adc_current_value[2] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_3);
adc_current_frame_ready = 1U;
adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOIC);
extern TaskScheduler scheduler;
scheduler.taskFlags |= (1u << TASK_CURRENT_UPDATE);
}
}
Robot_V2/device/Can.cpp
+339 -186
View File
@@ -37,26 +37,38 @@ extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式
// 添加 DMA 相关变量
dma_parameter_struct dma_rx_parameter;
dma_parameter_struct dma_tx_parameter;
uint8_t can_rx_buffer[64]; // CAN 接收缓冲区（支持CAN FD最大64字节）
uint8_t can_tx_buffer[64]; // CAN 发送缓冲区（支持CAN FD最大64字节）
static uint32_t can_rx_dma_words[16]; // CAN FD接收DMA按32位搬运，最多64字节
static volatile bool can_rx_dma_busy = false;
static can_receive_message_struct can_rx_dma_message;
// CAN接收/发送队列结构
#define CAN_RX_QUEUE_SIZE 32
#define CAN_TX_QUEUE_SIZE 32
// CAN发送队列结构
#define CAN_TX_QUEUE_SIZE 8
typedef struct {
u32 id;
u8 length;
u8 data[64];
u8 valid;
u32 id;
u8 length;
u8 data[64];
} can_rx_queue_item_t;
typedef struct {
can_trasnmit_message_struct message;
u8 valid;
} can_tx_queue_item_t;
static can_rx_queue_item_t rx_queue[CAN_RX_QUEUE_SIZE];
static volatile u8 rx_queue_head = 0;
static volatile u8 rx_queue_tail = 0;
static volatile u8 rx_queue_count = 0;
static can_tx_queue_item_t tx_queue[CAN_TX_QUEUE_SIZE];
static volatile u8 tx_queue_head = 0;
static volatile u8 tx_queue_tail = 0;
static volatile u8 tx_queue_count = 0;
// 发送队列使用的独立消息结构体
static can_trasnmit_message_struct queue_transmit_message;
// CAN FD DLC 9~15对应的非线性payload长度表
static constexpr u8 CANFD_DLC_LENGTH_TABLE[7] = {12, 16, 20, 24, 32, 48, 64};
CAN_transmiter CAN0_transmiter; // CAN 初始化
@@ -288,36 +300,21 @@ void can_dma_init(void) {
dma_struct_para_init(&dma_rx_parameter);
dma_rx_parameter.periph_addr =
(uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)); // CAN 接收数据寄存器地址
dma_rx_parameter.memory_addr = (uint32_t)can_rx_buffer; // 接收缓冲区地址
dma_rx_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; // 外设到内存
dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位
dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位
dma_rx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级
dma_rx_parameter.number = 8; // 传输字节数
dma_rx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增
dma_rx_parameter.memory_addr = (uint32_t)can_rx_dma_words; // 接收 DMA 缓冲区地址
dma_rx_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; // 外设到内存
dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_32BIT; // 内存宽度 32 位
dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_32BIT; // 外设宽度 32 位
dma_rx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级
dma_rx_parameter.number = 16; // 最多 16 个 word = 64 字节
dma_rx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // CAN FIFO 数据端口不递增
dma_rx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增
dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用 DMA 通道 4
dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用 DMA 通道 4
dma_memory_to_memory_enable(DMA0, DMA_CH4); // CAN 无独立 DMA request 时，按帧启动 M2M 读取 FIFO 数据端口
// 配置发送 DMA
dma_struct_para_init(&dma_tx_parameter);
dma_tx_parameter.periph_addr =
(uint32_t)(&CAN_TMI0(CAN0)); // CAN 发送数据寄存器地址
dma_tx_parameter.memory_addr = (uint32_t)can_tx_buffer; // 发送缓冲区地址
dma_tx_parameter.direction = DMA_MEMORY_TO_PERIPHERAL; // 内存到外设
dma_tx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位
dma_tx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位
dma_tx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级
dma_tx_parameter.number = 8; // 传输字节数
dma_tx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增
dma_tx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增
dma_init(DMA0, DMA_CH5, &dma_tx_parameter); // 使用 DMA 通道 5
// 使能 DMA 通道
dma_channel_enable(DMA0, DMA_CH4);
dma_channel_enable(DMA0, DMA_CH5);
// 使能 CAN 接收 DMA
// can_dma_enable(CAN0, CAN_DMA_RECEIVE);
// RX DMA 按帧启动，初始化时不使能 CH4，避免无有效帧时搬运垃圾数据
dma_channel_disable(DMA0, DMA_CH4);
can_rx_dma_busy = false;
memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words));
}
/*!
@@ -390,7 +387,7 @@ void can_config(void) {
*/
void canfd_config(void) {
can_gpio_init();
can_dma_init(); // 初始化 DMA
can_dma_init();
can_parameter_struct can_parameter;
can_fdframe_struct can_fd_parameter;
can_fd_tdc_struct can_fd_tdc_parameter;
@@ -451,11 +448,12 @@ void canfd_config(void) {
/* configure CAN0 NVIC */
nvic_irq_enable(CAN0_RX0_IRQn, 1, 0);
nvic_irq_enable(CAN0_TX_IRQn, 1, 1);
/* enable can receive FIFO0 not empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
// 使能 DMA 中断
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF);
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF | DMA_INT_ERR);
}
/*!
@@ -651,9 +649,19 @@ u8 CAN_transmiter::init() {
/* initialize receive message */
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
// 初始化收发队列和标志位
rx_queue_head = 0;
rx_queue_tail = 0;
rx_queue_count = 0;
tx_queue_head = 0;
tx_queue_tail = 0;
tx_queue_count = 0;
can0_send_flag = RESET;
// 初始化 DMA 缓冲区
memset(can_rx_buffer, 0, sizeof(can_rx_buffer));
memset(can_tx_buffer, 0, sizeof(can_tx_buffer));
memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words));
can_rx_dma_busy = false;
// 初始化节点 ID 为默认值
node_id = DEFAULT_NODE_ID;
@@ -663,6 +671,216 @@ u8 CAN_transmiter::init() {
return 0;
}
static uint32_t CAN_EnterCritical(void) {
uint32_t primask = __get_PRIMASK();
__disable_irq();
return primask;
}
static void CAN_ExitCritical(uint32_t primask) { __set_PRIMASK(primask); }
static u8 CANFD_DlcToLength(u8 dlc) {
return (dlc <= 8U) ? dlc
: ((dlc <= 15U) ? CANFD_DLC_LENGTH_TABLE[dlc - 9U] : 0U);
}
static u8 CANFD_NormalizeLength(u8 length) {
if (length <= 8U)
return length;
for (u8 i = 0; i < sizeof(CANFD_DLC_LENGTH_TABLE); i++) {
if (length <= CANFD_DLC_LENGTH_TABLE[i]) {
return CANFD_DLC_LENGTH_TABLE[i];
}
}
return 0U;
}
static u8 CANFD_WordCount(u8 length) { return (length + 3U) / 4U; }
static void CAN_RX_QueuePushFromIsr(const can_receive_message_struct &message) {
if (rx_queue_count < CAN_RX_QUEUE_SIZE) {
rx_queue[rx_queue_tail].id = message.rx_efid;
rx_queue[rx_queue_tail].length = message.rx_dlen;
if (rx_queue[rx_queue_tail].length > 64) {
rx_queue[rx_queue_tail].length = 64;
}
for (int i = 0; i < rx_queue[rx_queue_tail].length; i++) {
rx_queue[rx_queue_tail].data[i] = message.rx_data[i];
}
rx_queue_tail = (rx_queue_tail + 1) % CAN_RX_QUEUE_SIZE;
rx_queue_count++;
} else {
CAN_DEBUG_PRINT("RX queue full, message dropped\r\n");
}
}
static void CAN_RX_ReleaseFifo0(void) { CAN_RFIFO0(CAN0) |= CAN_RFIFO0_RFD0; }
static u8 CAN_RX_Fifo0PendingCount(void) {
return can_receive_message_length_get(CAN0, CAN_FIFO0);
}
static bool CAN_TX_HasEmptyMailbox(void) {
return (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET) ||
(can_flag_get(CAN0, CAN_FLAG_TME1) != RESET) ||
(can_flag_get(CAN0, CAN_FLAG_TME2) != RESET);
}
static void CAN_RX_ReadMetadata(can_receive_message_struct *message) {
uint8_t dlc;
uint32_t rfifomi = CAN_RFIFOMI(CAN0, CAN_FIFO0);
uint32_t rfifomp = CAN_RFIFOMP(CAN0, CAN_FIFO0);
uint32_t canfd_en = CAN_FDCTL(CAN0) & CAN_FDCTL_FDEN;
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, message);
message->rx_ff = (uint8_t)(CAN_RFIFOMI_FF & rfifomi);
if (CAN_FF_STANDARD == message->rx_ff) {
message->rx_sfid = (uint32_t)(GET_RFIFOMI_SFID(rfifomi));
} else {
message->rx_efid = (uint32_t)(GET_RFIFOMI_EFID(rfifomi));
}
message->rx_ft = (uint8_t)(CAN_RFIFOMI_FT & rfifomi);
message->rx_fi = (uint8_t)(GET_RFIFOMP_FI(rfifomp));
message->fd_flag = (uint8_t)((CAN_RFIFOMP_FDF & rfifomp) >> 7);
dlc = (uint8_t)(GET_RFIFOMP_DLENC(rfifomp));
if ((canfd_en != 0U) && (message->fd_flag != CAN_FDF_CLASSIC)) {
message->rx_dlen = CANFD_DlcToLength(dlc);
message->fd_brs = (uint8_t)((rfifomp & CAN_RFIFOMP_BRS) >> 5);
message->fd_esi = (uint8_t)((rfifomp & CAN_RFIFOMP_ESI) >> 4);
} else {
message->rx_dlen = (dlc <= 8U) ? dlc : 8U;
}
}
static void CAN_RX_StartPayloadDma(const can_receive_message_struct &metadata) {
can_rx_dma_message = metadata;
if (can_rx_dma_message.rx_dlen > 64) {
can_rx_dma_message.rx_dlen = 64;
}
u8 word_count = CANFD_WordCount(can_rx_dma_message.rx_dlen);
if (word_count == 0) {
CAN_RX_QueuePushFromIsr(can_rx_dma_message);
CAN_RX_ReleaseFifo0();
return;
}
can_rx_dma_busy = true;
dma_channel_disable(DMA0, DMA_CH4);
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
dma_periph_address_config(DMA0, DMA_CH4,
(uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)));
dma_memory_address_config(DMA0, DMA_CH4, (uint32_t)can_rx_dma_words);
dma_transfer_number_config(DMA0, DMA_CH4, word_count);
dma_channel_enable(DMA0, DMA_CH4);
}
static void CAN_RX_PumpFifo0(void) {
while (!can_rx_dma_busy) {
if (CAN_RX_Fifo0PendingCount() == 0U) {
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
return;
}
can_receive_message_struct temp_message;
CAN_RX_ReadMetadata(&temp_message);
if (temp_message.rx_ff == CAN_FF_EXTENDED) {
CAN_RX_StartPayloadDma(temp_message);
} else {
CAN_RX_ReleaseFifo0();
CAN_DEBUG_PRINT("Drop standard CAN frame in CANFD path\r\n");
}
}
}
static u8 CAN_RX_QueuePop(can_rx_queue_item_t *item) {
u8 popped = 0U;
uint32_t primask = CAN_EnterCritical();
if (rx_queue_count == 0) {
CAN_ExitCritical(primask);
return 0U;
}
*item = rx_queue[rx_queue_head];
rx_queue_head = (rx_queue_head + 1) % CAN_RX_QUEUE_SIZE;
rx_queue_count--;
popped = 1U;
CAN_ExitCritical(primask);
return popped;
}
static u8 CAN_TX_QueuePush(const can_trasnmit_message_struct &message) {
u8 queued = 0U;
uint32_t primask = CAN_EnterCritical();
if (tx_queue_count < CAN_TX_QUEUE_SIZE) {
tx_queue[tx_queue_tail].message = message;
tx_queue[tx_queue_tail].valid = 1U;
tx_queue_tail = (tx_queue_tail + 1U) % CAN_TX_QUEUE_SIZE;
tx_queue_count++;
queued = 1U;
}
CAN_ExitCritical(primask);
if (queued != 0U) {
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
return 1U;
}
CAN_DEBUG_PRINT("TX queue full, message dropped\r\n");
return 0U;
}
static u8 CAN_TX_QueuePeek(can_trasnmit_message_struct *message) {
u8 has_message = 0U;
uint32_t primask = CAN_EnterCritical();
if (tx_queue_count != 0U) {
*message = tx_queue[tx_queue_head].message;
has_message = 1U;
}
CAN_ExitCritical(primask);
return has_message;
}
static u8 CAN_TX_QueuePop(can_trasnmit_message_struct *message) {
u8 popped = 0U;
uint32_t primask = CAN_EnterCritical();
if (tx_queue_count != 0U) {
*message = tx_queue[tx_queue_head].message;
tx_queue[tx_queue_head].valid = 0U;
tx_queue_head = (tx_queue_head + 1U) % CAN_TX_QUEUE_SIZE;
tx_queue_count--;
popped = 1U;
}
CAN_ExitCritical(primask);
return popped;
}
static void CAN_TX_DisableTmeIfQueueEmpty(void) {
uint32_t primask = CAN_EnterCritical();
if (tx_queue_count == 0U) {
can_interrupt_disable(CAN0, CAN_INTEN_TMEIE);
}
CAN_ExitCritical(primask);
}
static void CAN_TX_ServiceQueue(void) {
can_trasnmit_message_struct queued_message;
while (CAN_TX_HasEmptyMailbox()) {
if (CAN_TX_QueuePeek(&queued_message) == 0U) {
CAN_TX_DisableTmeIfQueueEmpty();
return;
}
if (can_message_transmit(CAN0, &queued_message) == CAN_NOMAILBOX) {
CAN_DEBUG_PRINT(
"Mailbox became unavailable while draining TX queue\r\n");
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
return;
}
CAN_TX_QueuePop(&queued_message);
}
}
/*!
\brief CAN 数据更新
*/
@@ -683,6 +901,7 @@ void CAN_transmiter::update(void) {
EEPROMStatus = flashEEP.EEPROMStatus;
Target_Control_Mode = FOC.control_mode;
controller_Mode = FOC.control_mode;
cpu_usage = get_cpu_usage();
// 使用简单平均滤波的电流值
u_current_union.f = sampler.get_current('U') * 1000;
@@ -720,7 +939,9 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
pack_float_to_bytes(transmit_data, 21, rt_current_union);
pack_float_to_bytes(transmit_data, 25, testfloat_union);
pack_u16_to_bytes(transmit_data, 29, test_u16);
transmit_data[29] = test_u16 & 0xFF;
transmit_data[30] = (test_u16 >> 8) & 0xFF;
transmit_data[31] = cpu_usage;
// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
success_flag +=
CANFD_AsyncSend(build_can_id(CANFD_STATUS_ID), 32, transmit_data);
@@ -757,9 +978,15 @@ void CAN_transmiter::communication_check() {
can0_send_flag = RESET;
}
// 处理所有接收到的 CAN 消息（从环形缓冲区中）
can_receive_message_struct current_message;
while (dequeue_rx_message(current_message)) {
// 处理所有接收到的 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);
// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
u32 base_id = current_message.rx_efid & 0xFFFF;
@@ -771,11 +998,14 @@ void CAN_transmiter::communication_check() {
// 判断是广播消息还是其他节点的命令
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);
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");
CAN_DEBUG_PRINT(
"Received base_id=0x0000 from self, jumping to BOOT\r\n");
motor_sleep();
flashEEP.save_system_config(false);
fmc_unlock();
@@ -800,8 +1030,7 @@ void CAN_transmiter::communication_check() {
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 字节时直接使用
new_node_id = current_message.rx_data[0]; // 只发送 1 字节时直接使用
}
u16 old_node_id = node_id; // 保存旧节点 ID 用于构建响应 ID
CAN_DEBUG_PRINT(
@@ -811,7 +1040,7 @@ void CAN_transmiter::communication_check() {
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[0] = 0x01; // 成功
transmit_data[1] = (u8)(new_node_id & 0xFF); // 新 ID 低字节
transmit_data[2] =
(u8)((new_node_id >> 8) & 0xFF); // 新 ID 高字节
@@ -819,36 +1048,39 @@ void CAN_transmiter::communication_check() {
// 使用旧节点 ID 发送响应
node_id = old_node_id;
u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
transmit_data);
// 然后保存新节点 ID 到 Flash
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);
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);
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);
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);
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);
transmit_data);
// 保持原 ID 不变
}
continue; // SET_NODE_ID 命令处理完跳过后续逻辑
@@ -919,7 +1151,7 @@ void CAN_transmiter::communication_check() {
if (first_byte >> 3 & 0x01) {
KTH7823.angle_set(0);
Angle_kalman.init(0); // 角度滤波
Vel_kalman.init(0); // 角度滤波
Vel_kalman.init(0); // 角度滤波
}
// 休眠
if (eigth_byte & 0x80 >> 7) {
@@ -939,7 +1171,7 @@ void CAN_transmiter::communication_check() {
delay_1ms(1);
if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2,
transmit_data);
transmit_data);
}
}
// 查询节点 ID 命令 (基础 ID: 0x0402)
@@ -948,7 +1180,7 @@ void CAN_transmiter::communication_check() {
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);
current_message.rx_efid, node_id);
}
}
}
@@ -982,17 +1214,10 @@ u8 CAN_transmiter::CAN_Wakeup() {
* @retval 无
*/
void CAN0_RX0_IRQHandler(void) {
// 检查是否是 FIFO0 消息挂起中断
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFL0) != RESET) {
// 清除中断标志
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0);
// 直接读取消息（包括数据），不依赖DMA
can_receive_message_struct temp_message;
can_message_receive(CAN0, CAN_FIFO0, &temp_message);
// 将接收到的消息加入环形缓冲区
CAN0_transmiter.enqueue_rx_message_from_isr(temp_message);
// FIFO0 非空时，关中断并持续抽空 FIFO；DMA 完成后由 DMA ISR 继续续传。
if (CAN_RX_Fifo0PendingCount() != 0U) {
can_interrupt_disable(CAN0, CAN_INTEN_RFNEIE0);
CAN_RX_PumpFifo0();
}
// 检查其他可能的中断源
@@ -1014,14 +1239,32 @@ void CAN0_RX0_IRQHandler(void) {
* @retval 无
*/
void DMA0_Channel4_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_ERR)) {
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
dma_channel_disable(DMA0, DMA_CH4);
if (can_rx_dma_busy) {
CAN_RX_ReleaseFifo0();
can_rx_dma_busy = false;
}
CAN_DEBUG_PRINT("CAN RX DMA error, frame dropped\r\n");
CAN_RX_PumpFifo0();
return;
}
if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_FTF)) {
// 清除 DMA 传输完成中断标志
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
// 将 DMA 缓冲区的数据复制到接收消息结构体
for (int i = 0; i < CAN0_transmiter.receive_message.rx_dlen && i < 8;
i++) {
CAN0_transmiter.receive_message.rx_data[i] = can_rx_buffer[i];
dma_channel_disable(DMA0, DMA_CH4);
if (!can_rx_dma_busy) {
CAN_RX_PumpFifo0();
return;
}
memcpy(can_rx_dma_message.rx_data, can_rx_dma_words,
can_rx_dma_message.rx_dlen);
CAN_RX_QueuePushFromIsr(can_rx_dma_message);
CAN_RX_ReleaseFifo0();
can_rx_dma_busy = false;
CAN_RX_PumpFifo0();
}
}
@@ -1030,12 +1273,7 @@ void DMA0_Channel4_IRQHandler(void) {
* @retval 无
*/
void DMA0_Channel5_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH5, DMA_INT_FLAG_FTF)) {
// DMA 传输完成，可以在此处执行后续处理
// 清除 DMA 传输完成中断标志
dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_FTF);
// 可以在此添加后续处理逻辑，如发送下一个数据包等
}
// 保留空实现：发送链路改用 CAN mailbox + TME 中断队列。
}
/**
@@ -1087,50 +1325,6 @@ u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
return 1;
}
/**
* @brief 向发送队列添加消息
* @param id CAN消息ID
* @param length CAN消息长度
* @param data CAN消息数据
* @retval 1=成功, 0=队列满
*/
static u8 CAN_TX_QueuePush(u32 id, u8 length, u8 *data) {
if (tx_queue_count >= CAN_TX_QUEUE_SIZE) {
CAN_DEBUG_PRINT("TX queue full, message dropped\r\n");
return 0; // 队列满，丢弃消息
}
tx_queue[tx_queue_tail].id = id;
tx_queue[tx_queue_tail].length = length;
for (int i = 0; i < length && i < 64; i++) {
tx_queue[tx_queue_tail].data[i] = data[i];
}
tx_queue[tx_queue_tail].valid = 1;
tx_queue_tail = (tx_queue_tail + 1) % CAN_TX_QUEUE_SIZE;
tx_queue_count++;
return 1;
}
/**
* @brief 从发送队列取出消息
* @param item 指向队列项的指针
* @retval 1=成功, 0=队列空
*/
static u8 CAN_TX_QueuePop(can_tx_queue_item_t *item) {
if (tx_queue_count == 0) {
return 0; // 队列空
}
*item = tx_queue[tx_queue_head];
tx_queue[tx_queue_head].valid = 0;
tx_queue_head = (tx_queue_head + 1) % CAN_TX_QUEUE_SIZE;
tx_queue_count--;
return 1;
}
/**
* @brief 异步 CANFD 数据发送 (使用队列机制防止丢帧)
* @param id CAN 消息 ID
@@ -1138,42 +1332,32 @@ static u8 CAN_TX_QueuePop(can_tx_queue_item_t *item) {
* @param data CAN 消息数据
*/
u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
u8 valid_length = length;
u8 valid_length = 0U;
// 检查 CANFD 模式是否启用
if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
if ((CAN_FDCTL(CAN0) & CAN_FDCTL_FDEN) == 0U) {
CAN_DEBUG_PRINT("CANFD mode not enabled\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
if (data == nullptr && length != 0U) {
CAN_DEBUG_PRINT("Invalid data pointer\r\n");
return 0;
}
// 检查数据长度是否符合 CANFD 规范
if (length > 8) {
if (length > 8 and length < 13) {
valid_length = 12;
} else if (length > 12 and length < 17) {
valid_length = 16;
} else if (length > 16 and length < 21) {
valid_length = 20;
} else if (length > 20 and length < 25) {
valid_length = 24;
} else if (length > 24 and length < 33) {
valid_length = 32;
} else if (length > 32 and length < 49) {
valid_length = 48;
} else if (length > 48 and length < 65) {
valid_length = 64;
} else {
CAN_DEBUG_PRINT("CANFD data length error\r\n");
return 0;
}
// 检查数据长度是否符合 CANFD 规范，并映射到合法 DLC 长度
if (length == 0U) {
valid_length = 0U;
} else {
valid_length = CANFD_NormalizeLength(length);
}
if ((length != 0U) && (valid_length == 0U)) {
CAN_DEBUG_PRINT("CANFD data length error\r\n");
return 0;
}
/* 配置发送消息参数 */
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
transmit_message.tx_efid = id; // 扩展帧 ID
@@ -1183,12 +1367,10 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD 帧格式
transmit_message.fd_brs = ENABLE; // 使能比特率切换
transmit_message.fd_esi = DISABLE; // 错误状态指示
// 复制数据到发送缓冲区
memset(transmit_message.tx_data, 0, sizeof(transmit_message.tx_data));
for (int i = 0; i < valid_length; i++) {
if (i < length) {
transmit_message.tx_data[i] = *(data + i);
} else {
transmit_message.tx_data[i] = 0;
}
}
@@ -1199,7 +1381,7 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
if (transmit_mailbox == CAN_NOMAILBOX) {
// 没有可用的 mailbox，将消息加入发送队列
CAN_DEBUG_PRINT("No available mailbox, adding to TX queue\r\n");
return CAN_TX_QueuePush(id, valid_length, data);
return CAN_TX_QueuePush(transmit_message);
}
// 启用 CAN 发送中断以检测传输完成（用于队列发送）
@@ -1211,36 +1393,7 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
* @brief CAN 传输中断处理函数（支持队列发送）
*/
void CAN0_TX_IRQHandler(void) {
// 检查是否有发送邮箱空闲
if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET ||
can_flag_get(CAN0, CAN_FLAG_TME1) != RESET ||
can_flag_get(CAN0, CAN_FLAG_TME2) != RESET) {
can_tx_queue_item_t next_msg;
// 尝试从队列中取出下一条消息发送
if (CAN_TX_QueuePop(&next_msg)) {
// 配置并发送队列中的消息（使用独立的队列消息结构体）
queue_transmit_message.tx_ft = CAN_FT_DATA;
queue_transmit_message.tx_ff = CAN_FF_EXTENDED;
queue_transmit_message.tx_efid = next_msg.id;
queue_transmit_message.tx_dlen = next_msg.length;
queue_transmit_message.fd_flag = CAN_FDF_FDFRAME;
queue_transmit_message.fd_brs = ENABLE;
queue_transmit_message.fd_esi = DISABLE;
for (int i = 0; i < next_msg.length; i++) {
queue_transmit_message.tx_data[i] = next_msg.data[i];
}
can_message_transmit(CAN0, &queue_transmit_message);
} else {
// 队列为空，禁用发送中断
can_interrupt_disable(CAN0, CAN_INTEN_TMEIE);
}
// 清除发送中断标志
can_flag_clear(CAN0, CAN_FLAG_TME0);
can_flag_clear(CAN0, CAN_FLAG_TME1);
can_flag_clear(CAN0, CAN_FLAG_TME2);
if (CAN_TX_HasEmptyMailbox()) {
CAN_TX_ServiceQueue();
}
}
Robot_V2/device/Can.h
+2
View File
@@ -57,6 +57,7 @@ u8 can_gpio_init(void); // CAN 引脚初始化
void can_config(void); // CAN 初始化
void canfd_config(void); // CANFD 初始化
void can_dma_init(void); // CAN DMA 初始化
void CAN0_TX_IRQHandler(void); // CAN 发送中断处理函数
void CAN0_RX0_IRQHandler(void); // CAN 接收中断处理函数
void DMA0_Channel4_IRQHandler(void); // DMA 通道 4 中断处理函数 (CAN 接收)
void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发送)
@@ -87,6 +88,7 @@ class CAN_transmiter {
u16 Power_Voltage = 0; // 电源电压
u16 limit_voltage = 0; // 限制电压
u8 cpu_usage = 0; // CPU 使用率（%）
float_union_t rt_angle_union = {0}; // 实时角度
float_union_t rt_velocity_union = {0}; // 实时速度
Robot_V2/device/Timer.cpp
+11 -14
View File
@@ -10,7 +10,7 @@
#include "initial.h"
volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1
extern u16 adc_value[7];
extern u16 adc_value[3];
extern FOC_Controller FOC;
extern TaskScheduler scheduler;
extern CAN_transmiter CAN0_transmiter;
@@ -59,6 +59,7 @@ void nvic_config(void) {
nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
/* 使能 TIMER2 中断，设置抢占优先级为 1，子优先级为 1 */
nvic_irq_enable(TIMER2_IRQn, 0, 1);
nvic_irq_enable(ADC0_1_IRQn, 0, 0);
nvic_irq_enable(TIMER1_IRQn, 0, 2);
}
@@ -216,7 +217,8 @@ void timer1_pwm_config(void) {
// 配置 TIMER1 基本参数
timer_struct_para_init(&timer_initpara);
timer_initpara.prescaler = TIMER1_PRESCALER; // 预分频器
timer_initpara.counterdirection = TIMER_COUNTER_UP; // 向上计数
timer_initpara.alignedmode = TIMER_COUNTER_CENTER_BOTH; // 中心对齐，给采样留对称平台窗
timer_initpara.counterdirection = TIMER_COUNTER_UP;
timer_initpara.period = TIMER1_PERIOD; // 计数周期
timer_initpara.clockdivision = TIMER_CKDIV_DIV1;
timer_init(TIMER1, &timer_initpara);
@@ -231,9 +233,9 @@ void timer1_pwm_config(void) {
timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocinitpara);
timer_channel_output_config(TIMER1, TIMER_CH_2, &timer_ocinitpara);
// 配置第四个通道 (CH3) 用于在 PWM 周期 x% 位置触发中断
// 配置第四个通道 (CH3) 为固定采样点，只用于导出 ADC 触发
timer_ocinitpara.outputstate =
TIMER_CCX_DISABLE; // 不输出 PWM 波形，仅用于触发中断
TIMER_CCX_DISABLE; // 不输出 PWM 波形，仅用于比较事件
timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
timer_channel_output_config(TIMER1, TIMER_CH_3, &timer_ocinitpara);
@@ -244,13 +246,16 @@ void timer1_pwm_config(void) {
timer_channel_output_mode_config(TIMER1, TIMER_CH_3,
TIMER_OC_MODE_PWM0); // CH3 也设为 PWM 模式
/* 将 CH3 预装载比较点作为 TRGO 导出，供 ADC injected 组同步采样 */
timer_master_output_trigger_source_select(TIMER1, TIMER_TRI_OUT_SRC_O3CPRE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
TIMER1_DEFAULT_PULSE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1,
TIMER1_DEFAULT_PULSE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
TIMER1_DEFAULT_PULSE);
// 设置 CH3 比较值为 PWM 周期 x% 位置
// 设置 CH3 为固定 ADC 采样点，避免与三相 PWM duty 耦合
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_3,
TIMER1_CH3_COMPARE_VALUE);
@@ -258,26 +263,18 @@ void timer1_pwm_config(void) {
timer_primary_output_config(TIMER1, ENABLE);
timer_auto_reload_shadow_enable(TIMER1);
// 使能 CH3 比较中断
timer_interrupt_enable(TIMER1, TIMER_INT_CH3);
// 启动 TIMER1
timer_enable(TIMER1);
}
// 添加 TIMER1-PWM 中断处理函数
// CH3 现在仅作为 TRGO 采样点保留；采样完成由 ADC injected 中断接管
/**
* @brief TIMER1 中断处理函数
* @return void
*/
void TIMER1_IRQHandler(void) {
// 检查 CH3 比较中断标志并处理
if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) {
// 清除 CH3 比较中断标志
timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3);
scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE);
// 例如触发 ADC 采样或其他操作
}
}
Robot_V2/device/adc.h
+9 -7
View File
@@ -9,13 +9,13 @@ extern "C" {
class sampler {
private:
float vbus_samples[ADC_FILTER_SIZE + 1];
float currU_samples[ADC_FILTER_SIZE + 1];
float currV_samples[ADC_FILTER_SIZE + 1];
float currW_samples[ADC_FILTER_SIZE + 1];
float mcu_temp_samples[ADC_FILTER_SIZE + 1];
float drv_temp_samples[ADC_FILTER_SIZE + 1];
u8 current_sample_index;
float vbus_samples[ADC_FILTER_SIZE];
float currU_samples[ADC_FILTER_SIZE];
float currV_samples[ADC_FILTER_SIZE];
float currW_samples[ADC_FILTER_SIZE];
float mcu_temp_samples[ADC_FILTER_SIZE];
float drv_temp_samples[ADC_FILTER_SIZE];
u8 current_sample_head;
float tempora_sample;
@@ -32,6 +32,8 @@ class sampler {
float get_current(char);
};
void ADC0_1_IRQHandler(void);
#ifdef __cplusplus
}
#endif
Robot_V2/device/ang_sensor.h
+1 -1
View File
@@ -14,7 +14,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
class angle_sensor {
public:
float calibration_angle = 195.0f;
float calibration_angle = ANGLESENS_CALIBRATION_ANGLE;
u8 init(void);
float read_sensorAngle(void);
float update(void);
Robot_V2/main.cpp
+9 -2
View File
@@ -63,6 +63,11 @@ int main(void) {
scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志
__enable_irq();
if (currentTasks == 0U) {
idle_task_count();
continue;
}
do_task(currentTasks);
}
return 0;
@@ -92,6 +97,7 @@ u8 hardware_init(void) {
}
void do_task(u32 currentTasks) {
busy_task_enter(); // DWT: 记录忙碌起始周期
cnt_test++;
// 按照优先级和重要性处理任务
if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
@@ -101,8 +107,8 @@ void do_task(u32 currentTasks) {
KTH7823.update();
FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
}
if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度观测值
FOC.rt_velocity = Angle_kalman.getVelocity();
}
if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务 (最高优先级)
motor_runtime_50us++;
@@ -163,4 +169,5 @@ void do_task(u32 currentTasks) {
CAN0_transmiter.communication_check();
fwdgt_counter_reload(); // 看门狗复位
}
busy_task_exit(); // DWT: 累加忙碌周期
}
Robot_V2/sys/config.h
+16 -20
View File
@@ -16,8 +16,8 @@
// 版本号定义 - 可根据实际项目进行调整
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
constexpr u16 SOFTWARE_VERSION_MINOR = 4;
constexpr u16 SOFTWARE_VERSION_PATCH = 26;
constexpr u16 SOFTWARE_VERSION_MINOR = 5;
constexpr u16 SOFTWARE_VERSION_PATCH = 17;
constexpr u16 SOFTWARE_VERSION_BUILD = 2;
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
@@ -89,29 +89,25 @@ constexpr float DIR = 1.0f; // 默认方向：-1.0f 反转，1.0f 正转
constexpr u16 REDUCTION_RATIO = 25;
constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移
constexpr float HOLD_PWM_DUTY = 0.1f;
constexpr u16 ANGLESENS_OFFSET = +15;
constexpr u16 ANGLESENS_OFFSET = 0;
constexpr float ANGLESENS_CALIBRATION_ANGLE = 83.0f;
constexpr float KE_FACTOR = 0.0f;
// 角度卡尔曼滤波器参数
// 参数调节建议：
// - 增大 Q 值：增加对测量的信任，提高响应速度
// - 减小 Q 值：增加对预测的信任，提高平滑度
// - 增大 R 值：增加平滑度，减少噪声，但会降低响应速度
// - 减小 R 值：提高响应速度，但会增加噪声
// 角度/速度 α-β 观测器参数
constexpr float ANGLE_KALMAN_INITIAL_STATE = 0.0f;
constexpr float ANGLE_KALMAN_INITIAL_COVARIANCE = 0.0f;
constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q =
1e-3f; // 过程噪声协方差 - 调节此参数控制平滑度
constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R =
0.0062f; // 测量噪声协方差 - 调节此参数控制响应速度
constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q = 0.0f;
constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R = 0.0f;
constexpr float ANGLE_OBSERVER_ALPHA = 0.75f; // 角度校正增益，越大越跟测量
constexpr float ANGLE_OBSERVER_BETA = 0.002f; // 速度校正增益，原0.001微调至0.002
// 速度卡尔曼滤波器参数
// 旧速度 Kalman 参数保留用于接口兼容；主速度输出现在直接来自 α-β 观测器
constexpr float VELOCITY_KALMAN_INITIAL_STATE = 0.0f;
constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-3f;
constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
1e-6f; // 过程噪声协方差 - 通常保持较小
5e-4f; // 略增大，减少拖尾
constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
1.0f; // 测量噪声协方差 - 主要调节参数 1.0
0.05f; // 明显小于旧值，避免压得太死
// 电流卡尔曼滤波器参数
constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
@@ -223,7 +219,7 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
// TIMER2-任务调度
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us
constexpr int MOTOR_PERIOD = 2; // 100us/50us = 2
constexpr int MOTOR_PERIOD = 1; // 控制每50us执行一次，无分频
constexpr float TIME_SLICE =
(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率
@@ -231,10 +227,10 @@ constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
// TIMER1-PWM
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为 120MHz
constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
constexpr int TIMER1_PERIOD = 2999; // 120MHz / (0+1) / (2999+1) = 40kHz计数, 中心对齐实际PWM 20kHz
constexpr int TIMER1_DEFAULT_PULSE = 0;
constexpr int TIMER1_CH3_COMPARE_VALUE =
(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM 周期 50% 位置触发中断
(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM 周期 41% 位置触发ADC采样
// 电压检测参数（基于跌幅检测，不依赖固定额定电压）
// 快速检测层（fast_voltage_check，100us周期）：
Robot_V2/sys/systick.cpp
+82
View File
@@ -50,9 +50,43 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
volatile static uint32_t delay;
volatile static uint32_t systick_ms = 0; // 系统运行时间（毫秒）
// ============================================================================
// DWT-based CPU usage measurement
// ============================================================================
// GD32C103 = Cortex-M4 @ 120MHz, DWT_CYCCNT 可用
//
// 原理：
// do_task() 入口记录 CYCCNT → task_enter_cyccnt
// do_task() 出口记录 CYCCNT → 差值 = 本次忙碌周期
// 累加到 busy_cycles_accum
// SysTick 每 1s 统计：cpu% = busy_cycles_accum * 100 / SystemCoreClock
//
// 不需要 idle_task_count()，DWT 直接数周期
// ============================================================================
volatile static uint32_t task_enter_cyccnt = 0; // do_task 入口 CYCCNT
volatile static uint32_t busy_cycles_accum = 0; // 1s 内累计忙碌周期
volatile static uint16_t sample_seconds = 0; // 统计窗口（秒）
volatile static uint8_t cpu_usage = 0; // CPU 使用率（0~100%）
volatile static uint8_t dwt_initialized = 0; // DWT 初始化标志
extern CAN_transmiter CAN0_transmiter;
extern FlashEEPROM flashEEP;
/*!
\brief 初始化 DWT 周期计数器
\note 使能 DWT 的 CYCCNT，用于精确测量 CPU 周期
*/
static void dwt_init(void) {
/* 使能 DWT 和 ITM（TRCENA 位在 DEMCR 中） */
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
/* 复位 CYCCNT */
DWT->CYCCNT = 0;
/* 使能 CYCCNT */
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
dwt_initialized = 1;
}
/*!
\brief get system tick time in milliseconds
\param[in] none
@@ -70,6 +104,9 @@ uint32_t get_systick_ms(void) {
\retval none
*/
void systick_config(void) {
/* 初始化 DWT 周期计数器 */
dwt_init();
/* setup systick timer for 1000Hz interrupts */
if (SysTick_Config(SystemCoreClock / 1000U)) {
/* capture error */
@@ -89,6 +126,24 @@ void systick_config(void) {
extern "C" void SysTick_Handler(void) {
systick_ms++; // 递增系统时间
delay_decrement(); // 递减延时计数器
/* 每 1 秒统计一次 CPU 占用率 */
sample_seconds++;
if (sample_seconds >= 1000U) {
if (dwt_initialized) {
/* 1 秒总周期 = SystemCoreClock (120,000,000) */
/* CPU% = busy_cycles_accum * 100 / SystemCoreClock */
uint64_t busy_64 = (uint64_t)busy_cycles_accum * 100U;
uint32_t busy_percent = (uint32_t)(busy_64 / SystemCoreClock);
if (busy_percent > 100U) {
busy_percent = 100U;
}
cpu_usage = (uint8_t)busy_percent;
}
busy_cycles_accum = 0;
sample_seconds = 0;
}
}
/*!
\brief delay a time in seconds
@@ -128,6 +183,33 @@ void delay_decrement(void) {
}
}
/*!
\brief 空闲任务计数（兼容接口，DWT 版本无需操作）
*/
void idle_task_count(void) {
/* DWT 版本：空闲周期 = 总周期 - 忙碌周期，无需手动计数 */
}
/*!
\brief do_task 入口调用，记录忙碌起始周期
*/
void busy_task_enter(void) {
if (dwt_initialized) {
task_enter_cyccnt = DWT->CYCCNT;
}
}
/*!
\brief do_task 出口调用，累加忙碌周期
*/
void busy_task_exit(void) {
if (dwt_initialized) {
busy_cycles_accum += (DWT->CYCCNT - task_enter_cyccnt);
}
}
uint8_t get_cpu_usage(void) { return cpu_usage; }
/*!
\brief system soft reset
\param[in] none
Robot_V2/sys/systick.h
+6 -2
View File
@@ -18,7 +18,7 @@ this list of conditions and the following disclaimer.
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
software without without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
@@ -50,8 +50,12 @@ void delay_1ms(uint32_t count);
void delay_1s(uint32_t count);
/* delay decrement */
void delay_decrement(void);
/* 空闲任务计数 */
/* 空闲任务计数（兼容接口，DWT 版本无需操作） */
void idle_task_count(void);
/* do_task 入口调用，记录忙碌起始周期 */
void busy_task_enter(void);
/* do_task 出口调用，累加忙碌周期 */
void busy_task_exit(void);
/* 获取CPU使用率 */
uint8_t get_cpu_usage(void);
/* 系统软复位 */