feat(FOC): 优化FOC控制算法并修复多个控制环路问题
- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值 以避免重复三角函数计算，提升性能约15-300周期 - 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的 震荡问题和正转正常反转震荡的根本原因 - 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦 hunting抑制效果 - 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待， 提升实时性能 - 修复初始化期间电压分母为零的问题，添加下限保护防止P增益 无穷大 - 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击 - 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定 性能 perf(Filter): 优化中值滤波算法性能 - 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络， 用12次比较替代通用std::nth_element算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
This commit is contained in:
+154
-72
@@ -308,7 +308,7 @@ void calibrate_zero_position(void) {
|
||||
* @return float 返回值
|
||||
*/
|
||||
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
|
||||
float current_c, float angle_el) {
|
||||
float current_c) {
|
||||
if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
|
||||
current_c == ERROR_ADC_VAL) {
|
||||
return Current_kalman.getCurrent();
|
||||
@@ -320,19 +320,17 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
|
||||
current_b -= current_avg;
|
||||
current_c -= current_avg;
|
||||
|
||||
// 电气角度由调用方传入，避免重复计算
|
||||
// Clarke变换 (使用三相电流)
|
||||
float I_alpha, I_beta;
|
||||
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
|
||||
(1.0f / 3.0f) * current_c;
|
||||
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
|
||||
|
||||
// Park变换
|
||||
float sin_el, cos_el;
|
||||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
// Park变换：sin/cos 由 update_el_sincos() 每周期算一次，
|
||||
// 与 setSVPWM_PhaseVoltage 的逆 Park 共用，省一次三角函数
|
||||
float I_q, I_d;
|
||||
I_q = I_beta * cos_el - I_alpha * sin_el;
|
||||
I_d = I_beta * sin_el + I_alpha * cos_el;
|
||||
I_q = I_beta * el_cos - I_alpha * el_sin;
|
||||
I_d = I_beta * el_sin + I_alpha * el_cos;
|
||||
|
||||
// 返回Iq值，乘以1000以转换为mA
|
||||
return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
|
||||
@@ -354,6 +352,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
|
||||
float min_target = 0.0f;
|
||||
float stop_speed = 0.0f;
|
||||
float brake_err = 0.0f;
|
||||
float learn_err_max = 0.0f;
|
||||
float learn_alpha = 0.0f;
|
||||
float gain = 0.0f;
|
||||
float max_ff = 0.0f;
|
||||
@@ -366,6 +365,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
|
||||
min_target = MODE2_UQ_FF_MIN_TARGET;
|
||||
stop_speed = MODE2_UQ_FF_STOP_SPEED;
|
||||
brake_err = MODE2_UQ_FF_BRAKE_ERR;
|
||||
learn_err_max = MODE2_UQ_FF_LEARN_ERR_MAX;
|
||||
learn_alpha = MODE2_UQ_FF_LEARN_ALPHA;
|
||||
gain = MODE2_UQ_FF_GAIN;
|
||||
max_ff = MODE2_UQ_FF_MAX;
|
||||
@@ -377,6 +377,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
|
||||
min_target = MODE3_UQ_FF_MIN_TARGET;
|
||||
stop_speed = MODE3_UQ_FF_STOP_SPEED;
|
||||
brake_err = MODE3_UQ_FF_BRAKE_ERR;
|
||||
learn_err_max = MODE3_UQ_FF_LEARN_ERR_MAX;
|
||||
learn_alpha = MODE3_UQ_FF_LEARN_ALPHA;
|
||||
gain = MODE3_UQ_FF_GAIN;
|
||||
max_ff = MODE3_UQ_FF_MAX;
|
||||
@@ -386,10 +387,16 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// 超速/制动判定必须沿"运动方向"：velocity_error×dir_sign 是 PID 系误差，
|
||||
// 再乘运动方向符号后，< -brake_err 才表示"沿运动方向明显超速"。
|
||||
// 原实现只乘 dir_sign：反转(负速度)时判定极性恰好相反——反转超速时
|
||||
// 前馈继续输出、反转欠速时前馈反而被切断，已学到的电压(可达±MAX)
|
||||
// 随误差符号通断，形成正转没有、反转独有的 bang-bang 震荡。
|
||||
const float motion_sign = (target_vel * dir_sign >= 0.0f) ? 1.0f : -1.0f;
|
||||
const bool active = (fabsf(target_vel) > stop_speed) &&
|
||||
(fabsf(actual_vel) > stop_speed) &&
|
||||
((target_vel * actual_vel) > 0.0f) &&
|
||||
((velocity_error * dir_sign) > -brake_err);
|
||||
((velocity_error * dir_sign * motion_sign) > -brake_err);
|
||||
|
||||
float angle = normalizeAngle360(mech_angle);
|
||||
float index_f = angle / 360.0f * (float)table_size;
|
||||
@@ -402,7 +409,11 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
|
||||
if (ff > max_ff) ff = max_ff;
|
||||
else if (ff < -max_ff) ff = -max_ff;
|
||||
|
||||
if (learn_enable && active && fabsf(target_vel) > min_target) {
|
||||
// 学习仅在误差带内进行：启停/换向瞬态误差可达上百 °/s，直接学习会把
|
||||
// ±MAX 钳位值写进途经的角度桶，下一轮运动经过该角度时突加满幅前馈
|
||||
// 电压，本身就是一次转矩扰动。
|
||||
if (learn_enable && active && fabsf(target_vel) > min_target &&
|
||||
fabsf(velocity_error * dir_sign) < learn_err_max) {
|
||||
float ff_target = velocity_error * dir_sign * gain;
|
||||
if (ff_target > max_ff) ff_target = max_ff;
|
||||
else if (ff_target < -max_ff) ff_target = -max_ff;
|
||||
@@ -412,32 +423,44 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
|
||||
return (enable && active && fabsf(target_vel) > min_target) ? ff : 0.0f;
|
||||
}
|
||||
/**
|
||||
* @brief mode3 低速/静止附近降低速度内环 P，运行速度升高后平滑恢复。
|
||||
* @brief mode3 低速缩放因子：静止附近降到 min_scale，速度升高后线性恢复到 1。
|
||||
* @note P/I 使用各自的地板（I 地板更低）：低速段保持"比例主导"，
|
||||
* 积分充电足够慢，压制静摩擦 hunting（停下瞬间小幅来回抖动）。
|
||||
* D 不衰减，保持阻尼。
|
||||
*/
|
||||
float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
|
||||
float vel_p = vel_angloop_p;
|
||||
if (target_vel < -120.0f) {
|
||||
vel_p = vel_angloop_p * 2.0f;
|
||||
} else if (target_vel < -30.0f) {
|
||||
vel_p = vel_angloop_p * 1.5f;
|
||||
float FOC_Controller::get_mode3_velocity_scale(float target_vel,
|
||||
float actual_vel,
|
||||
float min_scale) {
|
||||
if (!MODE3_LOW_SPEED_P_SCALE_ENABLE) {
|
||||
return 1.0f;
|
||||
}
|
||||
|
||||
if (MODE3_LOW_SPEED_P_SCALE_ENABLE) {
|
||||
float abs_target = fabsf(target_vel);
|
||||
float abs_speed = fabsf(actual_vel);
|
||||
float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed;
|
||||
float p_scale = 1.0f;
|
||||
if (speed_ref <= MODE3_LOW_SPEED_P_LOW) {
|
||||
p_scale = MODE3_LOW_SPEED_P_MIN_SCALE;
|
||||
} else if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
|
||||
p_scale = MODE3_LOW_SPEED_P_MIN_SCALE +
|
||||
(1.0f - MODE3_LOW_SPEED_P_MIN_SCALE) *
|
||||
(speed_ref - MODE3_LOW_SPEED_P_LOW) /
|
||||
return min_scale;
|
||||
}
|
||||
if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
|
||||
return min_scale +
|
||||
(1.0f - min_scale) * (speed_ref - MODE3_LOW_SPEED_P_LOW) /
|
||||
(MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW);
|
||||
}
|
||||
vel_p *= p_scale;
|
||||
return 1.0f;
|
||||
}
|
||||
return vel_p;
|
||||
|
||||
/**
|
||||
* @brief mode3 速度内环 P：低速缩放因子（方向无关）。
|
||||
* @note 历史上曾有"反转 P 提升到 ×2"的增益调度（先是 −120/−30 硬台阶，
|
||||
* 后改为连续线性过渡）：连续化后反转仍剧烈震荡，说明问题不在
|
||||
* 台阶突跳，而在增益本身——×2 只对反转生效，把反转环路增益整体
|
||||
* 翻倍而正转不变，正是"正转正常、反转震荡"的直接来源之一。
|
||||
* 已移除。反转阻力差异由 REV 齿槽补偿表与 Uq 前馈学习承担；
|
||||
* 若实测反转欠速/偏慢，优先增大前馈学习限幅，不要恢复非对称 P。
|
||||
*/
|
||||
float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
|
||||
return vel_angloop_p * get_mode3_velocity_scale(target_vel, actual_vel,
|
||||
MODE3_LOW_SPEED_P_MIN_SCALE);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -489,14 +512,23 @@ u8 FOC_Controller::init() {
|
||||
memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV));
|
||||
cogging_comp_enabled = true;
|
||||
|
||||
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
|
||||
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
|
||||
FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
|
||||
// limit_voltage 从 Flash/CAN 恢复，可能为 0：分母下限保护，防止 P=inf
|
||||
const float denom = (limit_voltage > MIN_LIMIT_VOLTAGE)
|
||||
? limit_voltage
|
||||
: voltage_power_supply;
|
||||
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / denom;
|
||||
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / denom;
|
||||
FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / denom;
|
||||
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
|
||||
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
|
||||
FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
|
||||
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
|
||||
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
|
||||
FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / denom;
|
||||
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / denom;
|
||||
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / denom;
|
||||
|
||||
// 运行期不每周期 set_P 的环路（mode1/mode5）在此同步一次有效增益，
|
||||
// 避免沿用构造期的临时值
|
||||
angle_loop.set_P(ang_loop_p);
|
||||
current_loop.set_P(cur_loop_p);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -506,6 +538,15 @@ u8 FOC_Controller::init() {
|
||||
*/
|
||||
FOC_Controller::FOC_Controller() { ; }
|
||||
|
||||
/**
|
||||
* @brief 每控制周期调用一次：计算当前电角度的 sin/cos。
|
||||
* @note cal_Iq_Id 的 Park 变换与 setSVPWM_PhaseVoltage 的逆 Park 变换
|
||||
* 使用同一电角度，三角函数每周期只算这一次，两处共用。
|
||||
*/
|
||||
inline void FOC_Controller::update_el_sincos(float angle_el) {
|
||||
arm_sin_cos_f32(angle_el, &el_sin, &el_cos);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用SVPWM算法设置电机输出
|
||||
*
|
||||
@@ -514,13 +555,12 @@ FOC_Controller::FOC_Controller() { ; }
|
||||
*
|
||||
* 优化：atan2f → X/Y/Z 符号法判扇区（~15周期 vs atan2f ~300周期）
|
||||
* 边界上扇区编号可能与 atan2f 差1，但 duty 输出完全一致（已数值验证）
|
||||
* 逆 Park 所需 sin/cos 由 update_el_sincos() 预计算，不再重复计算
|
||||
*/
|
||||
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud, float angle_el) {
|
||||
// ① 逆Park变换（angle_el 由调用方传入）
|
||||
float sin_el, cos_el;
|
||||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
float Ualpha = Ud * cos_el - Uq * sin_el;
|
||||
float Ubeta = Ud * sin_el + Uq * cos_el;
|
||||
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
|
||||
// ① 逆Park变换（sin/cos 为本周期预计算值）
|
||||
float Ualpha = Ud * el_cos - Uq * el_sin;
|
||||
float Ubeta = Ud * el_sin + Uq * el_cos;
|
||||
|
||||
// ② 扇区判定（替代 atan2f）
|
||||
float X = Ubeta;
|
||||
@@ -727,11 +767,11 @@ void FOC_Controller::update_power(float voltage) {
|
||||
* @return void
|
||||
*/
|
||||
void FOC_Controller::fast_voltage_check(void) {
|
||||
// ADC1 单次转换读取 VBUS（ch6，11:1 分压）。
|
||||
// 旧实现读 regular DMA 的 adc_value[2]，通道顺序调整后该下标已是 DRV温度，
|
||||
// 属漏改：NTC 分压值(~1.06V)按 11 倍分压换算仅 ~11.5V，逼近跌落阈值。
|
||||
// ADC1 连续模式零等待读最新 VBUS（ch6，11:1 分压）。
|
||||
// 旧实现 adc1_read_raw 阻塞式"触发+等EOC"：10kHz 调用每次 ~1.5μs 死等。
|
||||
// 通道顺序问题的历史注释已随 ADC1 连续模式改造失效（本函数只读 DR）。
|
||||
float current_vbus =
|
||||
adc1_read_raw(ADC_CHANNEL_6) * (3.3f / 4096.0f) * 11.0f;
|
||||
adc1_read_vbus_latest() * (3.3f / 4096.0f) * 11.0f;
|
||||
|
||||
// 更新基准电压（峰值检测）
|
||||
// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
|
||||
@@ -782,8 +822,9 @@ float FOC_Controller::velocity_open_loop() {
|
||||
normalizeAngle360(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY);
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
update_el_sincos(angle_el_cog);
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'), angle_el_cog);
|
||||
sampler.get_current('W'));
|
||||
|
||||
// 因为在开环控制中，Ud 始终为 0，只控制 Uq
|
||||
// 幅度限幅必须保留符号：原 _constrain 下限为 +2V，负速度会被钳到 +2V，
|
||||
@@ -801,7 +842,7 @@ float FOC_Controller::velocity_open_loop() {
|
||||
}
|
||||
Uq = (Uq < 0.0f) ? -Uq_mag : Uq_mag;
|
||||
}
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
|
||||
return limit_voltage;
|
||||
}
|
||||
|
||||
@@ -812,17 +853,19 @@ float FOC_Controller::velocity_open_loop() {
|
||||
* @return float: 当前角度
|
||||
*/
|
||||
float FOC_Controller::angle_control() {
|
||||
// PID 控制计算
|
||||
float Uq = angle_loop(target_angle - rt_angle);
|
||||
// PID 控制计算（D 项对测量微分+低通，目标阶跃无微分踢）
|
||||
angle_loop.set_P(ang_loop_p);
|
||||
float Uq = angle_loop(target_angle - rt_angle, rt_angle);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
update_el_sincos(angle_el_cog);
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'), angle_el_cog);
|
||||
sampler.get_current('W'));
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
|
||||
return shaft_angle;
|
||||
}
|
||||
|
||||
@@ -842,24 +885,45 @@ float FOC_Controller::velocity_control() {
|
||||
// mode2 使用独立速度环参数。
|
||||
vel_loop.set_PID(vel_loop_p, vel_loop_Ki, vel_loop_Kd);
|
||||
vel_loop.set_limit(limit_voltage);
|
||||
float Uq_pid = vel_loop(velocity_error);
|
||||
|
||||
// 仅"实质性换向"时清积分：目标速度变号且新幅值够大（真实反向指令）才清。
|
||||
// 最终贴合阶段误差过零引起的微小变号（|target|=150×0.01°≈1.5°/s）不清——
|
||||
// 否则积分被反复清空，低速段缩小的 P 项（0.12V@0.01°）推不过静摩擦，
|
||||
// 永远停在差 ~0.01° 的位置
|
||||
static float last_mode2_target_vel = 0.0f;
|
||||
if (last_mode2_target_vel * target_velocity < 0.0f &&
|
||||
fabsf(target_velocity) > 5.0f) {
|
||||
vel_loop.reset_integral();
|
||||
}
|
||||
last_mode2_target_vel = target_velocity;
|
||||
|
||||
float Uq_pid =
|
||||
vel_loop(velocity_error, rt_velocity * DIR * REDUCTION_RATIO);
|
||||
|
||||
static float uq_ff_table[MODE2_UQ_FF_TABLE_SIZE] = {0.0f};
|
||||
float Uq_ff = update_angle_sync_uq_feedforward(
|
||||
2, uq_ff_table, MODE2_UQ_FF_TABLE_SIZE, shaft_angle,
|
||||
target_velocity, rt_velocity, velocity_error);
|
||||
|
||||
// PID 输出已内部钳位；前馈叠加后超限时泄放积分，
|
||||
// 让 PID 感知真实饱和（否则重载下积分继续累积→释载超调振荡）
|
||||
float Uq = Uq_pid + Uq_ff;
|
||||
if (Uq > limit_voltage) Uq = limit_voltage;
|
||||
else if (Uq < -limit_voltage) Uq = -limit_voltage;
|
||||
if (Uq > limit_voltage) {
|
||||
vel_loop.bleed_integral(Uq - limit_voltage);
|
||||
Uq = limit_voltage;
|
||||
} else if (Uq < -limit_voltage) {
|
||||
vel_loop.bleed_integral(Uq + limit_voltage);
|
||||
Uq = -limit_voltage;
|
||||
}
|
||||
CAN0_transmiter.testfloat_union.f = Uq;
|
||||
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
update_el_sincos(angle_el_cog);
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'), angle_el_cog);
|
||||
sampler.get_current('W'));
|
||||
|
||||
return rt_velocity;
|
||||
}
|
||||
@@ -874,18 +938,20 @@ float FOC_Controller::current_control() {
|
||||
CAN0_transmiter.testfloat_union.f = shaft_angle;
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
update_el_sincos(angle_el_cog);
|
||||
// PID 控制计算
|
||||
float Iq = rt_current =
|
||||
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'), angle_el_cog);
|
||||
float Uq = current_loop(target_current - Iq);
|
||||
sampler.get_current('W'));
|
||||
current_loop.set_P(cur_loop_p);
|
||||
float Uq = current_loop(target_current - Iq, Iq);
|
||||
if (Uq > limit_voltage) {
|
||||
Uq = limit_voltage;
|
||||
} else if (Uq < -limit_voltage) {
|
||||
Uq = -limit_voltage;
|
||||
}
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -901,21 +967,28 @@ float FOC_Controller::angle_control_with_velocity_limit() {
|
||||
float angle_error = target_angle - rt_angle;
|
||||
|
||||
ang_inVelAngloop.set_P(ang_angloop_p);
|
||||
// 使用角度PID控制器计算目标速度
|
||||
float target_velocity = ang_inVelAngloop(angle_error);
|
||||
// 使用角度PID控制器计算目标速度（D 项对测量微分+低通）
|
||||
float target_velocity = ang_inVelAngloop(angle_error, rt_angle);
|
||||
|
||||
// 使用速度PID控制器计算最终输出电压
|
||||
vel_inVelAngloop.set_P(get_mode3_velocity_p(target_velocity, rt_velocity));
|
||||
// 低速段 P/I 各自按地板衰减（D 保持满值）：I 地板更低（0.15），
|
||||
// 停止瞬间积分充电速率仅为全速的 15%，进一步压制静摩擦 hunting
|
||||
float i_scale = get_mode3_velocity_scale(target_velocity, rt_velocity,
|
||||
MODE3_LOW_SPEED_I_MIN_SCALE);
|
||||
vel_inVelAngloop.set_PID(get_mode3_velocity_p(target_velocity, rt_velocity),
|
||||
vel_angloop_Ki * i_scale, vel_angloop_Kd);
|
||||
float velocity_error = target_velocity - rt_velocity;
|
||||
|
||||
// 目标速度突变检测：target 大幅变化时清积分
|
||||
// 仅"实质性换向"时清积分（变号且新幅值>5°/s）。制动段不清（防欠程）；
|
||||
// 最终贴合阶段误差过零的微小变号也不清（防积分反复被清、停在差0.01°）
|
||||
static float last_vang_target_vel = 0.0f;
|
||||
if (fabsf(target_velocity - last_vang_target_vel) > 5.0f) {
|
||||
if (last_vang_target_vel * target_velocity < 0.0f &&
|
||||
fabsf(target_velocity) > 5.0f) {
|
||||
vel_inVelAngloop.reset_integral();
|
||||
}
|
||||
last_vang_target_vel = target_velocity;
|
||||
|
||||
float Uq_pid = vel_inVelAngloop(velocity_error) * DIR;
|
||||
float Uq_pid = vel_inVelAngloop(velocity_error, rt_velocity) * DIR;
|
||||
|
||||
// 获取电机轴角度：mode3 Uq 前馈补偿按物理 360° 查表。
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
@@ -924,18 +997,25 @@ float FOC_Controller::angle_control_with_velocity_limit() {
|
||||
3, mode3_uq_ff_table, MODE3_UQ_FF_TABLE_SIZE, shaft_angle,
|
||||
target_velocity, rt_velocity, velocity_error);
|
||||
|
||||
// 前馈叠加后超限时泄放积分（×DIR 补偿：Uq = PID输出×DIR + 前馈）
|
||||
float Uq = Uq_pid + Uq_ff;
|
||||
if (Uq > limit_voltage) Uq = limit_voltage;
|
||||
else if (Uq < -limit_voltage) Uq = -limit_voltage;
|
||||
if (Uq > limit_voltage) {
|
||||
vel_inVelAngloop.bleed_integral(DIR * (Uq - limit_voltage));
|
||||
Uq = limit_voltage;
|
||||
} else if (Uq < -limit_voltage) {
|
||||
vel_inVelAngloop.bleed_integral(DIR * (Uq + limit_voltage));
|
||||
Uq = -limit_voltage;
|
||||
}
|
||||
CAN0_transmiter.testfloat_union.f = shaft_angle;
|
||||
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
update_el_sincos(angle_el_cog);
|
||||
// 获取当前实际电流值
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'), angle_el_cog);
|
||||
sampler.get_current('W'));
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
|
||||
|
||||
return shaft_angle;
|
||||
}
|
||||
@@ -952,36 +1032,38 @@ float FOC_Controller::MIT_Mode() {
|
||||
float angle_error = target_angle - rt_angle;
|
||||
ang_inMITloop.set_P(ang_MIT_p);
|
||||
float target_velocity =
|
||||
ang_inMITloop(angle_error);
|
||||
ang_inMITloop(angle_error, rt_angle);
|
||||
|
||||
// 第二层：速度环（中环）
|
||||
float velocity_error = (target_velocity - rt_velocity)* DIR;
|
||||
|
||||
// 目标速度变化检测：target 大幅变化时清积分
|
||||
// 仅"实质性换向"时清积分（变号且新幅值>5°/s，理由同 mode3）
|
||||
static float last_mit_target_vel = 0.0f;
|
||||
if (fabsf(target_velocity - last_mit_target_vel) > 5.0f) {
|
||||
if (last_mit_target_vel * target_velocity < 0.0f &&
|
||||
fabsf(target_velocity) > 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);
|
||||
float target_current = vel_inMITloop(velocity_error, rt_velocity * DIR);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
update_el_sincos(angle_el_cog);
|
||||
|
||||
// 第三层：电流环（内环）
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'), angle_el_cog);
|
||||
sampler.get_current('W'));
|
||||
|
||||
float current_error = target_current - rt_current;
|
||||
current_inMITloop.set_P(cur_MIT_p);
|
||||
float Uq = current_inMITloop(current_error);
|
||||
float Uq = current_inMITloop(current_error, rt_current);
|
||||
|
||||
// 设置相电压
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
|
||||
|
||||
return shaft_angle;
|
||||
}
|
||||
|
||||
+28
-17
@@ -48,7 +48,6 @@ class FOC_Controller {
|
||||
float voltage_power_supply = 24.0f; // 电源电压
|
||||
float v_safe = voltage_power_supply / 1.05f;
|
||||
float duty_a = 0, duty_b = 0, duty_c = 0;
|
||||
float Ua = 0, Ub = 0, Uc = 0;
|
||||
|
||||
// 快速电压检测相关成员（基于跌幅检测）
|
||||
float vbus_reference = 0.0f; // 基准电压（峰值检测，跟踪当前额定电压）
|
||||
@@ -250,15 +249,19 @@ class FOC_Controller {
|
||||
#endif // FOC_DEBUG
|
||||
|
||||
// 此处默认值不作数
|
||||
// 注意：构造期 limit_voltage 尚为 0，分母一律用 voltage_power_supply(24V)，
|
||||
// 否则 P 初始化为 +inf；运行期各模式每周期 set_P/set_PID 覆盖。
|
||||
// 内环 output_ramp 保持原值：实测收紧到 2000 V/s 会在制动段引入相位滞后，
|
||||
// 导致到位抖动，已回退。
|
||||
PIDController vel_loop =
|
||||
PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
|
||||
vel_loop_Kd, 10000, voltage_power_supply}; // 12V
|
||||
PIDController{DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
|
||||
vel_loop_Kd, 10000, voltage_power_supply};
|
||||
PIDController angle_loop =
|
||||
PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
|
||||
10000000, voltage_power_supply}; // 12V
|
||||
PIDController{DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
|
||||
0.02f, 10000000, voltage_power_supply};
|
||||
PIDController current_loop = PIDController{
|
||||
DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
|
||||
current_loop_Kd, 1500, voltage_power_supply}; // 12V
|
||||
DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
|
||||
current_loop_Kd, 1500, voltage_power_supply};
|
||||
PIDController vel_inVelAngloop =
|
||||
PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
|
||||
vel_angloop_Kd, 1e6, v_safe};
|
||||
@@ -267,13 +270,13 @@ class FOC_Controller {
|
||||
ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
|
||||
|
||||
PIDController ang_inMITloop =
|
||||
PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
|
||||
PIDController{DEFAULT_angle_MIT_p * 100 / voltage_power_supply, angle_MIT_Ki,
|
||||
angle_MIT_Kd, 1e5, 100};
|
||||
PIDController vel_inMITloop = PIDController{
|
||||
DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
|
||||
PIDController current_inMITloop = PIDController{
|
||||
DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
|
||||
current_loop_Kd, 1500, voltage_power_supply}; // 12V
|
||||
DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
|
||||
current_loop_Kd, 1500, voltage_power_supply};
|
||||
|
||||
/**
|
||||
* @brief 更新 pid_limits 相关状态
|
||||
@@ -289,32 +292,40 @@ class FOC_Controller {
|
||||
protected:
|
||||
float shaft_angle = 0;
|
||||
float Ualpha, Ubeta = 0;
|
||||
float zero_electric_angle = 0;
|
||||
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
||||
float startup_ramp_val = 0.0f; // 软启动系数
|
||||
float startup_ramp_step = 0.01f; // 启动斜坡步长，可调节以控制启动速度
|
||||
// 本周期电角度的 sin/cos：update_el_sincos() 每周期算一次，
|
||||
// Park 变换与逆 Park 变换共用，避免同一角度算两次三角函数
|
||||
float el_sin = 0.0f;
|
||||
float el_cos = 0.0f;
|
||||
/**
|
||||
* @brief 每周期计算一次电角度 sin/cos 供 Park/逆Park 共用
|
||||
* @param[in] angle_el 电角度（度）
|
||||
* @return void
|
||||
*/
|
||||
inline void update_el_sincos(float angle_el);
|
||||
/**
|
||||
* @brief 设置 setSVPWM_PhaseVoltage 对应的参数或状态
|
||||
* @param[in] Uq 参数
|
||||
* @param[in] Ud 参数
|
||||
* @param[in] angle_el 参数
|
||||
* @return void
|
||||
*/
|
||||
void setSVPWM_PhaseVoltage(float Uq, float Ud, float angle_el);
|
||||
void setSVPWM_PhaseVoltage(float Uq, float Ud);
|
||||
/**
|
||||
* @brief cal_Iq_Id 函数
|
||||
* @param[in] current_a 参数
|
||||
* @param[in] current_b 参数
|
||||
* @param[in] current_c 参数
|
||||
* @param[in] angle_el 参数
|
||||
* @return float 返回值
|
||||
* @note 电气角度三角函数用 update_el_sincos() 的预计算结果
|
||||
*/
|
||||
float cal_Iq_Id(float current_a, float current_b, float current_c, float angle_el);
|
||||
float cal_Iq_Id(float current_a, float current_b, float current_c);
|
||||
float update_angle_sync_uq_feedforward(u8 mode, float table[],
|
||||
u16 table_size, float mech_angle,
|
||||
float target_vel, float actual_vel,
|
||||
float velocity_error);
|
||||
float get_mode3_velocity_p(float target_vel, float actual_vel);
|
||||
float get_mode3_velocity_scale(float target_vel, float actual_vel,
|
||||
float min_scale);
|
||||
};
|
||||
|
||||
// 定义函数指针类型
|
||||
|
||||
@@ -58,17 +58,38 @@ CurrentKalman Current_kalman( CURRENT_KALMAN_INITIAL_STATE, CURRENT_KALMAN_INITI
|
||||
\param[in] values 待滤波的数据数组
|
||||
\param[in] size 数组大小
|
||||
\retval 滤波后的中值
|
||||
\note 使用部分排序优化算法提高效率
|
||||
\note size==6（ADC_FILTER_SIZE，唯一实际调用）走固定 12 次比较的
|
||||
排序网络：std::nth_element 是通用算法，对 6 个元素的开销
|
||||
数倍于固定网络，而本函数在 1kHz 慢速任务里被 3 处调用。
|
||||
语义与旧版一致：返回上中值（完全排序后的 [size/2]）。
|
||||
*/
|
||||
static inline void median_cswap(float &a, float &b) {
|
||||
if (a > b) {
|
||||
float t = a;
|
||||
a = b;
|
||||
b = t;
|
||||
}
|
||||
}
|
||||
|
||||
float medium_filter(float values[], u8 size) {
|
||||
// 使用局部数组避免动态内存分配
|
||||
float sorted[size];
|
||||
if (size == 6) {
|
||||
float s0 = values[0], s1 = values[1], s2 = values[2];
|
||||
float s3 = values[3], s4 = values[4], s5 = values[5];
|
||||
// n=6 最优排序网络（12 个比较器，Knuth AoCP Vol.3）
|
||||
median_cswap(s1, s2); median_cswap(s4, s5);
|
||||
median_cswap(s0, s2); median_cswap(s3, s5);
|
||||
median_cswap(s0, s1); median_cswap(s3, s4); median_cswap(s2, s5);
|
||||
median_cswap(s0, s3); median_cswap(s1, s4); median_cswap(s2, s4);
|
||||
median_cswap(s1, s3); median_cswap(s2, s3);
|
||||
return s3;
|
||||
}
|
||||
|
||||
// 其他尺寸的通用回退路径（当前无调用者）
|
||||
float sorted[16];
|
||||
if (size > 16U) size = 16U;
|
||||
memcpy(sorted, values, size * SIZE_FLOAT);
|
||||
|
||||
// 这比完整排序更高效，时间复杂度从O(n log n)降低到O(n)
|
||||
std::nth_element(sorted, sorted + size / 2, sorted + size);
|
||||
|
||||
return sorted[size / 2]; // 返回中值
|
||||
return sorted[size / 2];
|
||||
}
|
||||
|
||||
// EncoderVelocityKalman类的实现
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
#ifndef _FILTER_H
|
||||
#define _FILTER_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
+56
-12
@@ -12,33 +12,55 @@
|
||||
#include <cmath>
|
||||
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
// 微分项一阶低通离散系数：α = Ts / (τ + Ts)
|
||||
constexpr float DERIV_LPF_ALPHA = TIME_SLICE / (DERIV_LPF_TAU + TIME_SLICE);
|
||||
|
||||
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
|
||||
: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
|
||||
,
|
||||
limit(limit) // PID控制器输出限幅
|
||||
,
|
||||
error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
|
||||
error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f),
|
||||
measurement_prev(0.0f), derivative_filt(0.0f), deriv_init(false) {}
|
||||
|
||||
// PID 控制器函数
|
||||
float PIDController::operator()(float error) {
|
||||
// 测量微分版本：D 项对测量量而非误差微分，目标阶跃不再产生微分踢
|
||||
float PIDController::operator()(float error, float measurement) {
|
||||
// 首次调用只建立测量基线，避免 (meas-0)/Ts 的假微分尖峰
|
||||
if (!deriv_init) {
|
||||
measurement_prev = measurement;
|
||||
deriv_init = true;
|
||||
}
|
||||
float d_raw = -(measurement - measurement_prev) / TIME_SLICE;
|
||||
derivative_filt += DERIV_LPF_ALPHA * (d_raw - derivative_filt);
|
||||
measurement_prev = measurement;
|
||||
return step(error, D * derivative_filt);
|
||||
}
|
||||
|
||||
// 公共计算路径：Tustin 积分 + 条件积分抗饱和 + 输出斜坡
|
||||
float PIDController::step(float error, float derivative) {
|
||||
// P环
|
||||
float proportional = P * error;
|
||||
|
||||
// Tustin 散点积分（I环）
|
||||
// 添加积分限幅，防止积分饱和
|
||||
float integral =
|
||||
integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
|
||||
|
||||
// 将P,I,D三环的计算值加起来，与限幅比较得到未饱和/饱和输出
|
||||
float output_unsat = proportional + integral + derivative;
|
||||
float output = _constrain(output_unsat, -limit, limit);
|
||||
|
||||
// 抗积分饱和（条件积分）：输出饱和且误差仍在推高饱和方向时冻结积分。
|
||||
// 不用大增益反算回退——深饱和时（P 项单独超限幅）反算会把积分压成反号，
|
||||
// 误差一缩小输出瞬间反向，实测表现为到位疯狂抖动/停不到位。
|
||||
if (output != output_unsat) {
|
||||
if ((output_unsat > limit && error > 0.0f) ||
|
||||
(output_unsat < -limit && error < 0.0f)) {
|
||||
integral = integral_prev; // 冻结：不再向饱和方向累积
|
||||
}
|
||||
}
|
||||
integral = _constrain(integral, -limit, limit);
|
||||
|
||||
// D环（微分环节）
|
||||
float derivative = D * (error - error_prev) / TIME_SLICE;
|
||||
|
||||
// 将P,I,D三环的计算值加起来
|
||||
float output = proportional + integral + derivative;
|
||||
output = _constrain(output, -limit, limit);
|
||||
|
||||
// 输出斜坡限制
|
||||
// 简化的斜坡限制（单行逻辑）
|
||||
if (output_ramp > 0) {
|
||||
float max_delta = output_ramp * TIME_SLICE;
|
||||
output = output_prev +
|
||||
@@ -52,6 +74,25 @@ float PIDController::operator()(float error) {
|
||||
return output;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 外部饱和泄放积分：前馈叠加后超出限幅时调用
|
||||
* @param[in] excess 超出限幅的部分（带符号）
|
||||
* @return void
|
||||
* @note 仅当积分与超限同号（积分在加剧超限）时泄放，且不泄过零点：
|
||||
* 防止前馈导致的饱和把积分抽干成反号，到位段失去保持力。
|
||||
*/
|
||||
void PIDController::bleed_integral(float excess) {
|
||||
if (excess > 0.0f && integral_prev > 0.0f) {
|
||||
float bleed = ANTI_WINDUP_GAIN * excess;
|
||||
integral_prev = (integral_prev > bleed) ? (integral_prev - bleed)
|
||||
: 0.0f;
|
||||
} else if (excess < 0.0f && integral_prev < 0.0f) {
|
||||
float bleed = ANTI_WINDUP_GAIN * (-excess);
|
||||
integral_prev = (integral_prev < -bleed) ? (integral_prev + bleed)
|
||||
: 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置PID参数
|
||||
*
|
||||
@@ -93,6 +134,9 @@ void PIDController::reset() {
|
||||
error_prev = 0.0f;
|
||||
output_prev = 0.0f;
|
||||
integral_prev = 0.0f;
|
||||
measurement_prev = 0.0f;
|
||||
derivative_filt = 0.0f;
|
||||
deriv_init = false;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -15,9 +15,15 @@ class PIDController {
|
||||
PIDController(float P, float I, float D, float ramp, float limit);
|
||||
~PIDController() = default;
|
||||
|
||||
float operator()(float error);
|
||||
/**
|
||||
* @brief 设置 set_PID 对应的参数或状态
|
||||
* @brief PID 计算（唯一入口）
|
||||
* @param[in] error 控制误差
|
||||
* @param[in] measurement 反馈测量量（D 项对它微分，目标阶跃不产生微分踢）
|
||||
* @return float PID 输出
|
||||
*/
|
||||
float operator()(float error, float measurement);
|
||||
/**
|
||||
* @brief 设置 set_PID 对应的参数
|
||||
* @param[in] P 参数
|
||||
* @param[in] I 参数
|
||||
* @param[in] D 参数
|
||||
@@ -25,13 +31,13 @@ class PIDController {
|
||||
*/
|
||||
void set_PID(float P, float I, float D);
|
||||
/**
|
||||
* @brief 设置 set_P 对应的参数或状态
|
||||
* @brief 设置 set_P 对应的参数
|
||||
* @param[in] pp 参数
|
||||
* @return void
|
||||
*/
|
||||
void set_P(float pp);
|
||||
/**
|
||||
* @brief 设置 set_limit 对应的参数或状态
|
||||
* @brief 设置 set_limit 对应的参数
|
||||
* @param[in] limit 参数
|
||||
* @return void
|
||||
*/
|
||||
@@ -46,6 +52,13 @@ class PIDController {
|
||||
* @return void
|
||||
*/
|
||||
void reset_integral(); // 仅清除积分项，保留输出斜坡状态
|
||||
/**
|
||||
* @brief 外部饱和泄放积分（反算抗饱和的补充入口）
|
||||
* @param[in] excess 输出超出限幅的部分（带符号），如前馈叠加后超限
|
||||
* @return void
|
||||
* @note 调用方向须与 PID 输出到最终 Uq 的符号一致（有 ×DIR 的要乘上 DIR）
|
||||
*/
|
||||
void bleed_integral(float excess);
|
||||
|
||||
float P; //!< 比例增益(P环增益)
|
||||
float I; //!< 积分增益（I环增益）
|
||||
@@ -54,11 +67,20 @@ class PIDController {
|
||||
float limit;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief 公共计算路径：积分、抗饱和、输出斜坡
|
||||
* @param[in] error 控制误差
|
||||
* @param[in] derivative 已完成低通滤波的微分项（D 已乘入）
|
||||
* @return float PID 输出
|
||||
*/
|
||||
float step(float error, float derivative);
|
||||
|
||||
float error_prev; //!< 最后的跟踪误差值
|
||||
float output_prev; //!< 最后一个 pid 输出值
|
||||
float integral_prev; //!< 最后一个积分分量值
|
||||
unsigned long timestamp_prev; //!< 上次执行时间戳
|
||||
float integral_separation_threshold; //!< 积分分离阈值，用于抗积分饱和
|
||||
float measurement_prev; //!< 上次测量值（对测量微分用）
|
||||
float derivative_filt; //!< 微分项一阶低通滤波后的变化率
|
||||
bool deriv_init; //!< 测量微分是否已完成首次采样
|
||||
};
|
||||
|
||||
#endif
|
||||
+93
-16
@@ -23,9 +23,19 @@ constexpr float ADC_SCALE = 3.3f / 4096.0f;
|
||||
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
|
||||
/** @brief VCC_FACTOR 常量定义 */
|
||||
constexpr float VCC_FACTOR = 11.0f;
|
||||
/** @brief CURRENT_FACTOR 常量定义 */
|
||||
#if GATE_DRIVER_DRV8323
|
||||
/** @brief CURRENT_FACTOR 常量定义（新板：DRV8323 片上 CSA）
|
||||
* SOx = VREF/2 ± I×R×G（SPI 已配置 VREF_DIV=1、GAIN 见 config.h）。
|
||||
* ADC_SCALE 输出为伏特，故系数单位为 mA/V = 1000/(G×R)：
|
||||
* G=10V/V、R=10mΩ → 10000 mA/V，量程约 ±16.5A。
|
||||
* 修改 config.h 的 DRV8323_CSA_GAIN_VV 后此系数自动跟随。 */
|
||||
constexpr float CURRENT_FACTOR =
|
||||
1000.0f / (DRV8323_CSA_GAIN_VV * DRV8323_SHUNT_OHM);
|
||||
#else
|
||||
/** @brief CURRENT_FACTOR 常量定义（老板：外置放大链，保持原标定） */
|
||||
constexpr float CURRENT_FACTOR =
|
||||
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
|
||||
#endif
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
extern FOC_Controller FOC;
|
||||
|
||||
@@ -51,13 +61,20 @@ static inline float current_adc_to_ma(u16 raw_adc) {
|
||||
return (raw_adc * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
|
||||
}
|
||||
|
||||
// ADC1 连续转换运行标志：adc1_config() 启动后置位，
|
||||
// adc1_read_raw() 据此判断是否需要暂停/恢复连续模式。
|
||||
static bool adc1_continuous_active = false;
|
||||
|
||||
/*!
|
||||
\brief configure the ADC1 peripheral（VBUS / DRV温度 专用）
|
||||
\brief configure the ADC1 peripheral（VBUS 连续转换 / DRV温度 单次轮询）
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval none
|
||||
\note ADC1 独占慢速量：单通道单次软件触发，无 DMA、无中断、无连续模式，
|
||||
软件按需轮询读取，与 ADC0 的 PWM 同步电流采样互不干扰。
|
||||
\note ADC1 独占慢速量：ch6(VBUS) 连续转换，读取零等待；ch9(DRV NTC)
|
||||
由 adc1_read_raw() 按需暂停连续模式单次轮询。无 DMA、无中断，
|
||||
与 ADC0 的 PWM 同步注入组互不干扰（独立外设）。
|
||||
原实现每次读取都要"软件触发+死等EOC"：fast_voltage_check
|
||||
10kHz 调用，每次阻塞 ~1.5μs，纯浪费在等待上。
|
||||
*/
|
||||
void adc1_config(void) {
|
||||
/* enable ADC1 clock（ADC 时钟分频与 ADC0 共用 RCU 配置） */
|
||||
@@ -67,36 +84,96 @@ void adc1_config(void) {
|
||||
adc_mode_config(ADC_MODE_FREE);
|
||||
adc_data_alignment_config(ADC1, ADC_DATAALIGN_RIGHT);
|
||||
|
||||
/* 单通道单次转换：调用方每次配置 rank0 通道后软件触发 */
|
||||
/* VBUS 单通道连续转换：软件触发一次后自持续 */
|
||||
adc_channel_length_config(ADC1, ADC_REGULAR_CHANNEL, 1);
|
||||
adc_regular_channel_config(ADC1, 0, ADC_CHANNEL_6, ADC_SAMPLETIME_28POINT5);
|
||||
adc_external_trigger_source_config(ADC1, ADC_REGULAR_CHANNEL,
|
||||
ADC0_1_EXTTRIG_REGULAR_NONE);
|
||||
adc_external_trigger_config(ADC1, ADC_REGULAR_CHANNEL, ENABLE);
|
||||
|
||||
adc_enable(ADC1);
|
||||
/* ADC校准 */
|
||||
/* ADC校准（校准完成后再进入连续模式，避免校准期间 CONT 位参与） */
|
||||
adc_calibration_enable(ADC1);
|
||||
adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, ENABLE);
|
||||
|
||||
/* 启动连续转换并等待首个结果，保证 DR 立即有效 */
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
adc_software_trigger_enable(ADC1, ADC_REGULAR_CHANNEL);
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
}
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
adc1_continuous_active = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief ADC1 单次软件触发转换并等待完成
|
||||
* @param[in] channel ADC 通道号（如 ADC_CHANNEL_6 / ADC_CHANNEL_9）
|
||||
* @return u16 转换原始值（超时返回上次 RDATA 残留值）
|
||||
* @note 仅在主循环任务上下文调用（所有任务由调度器串行派发，无并发）。
|
||||
* 单次转换约 41 周期 ≈ 1.4μs @30MHz ADC 时钟。
|
||||
/*!
|
||||
\brief 零等待读取最新 VBUS 转换结果
|
||||
\retval u16 最近一次完成转换的原始值
|
||||
\note 仅在 VBUS 连续模式运行时调用（主循环任务上下文）。
|
||||
读 DR 硬件清 EOC，下一次完成转换会重新刷新 DR；两次读取之间
|
||||
完成的转换被静默丢弃（F1 型 ADC 无溢出标志问题），
|
||||
读到的值陈旧度 ≤1 个转换周期（41 周期 @30MHz ≈ 1.4μs）。
|
||||
*/
|
||||
u16 adc1_read_vbus_latest(void) {
|
||||
return (u16)adc_regular_data_read(ADC1);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief ADC1 单次软件触发转换并等待完成
|
||||
\param[in] channel ADC 通道号（如 ADC_CHANNEL_9）
|
||||
\retval u16 转换原始值（超时返回上次 RDATA 残留值）
|
||||
\note 仅在主循环任务上下文调用（所有任务由调度器串行派发，无并发，
|
||||
与 fast_voltage_check / update_vbus_voltage 不会交错执行）。
|
||||
单次转换约 41 周期 ≈ 1.4μs @30MHz ADC 时钟。
|
||||
连续模式运行中调用时：暂停→单次转换→恢复，VBUS 流中断约 3~5μs。
|
||||
*/
|
||||
u16 adc1_read_raw(u8 channel) {
|
||||
const bool was_continuous = adc1_continuous_active;
|
||||
u16 timeout = 0;
|
||||
if (was_continuous) {
|
||||
adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, DISABLE);
|
||||
/* 先清 EOC 再等置位：连续模式下 EOC 常态置位（DR 久未读，
|
||||
* 后续转换溢出丢失），直接等会观察到陈旧标志而另有转换在飞行中。
|
||||
* 清除后观察到的置位必然来自"暂停瞬间在飞行中"的最后一次转换
|
||||
* 完成后才置位——此时 ADC 真正静止，后续配置/触发不会再被
|
||||
* 飞行中的转换干扰（SWSTART 被忽略、读到上一通道的 DR，
|
||||
* 曾表现为 DRV 温度偶发出错）。超时兜底覆盖"暂停恰好落在
|
||||
* 两次转换间隙"的无在飞行转换情形。 */
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
/* 超时只需覆盖一个转换周期（41 ADC 周期 ≈ 20 次循环），
|
||||
* 仅"暂停恰好落在转换间隙"时才会走到超时 */
|
||||
if (++timeout > 100U) break;
|
||||
}
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
}
|
||||
|
||||
adc_regular_channel_config(ADC1, 0, channel, ADC_SAMPLETIME_28POINT5);
|
||||
adc_software_trigger_enable(ADC1, ADC_REGULAR_CHANNEL);
|
||||
timeout = 0;
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
if (++timeout > 2000U) { // 硬件异常保护，避免死等
|
||||
break;
|
||||
}
|
||||
}
|
||||
u16 raw = (u16)adc_regular_data_read(ADC1);
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
return (u16)adc_regular_data_read(ADC1);
|
||||
|
||||
if (was_continuous) {
|
||||
/* 恢复 VBUS 连续转换 */
|
||||
adc_regular_channel_config(ADC1, 0, ADC_CHANNEL_6,
|
||||
ADC_SAMPLETIME_28POINT5);
|
||||
adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, ENABLE);
|
||||
adc_software_trigger_enable(ADC1, ADC_REGULAR_CHANNEL);
|
||||
/* 等恢复后的首个 ch6 转换完成并读走（清 EOC），
|
||||
* 避免 vbus 零等待读取在接下来的 ~1.4μs 窗口内
|
||||
* 读到 ch9 的残留值 */
|
||||
timeout = 0;
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
if (++timeout > 2000U) break;
|
||||
}
|
||||
(void)adc_regular_data_read(ADC1);
|
||||
}
|
||||
return raw;
|
||||
}
|
||||
|
||||
/*!
|
||||
@@ -172,7 +249,7 @@ u8 sampler::init() {
|
||||
currV_samples[i] = current_init;
|
||||
currW_samples[i] = current_init;
|
||||
|
||||
vbus_samples[i] = adc1_read_raw(ADC_CHANNEL_6) * ADC_SCALE * VCC_FACTOR;
|
||||
vbus_samples[i] = adc1_read_vbus_latest() * ADC_SCALE * VCC_FACTOR;
|
||||
|
||||
const float drv_raw = (float)adc1_read_raw(ADC_CHANNEL_9);
|
||||
float R_now = drv_raw / (4096 - drv_raw) * 4700;
|
||||
@@ -315,10 +392,10 @@ void sampler::update_current() {
|
||||
* @brief 更新供电电压
|
||||
*/
|
||||
void sampler::update_vbus_voltage() {
|
||||
// ADC1 单次轮询转换 ch6（电阻分压，低阻源）
|
||||
// ADC1 连续模式零等待读最新 VBUS（ch6，电阻分压，低阻源）
|
||||
// 环形缓冲区写入，替代 memmove
|
||||
vbus_samples[temp_vbus_head] =
|
||||
adc1_read_raw(ADC_CHANNEL_6) * ADC_SCALE * VCC_FACTOR;
|
||||
adc1_read_vbus_latest() * ADC_SCALE * VCC_FACTOR;
|
||||
temp_vbus_head++;
|
||||
if (temp_vbus_head >= ADC_FILTER_SIZE) temp_vbus_head = 0;
|
||||
|
||||
|
||||
+8
-261
@@ -22,7 +22,6 @@
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <random>
|
||||
|
||||
// CAN 调试宏定义
|
||||
#if CAN_DEBUG
|
||||
@@ -33,9 +32,6 @@
|
||||
#define CAN_DEBUG_PRINT(fmt, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// CAN0 固定外设号，编译期常量，非可变全局状态。
|
||||
/** @brief CANX 常量定义 */
|
||||
constexpr u16 CANX = CAN0;
|
||||
extern FOC_Controller FOC;
|
||||
extern sampler sampler;
|
||||
extern angle_sensor KTH7823;
|
||||
@@ -150,7 +146,8 @@ void CAN_transmiter::set_node_id(u16 new_id) {
|
||||
\param[out] none
|
||||
\retval FLASH_EEPROM_OK: 加载成功
|
||||
其他：加载失败，使用默认值
|
||||
\note 使用独立的节点 ID 存储区域，带魔数验证和 CRC 校验
|
||||
\note 使用独立的节点 ID 双备份存储区（主/备独立 Flash 页），
|
||||
单份损坏自动回退另一份；带魔数验证和 CRC 校验
|
||||
防止位翻转导致错读
|
||||
*/
|
||||
u8 CAN_transmiter::load_node_id_from_flash() {
|
||||
@@ -185,7 +182,8 @@ u8 CAN_transmiter::load_node_id_from_flash() {
|
||||
\param[out] none
|
||||
\retval FLASH_EEPROM_OK: 保存成功
|
||||
其他：保存失败
|
||||
\note 写入主备份和备用备份，确保数据可靠性
|
||||
\note 乒乓式写入主/备两个独立 Flash 页之一，另一页始终保有
|
||||
旧的有效记录，掉电时至少保留一份有效数据
|
||||
*/
|
||||
u8 CAN_transmiter::save_node_id_to_flash(u16 new_id) {
|
||||
if (new_id > MAX_NODE_ID) {
|
||||
@@ -337,67 +335,6 @@ void can_dma_init(void) {
|
||||
memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words));
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 初始化 CAN0 2.0B 模式
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
void can_config(void) {
|
||||
can_gpio_init();
|
||||
can_dma_init(); // 初始化 DMA
|
||||
can_parameter_struct can_parameter;
|
||||
can_filter_parameter_struct can_filter;
|
||||
|
||||
can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
|
||||
/* initialize CAN register */
|
||||
can_deinit(CAN0);
|
||||
|
||||
/* initialize CAN parameters */
|
||||
can_parameter.time_triggered = DISABLE;
|
||||
can_parameter.auto_bus_off_recovery = DISABLE;
|
||||
can_parameter.auto_wake_up = DISABLE;
|
||||
can_parameter.auto_retrans = DISABLE;
|
||||
can_parameter.rec_fifo_overwrite = DISABLE;
|
||||
can_parameter.trans_fifo_order = DISABLE;
|
||||
can_parameter.working_mode = CAN_NORMAL_MODE;
|
||||
/* configure CAN0 baud rate 1MBps, sample point at 80% */
|
||||
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
|
||||
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
|
||||
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
|
||||
can_parameter.prescaler = 6;
|
||||
/* initialize CAN */
|
||||
if (can_init(CAN0, &can_parameter) != SUCCESS) {
|
||||
while (1)
|
||||
CAN_DEBUG_PRINT("can init error \r\n");
|
||||
}
|
||||
|
||||
/* initialize filter */
|
||||
/* configure filter mode */
|
||||
can_filter.filter_mode = CAN_FILTERMODE_MASK;
|
||||
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
|
||||
/* configure filter mask */
|
||||
can_filter.filter_mask_high = 0x0000;
|
||||
can_filter.filter_mask_low = 0x0000;
|
||||
/* select receiver fifo */
|
||||
can_filter.filter_fifo_number = CAN_FIFO0;
|
||||
can_filter.filter_enable = ENABLE;
|
||||
/* configure filter number */
|
||||
can_filter.filter_number = 0;
|
||||
can_filter_init(&can_filter);
|
||||
|
||||
/* 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);
|
||||
/* enable can transmit mailbox empty interrupt */
|
||||
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
|
||||
|
||||
// 使能 DMA 中断
|
||||
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
|
||||
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 初始化 CAN0 CANFD 模式
|
||||
@@ -476,198 +413,6 @@ void canfd_config(void) {
|
||||
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF | DMA_INT_ERR);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 初始化 CAN0 回环测试
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
void can_loopback_init(void) {
|
||||
can_gpio_init();
|
||||
can_parameter_struct can_parameter;
|
||||
can_filter_parameter_struct can_filter;
|
||||
/* initialize CAN register */
|
||||
can_deinit(CANX);
|
||||
|
||||
/* initialize CAN */
|
||||
can_parameter.time_triggered = DISABLE;
|
||||
can_parameter.auto_bus_off_recovery = DISABLE;
|
||||
can_parameter.auto_wake_up = DISABLE;
|
||||
can_parameter.auto_retrans = DISABLE;
|
||||
can_parameter.rec_fifo_overwrite = DISABLE;
|
||||
can_parameter.trans_fifo_order = DISABLE;
|
||||
can_parameter.working_mode = CAN_NORMAL_MODE;
|
||||
/* baudrate 1Mbps, sample piont at 80% */
|
||||
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
|
||||
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
|
||||
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
|
||||
can_parameter.prescaler = 6;
|
||||
if (can_init(CANX, &can_parameter) != SUCCESS) {
|
||||
u16 i = 200;
|
||||
while (--i > 0) {
|
||||
delay_1ms(10);
|
||||
CAN_DEBUG_PRINT("can init error \r\n");
|
||||
}
|
||||
system_soft_reset();
|
||||
}
|
||||
|
||||
/* initialize filter */
|
||||
/* configure filter mode */
|
||||
can_filter.filter_mode = CAN_FILTERMODE_MASK;
|
||||
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
|
||||
/* configure filter ID */
|
||||
can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U);
|
||||
can_filter.filter_list_low =
|
||||
((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2);
|
||||
/* configure filter mask */
|
||||
can_filter.filter_mask_high = 0x0000;
|
||||
can_filter.filter_mask_low = 0x0000;
|
||||
/* select receiver fifo */
|
||||
can_filter.filter_fifo_number = CAN_FIFO0;
|
||||
can_filter.filter_enable = ENABLE;
|
||||
/* configure filter number */
|
||||
can_filter.filter_number = 0;
|
||||
can_filter_init(&can_filter);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief CAN0 FD 回环测试
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval ErrStatus
|
||||
*/
|
||||
void canfd_loopback_test(void) {
|
||||
can_trasnmit_message_struct transmit_message;
|
||||
can_receive_message_struct receive_message;
|
||||
uint32_t timeout = 0xFFFF;
|
||||
uint8_t transmit_mailbox = 0;
|
||||
|
||||
std::mt19937 gen;
|
||||
/**
|
||||
* @brief dis 函数
|
||||
* @return std::uniform_int_distribution<int> 返回值
|
||||
*/
|
||||
std::uniform_int_distribution<int> dis(0, 255);
|
||||
|
||||
can_struct_para_init(CAN_FD_FRAME_STRUCT, &transmit_message);
|
||||
transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID;
|
||||
transmit_message.tx_efid = 0x00;
|
||||
transmit_message.tx_ft = CAN_FT_DATA;
|
||||
transmit_message.tx_ff = CAN_FF_EXTENDED;
|
||||
transmit_message.tx_dlen = 7;
|
||||
transmit_message.fd_flag = 1;
|
||||
transmit_message.fd_brs = 1;
|
||||
transmit_message.fd_esi = 0;
|
||||
|
||||
int i = 0;
|
||||
while (i++ < transmit_message.tx_dlen) {
|
||||
transmit_message.tx_data[i] = dis(gen);
|
||||
CAN_DEBUG_PRINT("%x\n", transmit_message.tx_data[i]);
|
||||
}
|
||||
|
||||
/* transmit message */
|
||||
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
|
||||
/* waiting for transmit completed */
|
||||
while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) &&
|
||||
(0 != timeout)) {
|
||||
timeout--;
|
||||
}
|
||||
timeout = 0xFFFF;
|
||||
/* waiting for receive completed */
|
||||
while ((can_receive_message_length_get(CAN0, CAN_FIFO0) < 1) &&
|
||||
(0 != timeout)) {
|
||||
timeout--;
|
||||
}
|
||||
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
|
||||
can_message_receive(CAN0, CAN_FIFO0, &receive_message);
|
||||
|
||||
/* check the receive message */
|
||||
if ((transmit_message.tx_sfid == receive_message.rx_sfid) &&
|
||||
(transmit_message.tx_ff == receive_message.rx_ff) &&
|
||||
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
|
||||
/* loopback test is success */
|
||||
CAN_DEBUG_PRINT("CAN LO0PBACK SUCCESS\r\n");
|
||||
} else {
|
||||
/* loopback test is failed */
|
||||
CAN_DEBUG_PRINT("CAN LO0PBACK FAIL\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
\brief CAN0 回环测试
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
void can_loopback_test(void) {
|
||||
can_trasnmit_message_struct transmit_message;
|
||||
can_receive_message_struct receive_message;
|
||||
uint32_t timeout = 0xFF;
|
||||
uint8_t transmit_mailbox = 0;
|
||||
|
||||
/**
|
||||
* @brief gen 函数
|
||||
* @param[in] rand 参数
|
||||
* @return std::mt19937 返回值
|
||||
*/
|
||||
std::mt19937 gen(rand()); // 使用随机设备生成的种子
|
||||
/**
|
||||
* @brief dis 函数
|
||||
* @param[in] xff 参数
|
||||
* @return std::uniform_int_distribution<int> 返回值
|
||||
*/
|
||||
std::uniform_int_distribution<int> dis(0, 0xff);
|
||||
|
||||
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
|
||||
/* initialize transmit message */
|
||||
transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID;
|
||||
transmit_message.tx_ft = CAN_FT_DATA;
|
||||
transmit_message.tx_ff = CAN_FF_STANDARD;
|
||||
transmit_message.tx_dlen = 7;
|
||||
int i = 0;
|
||||
while (i++ < transmit_message.tx_dlen) {
|
||||
// transmit_message.tx_data[i] = dis(gen);
|
||||
transmit_message.tx_data[i] = i;
|
||||
}
|
||||
/* transmit message */
|
||||
transmit_mailbox = can_message_transmit(CANX, &transmit_message);
|
||||
/* waiting for transmit completed */
|
||||
while ((CAN_TRANSMIT_OK != can_transmit_states(CANX, transmit_mailbox)) &&
|
||||
(0 != timeout)) {
|
||||
CAN_DEBUG_PRINT("sending:%x...\n",
|
||||
can_transmit_states(CANX, transmit_mailbox));
|
||||
timeout--;
|
||||
}
|
||||
timeout = 0xFF;
|
||||
/* waiting for receive completed */
|
||||
while ((can_receive_message_length_get(CANX, CAN_FIFO0) < 1) &&
|
||||
(0 != timeout)) {
|
||||
timeout--;
|
||||
}
|
||||
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
|
||||
can_message_receive(CANX, CAN_FIFO0, &receive_message);
|
||||
/* check the receive message */
|
||||
if ((DEV_CAN_LOOPBACK_ID == receive_message.rx_sfid) &&
|
||||
(CAN_FF_STANDARD == receive_message.rx_ff) &&
|
||||
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
|
||||
i = transmit_message.tx_dlen;
|
||||
while (i-- > 0) {
|
||||
CAN_DEBUG_PRINT("rx %d:%x:%x\n", i, transmit_message.tx_data[i],
|
||||
receive_message.rx_data[i]);
|
||||
}
|
||||
if (std::memcmp(receive_message.rx_data, transmit_message.tx_data,
|
||||
transmit_message.tx_dlen) == 0) {
|
||||
/* loopback test is success */
|
||||
CAN_DEBUG_PRINT("loopback success\n");
|
||||
} else {
|
||||
/* loopback test is failed */
|
||||
CAN_DEBUG_PRINT("loopback failed\n");
|
||||
}
|
||||
} else {
|
||||
/* loopback test is failed */
|
||||
CAN_DEBUG_PRINT("loopback failed\n");
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief CAN_transmiter 构造函数
|
||||
@@ -675,7 +420,6 @@ void can_loopback_test(void) {
|
||||
*/
|
||||
u8 CAN_transmiter::init() {
|
||||
// 初始化 CAN 硬件
|
||||
// can_config();
|
||||
canfd_config();
|
||||
|
||||
// 初始化发送消息结构体
|
||||
@@ -1241,7 +985,11 @@ void CAN_transmiter::handle_control_command(can_receive_message_struct ¤t_
|
||||
FOC.target_velocity = target_velocity_union.f;
|
||||
FOC.target_current = target_current_union.f;
|
||||
|
||||
// limit_voltage=0 时 P 换算除零会得到 inf 增益，钳到下限
|
||||
float voltage = FOC.limit_voltage;
|
||||
if (voltage < MIN_LIMIT_VOLTAGE) {
|
||||
voltage = MIN_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;
|
||||
@@ -1299,7 +1047,6 @@ void CAN_transmiter::handle_ota_ack_message() {
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -122,11 +122,6 @@ static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
|
||||
* @return u8 返回值
|
||||
*/
|
||||
u8 can_gpio_init(void); // CAN 引脚初始化
|
||||
/**
|
||||
* @brief 配置 CAN 外设
|
||||
* @return void
|
||||
*/
|
||||
void can_config(void); // CAN 初始化
|
||||
/**
|
||||
* @brief 配置 CAN FD 外设
|
||||
* @return void
|
||||
@@ -163,16 +158,6 @@ void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发
|
||||
*/
|
||||
void jumpToBOOT();
|
||||
|
||||
/**
|
||||
* @brief can_loopback_init 函数
|
||||
* @return void
|
||||
*/
|
||||
void can_loopback_init(void);
|
||||
/**
|
||||
* @brief can_loopback_test 函数
|
||||
* @return void
|
||||
*/
|
||||
void can_loopback_test(void);
|
||||
|
||||
/*!
|
||||
\brief CAN 通信类
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
#include "Can.h"
|
||||
#include "FOC.h"
|
||||
#include "Timer.h"
|
||||
#include "config.h"
|
||||
#include "gd32c10x.h"
|
||||
#include "gd32c10x_gpio.h"
|
||||
#include "gd32c10x_misc.h"
|
||||
@@ -23,16 +24,13 @@ extern CAN_transmiter CAN0_transmiter;
|
||||
extern u32 cnt_test;
|
||||
|
||||
// 任务调度查找表 - 每 100us 执行的任务 (1ms 周期内)
|
||||
// MCU/DRV 温度保持 1kHz：热模型兼作过流保护，温度积累按 1ms 步进，
|
||||
// 每毫秒都要拿到新温度采样。
|
||||
static const u32 task_schedule_100us[10] = {
|
||||
(1u << TASK_MCU_TEMP_UPDATE), // 0: MCU 温度更新
|
||||
(1u << TASK_DRV_TEMP_UPDATE), // 1: 驱动温度更新
|
||||
(1u << TASK_VBUS_UPDATE), // 2: 电压更新
|
||||
0, // 3: 空闲
|
||||
0, // 4: 空闲
|
||||
0, // 5: 空闲
|
||||
0, // 6: 空闲
|
||||
0, // 7: 空闲
|
||||
0, // 8: 空闲
|
||||
0, 0, 0, 0, 0, 0, // 3~8: 空闲
|
||||
(1u << TASK_CAN_TRANSMIT) // 9: CAN 发送
|
||||
};
|
||||
|
||||
@@ -197,10 +195,18 @@ void timer1_gpio_config(void) {
|
||||
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
|
||||
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
|
||||
|
||||
#if GATE_DRIVER_DRV8323
|
||||
/* 新板：PB12(DRVSLP=ENABLE+INLx) 由 gate_driver 统一管理——
|
||||
* gate_driver_preinit() 已置低保持芯片睡眠，gate_driver_bringup()
|
||||
* 在 SPI 配置完成后才拉高。此处严禁提前置高（SPI 未配置前芯片
|
||||
* 带默认 6x PWM 模式醒来，INLx=1 时上管永远无法导通）。
|
||||
* PB0 板上接地未用，不配置。 */
|
||||
#else
|
||||
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,
|
||||
GPIO_PIN_0 | GPIO_PIN_12);
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_0);
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_12);
|
||||
#endif
|
||||
|
||||
/* 配置 TIMER1 完全重映射*/
|
||||
gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE);
|
||||
@@ -295,6 +301,15 @@ void TIMER1_IRQHandler(void) {
|
||||
void Set_Pwm(u16 channel, float duty_cycle) {
|
||||
float pulse = duty_cycle * TIMER1_PERIOD;
|
||||
|
||||
#if GATE_DRIVER_DRV8323
|
||||
/* 新板通道重映射：逻辑相 A→CH2、B→CH1、C→CH0（硬件 INHA/B/C
|
||||
* 分别接 PWM1/2/3 = PA2/PA1/PA0），与 Set_3Pwm 保持一致 */
|
||||
if (channel == 0) {
|
||||
channel = 2;
|
||||
} else if (channel == 2) {
|
||||
channel = 0;
|
||||
}
|
||||
#endif
|
||||
if (channel <= 3) {
|
||||
timer_channel_output_pulse_value_config(TIMER1, channel, pulse);
|
||||
} else {
|
||||
@@ -311,8 +326,17 @@ void Set_Pwm(u16 channel, float duty_cycle) {
|
||||
\note The channel parameter must be between 0 and 3 inclusive.
|
||||
*/
|
||||
void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) {
|
||||
// 直接写寄存器
|
||||
#if GATE_DRIVER_DRV8323
|
||||
/* 新板硬件映射：CH0(PA0)→PWM3→INHC，CH1(PA1)→PWM2→INHB，
|
||||
* CH2(PA2)→PWM1→INHA。入参按 (A,B,C) 相序书写，此处把 A/C 换到
|
||||
* 物理通道，修正新板与老板相反的 INHx 接线。 */
|
||||
TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
|
||||
TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
|
||||
TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
|
||||
#else
|
||||
// 直接写寄存器（老板：CH0/1/2 = IN1/2/3 = A/B/C 相）
|
||||
TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
|
||||
TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
|
||||
TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
#ifndef _TIMER_H
|
||||
#define _TIMER_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
+15
-2
@@ -91,15 +91,28 @@ class sampler {
|
||||
void ADC0_1_IRQHandler(void);
|
||||
|
||||
/**
|
||||
* @brief ADC1 初始化（VBUS / DRV温度 专用，单通道单次软件触发）
|
||||
* @brief ADC1 初始化：VBUS 连续转换模式（零等待读取）
|
||||
* @return void
|
||||
* @note ADC1 上电后即对 ch6(VBUS) 连续转换，软件触发一次后自持续，
|
||||
* 之后读取 DR 寄存器即可拿到最新完成的转换值（无需阻塞等待）。
|
||||
*/
|
||||
void adc1_config(void);
|
||||
|
||||
/**
|
||||
* @brief 零等待读取最新 VBUS 原始值（连续模式下任意时刻可调）
|
||||
* @return u16 最近一次完成转换的原始值（陈旧度约 1 个转换周期 ≈1.4μs）
|
||||
* @note 仅主循环任务上下文调用（与其他 ADC1 访问串行，无并发）。
|
||||
* 原理：读 DR 硬件清 EOC，随后下一次完成转换会刷新 DR，
|
||||
* 其间完成的转换被丢弃——每次读到的都是刚完成的样本。
|
||||
*/
|
||||
u16 adc1_read_vbus_latest(void);
|
||||
|
||||
/**
|
||||
* @brief ADC1 单次软件触发转换并等待完成（仅主循环任务上下文调用）
|
||||
* @param[in] channel ADC 通道号（ADC_CHANNEL_6 / ADC_CHANNEL_9）
|
||||
* @param[in] channel ADC 通道号（如 ADC_CHANNEL_9）
|
||||
* @return u16 转换原始值
|
||||
* @note 会临时暂停 VBUS 连续模式、切到目标通道单次转换、再恢复连续，
|
||||
* 全程约 3~5μs，期间 VBUS 读数最多滞后该时长。drv 温度 1kHz 用。
|
||||
*/
|
||||
u16 adc1_read_raw(u8 channel);
|
||||
|
||||
|
||||
+24
-107
@@ -34,10 +34,6 @@
|
||||
/** @brief DRV_SPI_CS_DISABLE 宏定义 */
|
||||
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
|
||||
|
||||
// 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; // 角度传感器初始化
|
||||
@@ -47,120 +43,42 @@ extern EncoderVelocityKalman Vel_kalman;
|
||||
extern CAN_transmiter CAN0_transmiter; // CAN 初始化
|
||||
|
||||
/**
|
||||
* @brief SPI DMA initialization
|
||||
*/
|
||||
void spi_dma_init(void) {
|
||||
dma_parameter_struct dma_init_struct;
|
||||
|
||||
// 使能 DMA 时钟
|
||||
rcu_periph_clock_enable(RCU_DMA0);
|
||||
|
||||
// 初始化发送 DMA 通道
|
||||
dma_deinit(DMA0, DMA_CH2);
|
||||
dma_struct_para_init(&dma_init_struct);
|
||||
dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
|
||||
dma_init_struct.memory_addr = (uint32_t)0; // 实际地址将在传输时设置
|
||||
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
|
||||
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_16BIT;
|
||||
dma_init_struct.number = 1;
|
||||
dma_init_struct.periph_addr = (uint32_t)(&SPI_DATA(SPI0));
|
||||
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
|
||||
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
|
||||
dma_init_struct.priority = DMA_PRIORITY_HIGH;
|
||||
dma_init(DMA0, DMA_CH2, &dma_init_struct);
|
||||
|
||||
// 初始化接收 DMA 通道
|
||||
dma_deinit(DMA0, DMA_CH1);
|
||||
dma_struct_para_init(&dma_init_struct);
|
||||
dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
|
||||
dma_init_struct.memory_addr = (uint32_t)0; // 实际地址将在传输时设置
|
||||
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
|
||||
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_16BIT;
|
||||
dma_init_struct.number = 1;
|
||||
dma_init_struct.periph_addr = (uint32_t)(&SPI_DATA(SPI0));
|
||||
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
|
||||
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
|
||||
dma_init_struct.priority = DMA_PRIORITY_HIGH;
|
||||
dma_init(DMA0, DMA_CH1, &dma_init_struct);
|
||||
|
||||
// 使能 SPI 的 DMA 发送和接收功能
|
||||
spi_dma_enable(SPI0, SPI_DMA_TRANSMIT);
|
||||
spi_dma_enable(SPI0, SPI_DMA_RECEIVE);
|
||||
|
||||
// 配置 DMA 中断
|
||||
nvic_irq_enable(DMA0_Channel2_IRQn, 0, 1);
|
||||
nvic_irq_enable(DMA0_Channel1_IRQn, 0, 1);
|
||||
|
||||
// 使能 DMA 传输完成中断
|
||||
dma_interrupt_enable(DMA0, DMA_CH2, DMA_INT_FTF);
|
||||
dma_interrupt_enable(DMA0, DMA_CH1, DMA_INT_FTF);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send or read half word through SPI using DMA
|
||||
* @brief Send or read half word through SPI by register polling
|
||||
*
|
||||
* @param[in] spi_periph SPI peripheral
|
||||
* @param[in] tx_data Data to transmit
|
||||
* @param[out] rx_data Pointer to store received data
|
||||
* @return 0 if success, -1 if error
|
||||
* @note 单半字轮询收发。DMA 版本（已移除）每次传输需要：重配 2 个 DMA
|
||||
* 通道(约 10 次寄存器写) + 2 次 FTF 中断 + 忙等完成标志，而 16bit
|
||||
* @5MHz 帧本身只有 3.2μs——DMA 的准备与中断开销和传输时间同量级。
|
||||
* 轮询同样忙等帧时间，但砍掉了全部 DMA 开销与 40k 次/秒的中断。
|
||||
*/
|
||||
int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
|
||||
int spi_rw_half_word_polling(uint32_t spi_periph, uint16_t tx_data,
|
||||
uint16_t *rx_data) {
|
||||
// 重置完成标志
|
||||
spi_dma_tx_complete = false;
|
||||
spi_dma_rx_complete = false;
|
||||
u32 timeout = 1000;
|
||||
|
||||
// 配置发送 DMA
|
||||
dma_channel_disable(DMA0, DMA_CH2);
|
||||
dma_memory_address_config(DMA0, DMA_CH2, (uint32_t)&tx_data);
|
||||
dma_transfer_number_config(DMA0, DMA_CH2, 1);
|
||||
dma_channel_enable(DMA0, DMA_CH2);
|
||||
|
||||
// 配置接收 DMA
|
||||
dma_channel_disable(DMA0, DMA_CH1);
|
||||
dma_memory_address_config(DMA0, DMA_CH1, (uint32_t)rx_data);
|
||||
dma_transfer_number_config(DMA0, DMA_CH1, 1);
|
||||
dma_channel_enable(DMA0, DMA_CH1);
|
||||
|
||||
// 等待传输完成，超时时间为 100 个循环
|
||||
uint32_t timeout = 100;
|
||||
while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
|
||||
// 空循环等待 DMA 传输完成
|
||||
}
|
||||
|
||||
// 检查是否超时
|
||||
if (timeout == 0) {
|
||||
// 禁用 DMA 通道
|
||||
dma_channel_disable(DMA0, DMA_CH2);
|
||||
dma_channel_disable(DMA0, DMA_CH1);
|
||||
ANG_SENS_DEBUG_PRINT("SPI DMA transfer timeout\r\n");
|
||||
while (spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE) == RESET) {
|
||||
if (--timeout == 0U) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
spi_i2s_data_transmit(spi_periph, tx_data);
|
||||
|
||||
timeout = 1000;
|
||||
while (spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) == RESET) {
|
||||
if (--timeout == 0U) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
*rx_data = spi_i2s_data_receive(spi_periph);
|
||||
|
||||
// 等待移位完成再返回：调用方返回后立即拉高 CS，
|
||||
// 若最后一位仍在移位会截断帧
|
||||
while (spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS) != RESET) {
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief DMA0_Channel2 中断处理函数
|
||||
* @param 无
|
||||
*/
|
||||
extern "C" void DMA0_Channel2_IRQHandler(void) {
|
||||
if (dma_interrupt_flag_get(DMA0, DMA_CH2, DMA_INT_FLAG_FTF)) {
|
||||
dma_interrupt_flag_clear(DMA0, DMA_CH2, DMA_INT_FLAG_FTF);
|
||||
spi_dma_tx_complete = true;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief DMA0_Channel1 中断处理函数
|
||||
* @param 无
|
||||
*/
|
||||
extern "C" void DMA0_Channel1_IRQHandler(void) {
|
||||
if (dma_interrupt_flag_get(DMA0, DMA_CH1, DMA_INT_FLAG_FTF)) {
|
||||
dma_interrupt_flag_clear(DMA0, DMA_CH1, DMA_INT_FLAG_FTF);
|
||||
spi_dma_rx_complete = true;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief 角度传感器初始化
|
||||
* @param 无
|
||||
@@ -216,7 +134,6 @@ void KTH7823_spi_init(void) {
|
||||
u8 angle_sensor::init(void) {
|
||||
KTH7823_gpio_init(); // GPIO 初始化
|
||||
KTH7823_spi_init(); // SPI 初始化
|
||||
spi_dma_init(); // DMA 初始化
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -239,7 +156,7 @@ inline float angle_sensor::read_sensorAngle(void) {
|
||||
uint16_t raw_data;
|
||||
|
||||
ANG_SPI_CS_ENABLE();
|
||||
u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
|
||||
u8 result = spi_rw_half_word_polling(SPI0, DEFAULT_COMMAND, &raw_data);
|
||||
ANG_SPI_CS_DISABLE();
|
||||
|
||||
if (result == 0) {
|
||||
|
||||
@@ -21,13 +21,16 @@ extern "C" {
|
||||
*/
|
||||
u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
|
||||
/**
|
||||
* @brief spi_rw_half_word_dma 函数
|
||||
* @param[in] spi_periph 参数
|
||||
* @param[in] tx_data 参数
|
||||
* @param[in] rx_data 参数
|
||||
* @return int 返回值
|
||||
* @brief spi_rw_half_word_polling 函数
|
||||
* @param[in] spi_periph SPI 外设
|
||||
* @param[in] tx_data 发送的半字
|
||||
* @param[out] rx_data 接收数据输出指针
|
||||
* @return int 0 成功，-1 超时
|
||||
* @note 单个半字收发：直接寄存器轮询。原 DMA 实现每次传输要重配两个
|
||||
* DMA 通道并触发两次 FTF 中断，之后 CPU 仍忙等完成——对单半字
|
||||
* 传输 DMA 只有开销没有收益（20kHz 调用下约 40k 次/秒额外中断）。
|
||||
*/
|
||||
int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
|
||||
int spi_rw_half_word_polling(uint32_t spi_periph, uint16_t tx_data,
|
||||
uint16_t *rx_data);
|
||||
|
||||
|
||||
@@ -91,8 +94,6 @@ class angle_sensor {
|
||||
float current_angle_rad;
|
||||
bool m_firstUpdate;
|
||||
float m_lastAngle;
|
||||
float m_lastcumulativeAngle;
|
||||
float velocity[VEL_FILTER_SIZE] = {0.0f};
|
||||
};
|
||||
/**
|
||||
* @brief 快速角度归一化到 [0, 360)
|
||||
|
||||
+210
-10
@@ -1,15 +1,218 @@
|
||||
/**
|
||||
* @file device/motor.cpp
|
||||
* @brief 底层设备驱动与外设封装源文件
|
||||
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
|
||||
* @note 功率级门驱动芯片抽象层：
|
||||
* - 老板 DRV8313（集成 FET 三半桥，无 SPI）：保持原使能时序
|
||||
* - 新板 DRV8323RS（外置 MOS）：使能前经 SPI 配置 3x PWM 模式、
|
||||
* CSA 增益、VDS 过流门限、死区与 IDRIVE，并逐条回读校验
|
||||
*/
|
||||
|
||||
#include "motor.h"
|
||||
#include "Timer.h"
|
||||
#include "config.h"
|
||||
#include "ang_sensor.h"
|
||||
#include "gd32c10x_eval.h"
|
||||
#include "gd32c10x_gpio.h"
|
||||
#include "gd32c10x_spi.h"
|
||||
#include "systick.h"
|
||||
#include <cstdio>
|
||||
|
||||
/* 角度传感器 SPI 初始化（ang_sensor.cpp）：DRV 配置完成后恢复其 SPI 模式 */
|
||||
extern void KTH7823_spi_init(void);
|
||||
|
||||
/**
|
||||
* @brief 门驱动芯片引脚预初始化
|
||||
* @note 新板：DRVSLP(PB12) 置低保持 DRV8323 睡眠、片选 PB14 释放、
|
||||
* PB2(nFAULT) 浮空输入。必须在任何可能拉高使能脚的代码之前调用；
|
||||
* 老板：无操作（原时序在 timer1_gpio_config 中保持不变）。
|
||||
*/
|
||||
void gate_driver_preinit(void) {
|
||||
#if GATE_DRIVER_DRV8323
|
||||
rcu_periph_clock_enable(RCU_GPIOB);
|
||||
/* 使能脚推挽输出并保持低：芯片睡眠，栅极全 Hi-Z，寄存器默认值不起作用 */
|
||||
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12);
|
||||
gpio_bit_reset(GPIOB, GPIO_PIN_12);
|
||||
/* SPI 片选默认释放（SCLK 空闲低时才允许 CS 翻转） */
|
||||
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_14);
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_14);
|
||||
/* nFAULT 低有效 */
|
||||
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
|
||||
#endif
|
||||
}
|
||||
|
||||
#if GATE_DRIVER_DRV8323
|
||||
|
||||
/* ---------------- DRV8323 SPI ----------------
|
||||
* 帧格式：[15:11] 寄存器地址 [10] 1读0写 [9:0] 数据。
|
||||
* 时序：SCLK 空闲低、SDI 下降沿采样（SPI 模式1），与角度传感器
|
||||
* （模式3）不同：配置前重初始化 SPI0，配置完调 KTH7823_spi_init() 恢复。
|
||||
* 寄存器/位定义来源：TI SLVSDJ3D（DRV832x datasheet）Register Maps。 */
|
||||
#define DRV_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
|
||||
#define DRV_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
|
||||
|
||||
#define DRV8323_REG_FAULT1 0x00u /* FAULT/VDS_OCP/GDF/UVLO/OTSD/VDS_Hx,VDS_Lx */
|
||||
#define DRV8323_REG_VGS2 0x01u /* SA/SB/SC_OC、OTW、CPUV、VGS_Hx,VGS_Lx */
|
||||
#define DRV8323_REG_DRVCTRL 0x02u /* [6:5]PWM_MODE [2]COAST [1]BRAKE [0]CLR_FLT */
|
||||
#define DRV8323_REG_GDRV_HS 0x03u /* [10]LOCK [9:6]IDRIVEP_HS [5:2]IDRIVEN_HS */
|
||||
#define DRV8323_REG_GDRV_LS 0x04u /* [10]CBC [9:8]TDRIVE [7:4]IDRIVEP_LS [3:0]IDRIVEN_LS */
|
||||
#define DRV8323_REG_OCPCTRL 0x05u /* [10]TRETRY [9:8]DEAD_TIME [7:6]OCP_MODE [5:4]OCP_DEG [3:0]VDS_LVL */
|
||||
#define DRV8323_REG_CSACTRL 0x06u /* [10]CSA_FET [9]VREF_DIV [8]LS_REF [7:6]CSA_GAIN [1:0]SEN_LVL */
|
||||
|
||||
/* PWM_MODE=01b：3x PWM。INHx 低→下管开、高→上管开，死区芯片内部插入。
|
||||
芯片复位默认 00b(6x PWM)，6x 下 INLx 语义不同，本板 INLx=DRVSLP 常高，
|
||||
必须切到 3x，否则上管永远开不了（电流全走体二极管）。 */
|
||||
#define DRV8323_PWM_MODE_3X (1u << 5)
|
||||
/* 栅驱动电流：源 170mA/灌 190mA（WSD6035DN33 Qg≈12.6nC，边沿约 75ns，
|
||||
* 无栅电阻布局下避免过快边沿加重振铃） */
|
||||
#define DRV8323_IDRIVE_HS ((0x6u << 6) | (0x7u << 2))
|
||||
#define DRV8323_IDRIVE_HS_MASK ((0xFu << 6) | (0xFu << 2))
|
||||
#define DRV8323_IDRIVE_LS ((0x6u << 4) | (0x7u << 0))
|
||||
#define DRV8323_IDRIVE_LS_MASK ((0xFu << 4) | (0xFu << 0))
|
||||
/* 死区 200ns（20kHz 大 Qg 管的安全值）；OCP_MODE=10b 仅上报不动作：
|
||||
* VDS 门限受 RDSon 温漂影响大（热态 20→30mΩ），先靠软件限流+热模型，
|
||||
* 避免误跳闸；VDS_LVL=0.75V（默认），故障位可经 drv_fault_status() 读取 */
|
||||
#define DRV8323_OCP_CFG ((0x2u << 8) | (0x2u << 6) | (0x9u << 0))
|
||||
#define DRV8323_OCP_CFG_MASK ((0x3u << 8) | (0x3u << 6) | (0xFu << 0))
|
||||
|
||||
/** @brief CSA 增益值(V/V) → 0x06[7:6] 编码（5:00b 10:01b 20:10b 40:11b） */
|
||||
constexpr u16 drv8323_gain_code(float g) {
|
||||
return (g <= 5.0f) ? 0x0u : (g <= 10.0f) ? 0x1u : (g <= 20.0f) ? 0x2u : 0x3u;
|
||||
}
|
||||
#define DRV8323_CSA_GAIN_BITS (drv8323_gain_code(DRV8323_CSA_GAIN_VV) << 6)
|
||||
|
||||
/**
|
||||
* @brief DRV8323 单帧 SPI 收发（片选内 16bit，帧间隔由调用节奏保证）
|
||||
* @param[in] frame 完整 16bit 帧
|
||||
* @return u16 从机返回的 10bit 数据
|
||||
*/
|
||||
static u16 drv8323_xfer(u16 frame) {
|
||||
u16 rx = 0;
|
||||
DRV_CS_ENABLE();
|
||||
spi_rw_half_word_polling(SPI0, frame, &rx);
|
||||
DRV_CS_DISABLE();
|
||||
return rx & 0x03FFu;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读改写一个寄存器并回读校验
|
||||
* @param[in] addr 寄存器地址
|
||||
* @param[in] clrmask 要清零的位
|
||||
* @param[in] setmask 要置一的位
|
||||
* @return u8 0=回读一致 1=校验失败
|
||||
*/
|
||||
static u8 drv8323_write_verify(u8 addr, u16 clrmask, u16 setmask) {
|
||||
u16 cur = drv8323_xfer((u16)(((u16)addr << 11) | (1u << 10)));
|
||||
u16 nv = (u16)((cur & ~clrmask) | setmask);
|
||||
drv8323_xfer((u16)(((u16)addr << 11) | nv));
|
||||
u16 rb = drv8323_xfer((u16)(((u16)addr << 11) | (1u << 10)));
|
||||
return (rb == nv) ? 0u : 1u;
|
||||
}
|
||||
|
||||
#endif /* GATE_DRIVER_DRV8323 */
|
||||
|
||||
/**
|
||||
* @brief 门驱动芯片上电配置
|
||||
* @return u8 0=成功；非 0 按位指示回读校验失败的寄存器
|
||||
* （bit0:DRVCTRL bit1:GDRV_HS bit2:GDRV_LS bit3:OCPCTRL bit4:CSACTRL）
|
||||
* @note 调用时机：TIMER1/PWM 已配置且占空比为 0 之后。拉高 DRVSLP 唤醒
|
||||
* 芯片的瞬间 INHx=0、INLx=1，默认 6x 模式下为"三相下管导通"，
|
||||
* 静止电机零电流，与 3x 模式 0% 占空比稳态一致，随后写模式无扰动。
|
||||
*/
|
||||
u8 gate_driver_bringup(void) {
|
||||
#if GATE_DRIVER_DRV8323
|
||||
/* 1. 唤醒芯片：DRVSLP 网络 200nF + 芯片内部复位，留足建立时间 */
|
||||
drv_enable(1);
|
||||
delay_1ms(10);
|
||||
|
||||
/* 2. SPI0 切到 DRV8323 时序（模式1：CPOL=0/下降沿采样，2MHz） */
|
||||
spi_parameter_struct spi_init_str;
|
||||
spi_struct_para_init(&spi_init_str);
|
||||
spi_init_str.device_mode = SPI_MASTER;
|
||||
spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
|
||||
spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
|
||||
spi_init_str.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE;
|
||||
spi_init_str.nss = SPI_NSS_SOFT;
|
||||
spi_init_str.endian = SPI_ENDIAN_MSB;
|
||||
spi_init_str.prescale = 60; /* 120MHz/60 = 2MHz */
|
||||
spi_init(SPI0, &spi_init_str);
|
||||
spi_nss_output_enable(SPI0);
|
||||
spi_nssp_mode_enable(SPI0);
|
||||
spi_enable(SPI0);
|
||||
|
||||
/* 3. 配置并逐条回读校验（读改写，保留未触碰位的出厂默认：
|
||||
* VREF_DIV=1→SOx 中点 VREF/2，与 CURRENT_OFFSET 的 1.655V 一致；
|
||||
* CSA_FET=0→正端 SPx，与 shunt 的 SP/SN 接法一致） */
|
||||
u8 err = 0;
|
||||
err |= (u8)(drv8323_write_verify(DRV8323_REG_DRVCTRL, 0x0000u,
|
||||
DRV8323_PWM_MODE_3X)
|
||||
<< 0);
|
||||
err |= (u8)(drv8323_write_verify(DRV8323_REG_GDRV_HS,
|
||||
DRV8323_IDRIVE_HS_MASK, DRV8323_IDRIVE_HS)
|
||||
<< 1);
|
||||
err |= (u8)(drv8323_write_verify(DRV8323_REG_GDRV_LS,
|
||||
DRV8323_IDRIVE_LS_MASK, DRV8323_IDRIVE_LS)
|
||||
<< 2);
|
||||
err |= (u8)(drv8323_write_verify(DRV8323_REG_OCPCTRL, DRV8323_OCP_CFG_MASK,
|
||||
DRV8323_OCP_CFG)
|
||||
<< 3);
|
||||
err |= (u8)(drv8323_write_verify(DRV8323_REG_CSACTRL, 0x3u << 6,
|
||||
DRV8323_CSA_GAIN_BITS)
|
||||
<< 4);
|
||||
|
||||
/* 4. CLR_FLT 脉冲清一次上电故障锁存，再恢复正常值 */
|
||||
drv8323_xfer((u16)((DRV8323_REG_DRVCTRL << 11) | DRV8323_PWM_MODE_3X | 0x1u));
|
||||
drv8323_xfer((u16)((DRV8323_REG_DRVCTRL << 11) | DRV8323_PWM_MODE_3X));
|
||||
|
||||
/* 5. 恢复角度传感器的 SPI 模式（模式3） */
|
||||
KTH7823_spi_init();
|
||||
return err;
|
||||
#else
|
||||
return 0; /* 老板 DRV8313 无 SPI，原使能时序保持不变 */
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 门驱动运行时使能/睡眠
|
||||
* @note 新板：PB12 同时是 DRV8323 的 ENABLE 与 INLA/B/C 电平——
|
||||
* 高=正常运转，低=芯片睡眠三相 Hi-Z（惯性滑行，兼作 MCU 复位
|
||||
* 时的天然 fail-safe）；老板：PB12+PC13 双脚，保持原行为。
|
||||
*/
|
||||
void drv_enable(u8 en) {
|
||||
if (en) {
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_12);
|
||||
#if !GATE_DRIVER_DRV8323
|
||||
gpio_bit_set(MOTOR_EN_GPIO_PORT, MOTOR_EN_GPIO_PIN);
|
||||
#endif
|
||||
} else {
|
||||
gpio_bit_reset(GPIOB, GPIO_PIN_12);
|
||||
#if !GATE_DRIVER_DRV8323
|
||||
gpio_bit_reset(MOTOR_EN_GPIO_PORT, MOTOR_EN_GPIO_PIN);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取 nFAULT 引脚状态（PB2，低有效）
|
||||
* @return u8 1=故障有效 0=正常
|
||||
*/
|
||||
u8 drv_nfault_active(void) {
|
||||
return (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET) ? 1u : 0u;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取门驱动故障状态寄存器
|
||||
* @return u32 [9:0]=Fault Status1(0x00) [19:10]=VGS Status2(0x01)；老板返回 0
|
||||
*/
|
||||
u32 drv_fault_status(void) {
|
||||
#if GATE_DRIVER_DRV8323
|
||||
u32 f1 = drv8323_xfer((u16)((DRV8323_REG_FAULT1 << 11) | (1u << 10)));
|
||||
u32 f2 = drv8323_xfer((u16)((DRV8323_REG_VGS2 << 11) | (1u << 10)));
|
||||
return f1 | (f2 << 10);
|
||||
#else
|
||||
return 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief driver输出初始化
|
||||
*/
|
||||
@@ -18,16 +221,17 @@ void motor_init(void) {
|
||||
timer1_pwm_config();
|
||||
// 使能GPIOB时钟
|
||||
rcu_periph_clock_enable(RCU_GPIOB);
|
||||
// 配置PB2为输入引脚，用于读取电机状态
|
||||
// 配置PB2为输入引脚，用于读取电机状态(nFAULT)
|
||||
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
|
||||
/* 配置GPIOB的PB14为推挽输出 SPICS */
|
||||
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,
|
||||
GPIO_PIN_14 | GPIO_PIN_12);
|
||||
|
||||
#if !GATE_DRIVER_DRV8323
|
||||
// 使能GPIOC时钟，用于电机驱动使能引脚
|
||||
rcu_periph_clock_enable(RCU_MOTOR_EN_GPIO);
|
||||
// 配置PC13为推挽输出，用于控制电机驱动器使能
|
||||
// 配置PC13为推挽输出，用于控制电机驱动器使能（老板专用）
|
||||
gpio_init(MOTOR_EN_GPIO_PORT, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, MOTOR_EN_GPIO_PIN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -45,16 +249,12 @@ void motor_reset(void) {
|
||||
* @brief 电机休眠
|
||||
*/
|
||||
void motor_sleep(void) {
|
||||
gpio_bit_reset(GPIOB, GPIO_PIN_12);
|
||||
// 同时禁用电机驱动器
|
||||
gpio_bit_reset(MOTOR_EN_GPIO_PORT, MOTOR_EN_GPIO_PIN);
|
||||
drv_enable(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机启动
|
||||
*/
|
||||
void motor_resume(void) {
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_12);
|
||||
// 同时启用电机驱动器
|
||||
gpio_bit_set(MOTOR_EN_GPIO_PORT, MOTOR_EN_GPIO_PIN);
|
||||
drv_enable(1);
|
||||
}
|
||||
@@ -7,6 +7,10 @@
|
||||
#ifndef _MOTOR_H
|
||||
#define _MOTOR_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#include "gd32c10x_eval.h"
|
||||
|
||||
// 定义电机驱动使能引脚
|
||||
/** @brief MOTOR_EN_GPIO_PORT 宏定义 */
|
||||
#define MOTOR_EN_GPIO_PORT GPIOC
|
||||
@@ -36,4 +40,37 @@ void motor_resume(void);
|
||||
*/
|
||||
void motor_sleep(void);
|
||||
|
||||
/**
|
||||
* @brief 门驱动芯片引脚预初始化（全部置安全态：使能拉低、CS 释放）
|
||||
* @note 必须在任何可能拉高使能脚的代码之前调用
|
||||
* @return void
|
||||
*/
|
||||
void gate_driver_preinit(void);
|
||||
/**
|
||||
* @brief 门驱动芯片上电配置
|
||||
* 新板(DRV8323RS)：拉高使能 → SPI 写 3x PWM 模式/CSA 增益/VDS
|
||||
* 保护/死区/IDRIVE 并逐条回读校验，最后恢复角度传感器 SPI 模式；
|
||||
* 老板(DRV8313)：无 SPI，保持原上电序列（无操作）。
|
||||
* @return u8 0 成功；非 0 为配置/回读校验失败的寄存器位图
|
||||
* @note 调用时机：TIMER1/PWM 已配置且占空比为 0 之后（此时使能芯片，
|
||||
* 半桥只会进入"下管常开"的静止安全态）
|
||||
*/
|
||||
u8 gate_driver_bringup(void);
|
||||
/**
|
||||
* @brief 门驱动运行时使能/睡眠（分板封装 PB12，老板同时控制 PC13）
|
||||
* @param[in] en 1=使能 0=睡眠
|
||||
* @return void
|
||||
*/
|
||||
void drv_enable(u8 en);
|
||||
/**
|
||||
* @brief 读取 nFAULT 引脚状态
|
||||
* @return u8 1=故障有效（低电平） 0=正常
|
||||
*/
|
||||
u8 drv_nfault_active(void);
|
||||
/**
|
||||
* @brief 读取门驱动故障状态寄存器（仅 DRV8323 有效）
|
||||
* @return u16 [9:0]=Fault Status1(0x00) [19:10]=VGS Status2(0x01)；老板返回 0
|
||||
*/
|
||||
u32 drv_fault_status(void);
|
||||
|
||||
#endif
|
||||
@@ -15,6 +15,7 @@
|
||||
#include "flash.h"
|
||||
#include "health.h"
|
||||
#include "initial.h"
|
||||
#include "motor.h"
|
||||
#include "systick.h"
|
||||
|
||||
// MAIN 调试宏定义
|
||||
@@ -81,6 +82,7 @@ u8 hardware_init(void) {
|
||||
__enable_irq();
|
||||
systick_config(); // 系统时钟初始化
|
||||
unused_pin_init(); // 未使用引脚初始化
|
||||
gate_driver_preinit(); // 门驱动使能脚预置安全态（新板：DRVSLP 低=芯片睡眠）
|
||||
USART0_init(); // 调试串口初始化
|
||||
User_timer_init(); // 任务 Timer 初始化
|
||||
|
||||
@@ -97,6 +99,11 @@ u8 hardware_init(void) {
|
||||
init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
|
||||
init_status += Vel_kalman.init(0.0f); // 速度滤波初始化
|
||||
init_status += FOC.init(); // FOC 初始化
|
||||
|
||||
// 门驱动上电配置放在最后：此时 TIMER1/PWM 已启动且占空比为 0，
|
||||
// 新板(DRV8323)在此拉高 DRVSLP 唤醒芯片并经 SPI 配置 3x PWM/增益/保护，
|
||||
// 随后即使任务立即输出 0 占空比，桥臂也处于"下管常开"的静止安全态
|
||||
init_status += gate_driver_bringup();
|
||||
return init_status;
|
||||
}
|
||||
|
||||
|
||||
@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#ifndef MAIN_H
|
||||
#define MAIN_H
|
||||
|
||||
#include "config.h"
|
||||
#include <cstdint>
|
||||
|
||||
/**
|
||||
|
||||
+105
-45
@@ -9,6 +9,18 @@
|
||||
#include "gd32c10x_eval.h"
|
||||
#include <cstdint>
|
||||
|
||||
// 项目级定宽类型别名（u8/u16/u32 等）：整个工程统一从这里取，
|
||||
// 重复 typedef 同一类型在 C++ 中合法，包含多次无副作用
|
||||
typedef uint8_t u8;
|
||||
typedef uint16_t u16;
|
||||
typedef uint32_t u32;
|
||||
typedef int8_t s8;
|
||||
typedef int16_t s16;
|
||||
typedef int32_t s32;
|
||||
typedef volatile uint8_t vu8;
|
||||
typedef volatile uint16_t vu16;
|
||||
typedef volatile uint32_t vu32;
|
||||
|
||||
// 如果使用 CMSIS DSP 库，需要定义 ARM_MATH_CM4
|
||||
#define ARM_MATH_CM4
|
||||
|
||||
@@ -20,27 +32,81 @@
|
||||
#define MAIN_DEBUG 0
|
||||
#define FOC_DEBUG 0
|
||||
|
||||
// 版本号定义 - 可根据实际项目进行调整
|
||||
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
|
||||
constexpr u16 SOFTWARE_VERSION_MINOR = 8;
|
||||
constexpr u16 SOFTWARE_VERSION_PATCH = 16;
|
||||
constexpr u16 SOFTWARE_VERSION_BUILD = 7;
|
||||
// ===================== 版本号定义 =====================
|
||||
// 软件版本 = 编译日期自动生成（年.月.日）+ 当天构建小版本号（手动递增）
|
||||
// __DATE__ 形如 "Mmm dd yyyy"（如 "Aug 29 2026"）：2026-08-29 当天第一次
|
||||
// 构建得到 26.8.29.1；同一天多次出固件时把 SOFTWARE_VERSION_BUILD +1
|
||||
#define CFG_VER_YEAR \
|
||||
((__DATE__[7] - '0') * 1000 + (__DATE__[8] - '0') * 100 + \
|
||||
(__DATE__[9] - '0') * 10 + (__DATE__[10] - '0'))
|
||||
#define CFG_VER_IS_JAN (__DATE__[0] == 'J' && __DATE__[1] == 'a' && __DATE__[2] == 'n')
|
||||
#define CFG_VER_IS_FEB (__DATE__[0] == 'F' && __DATE__[1] == 'e' && __DATE__[2] == 'b')
|
||||
#define CFG_VER_IS_MAR (__DATE__[0] == 'M' && __DATE__[1] == 'a' && __DATE__[2] == 'r')
|
||||
#define CFG_VER_IS_APR (__DATE__[0] == 'A' && __DATE__[1] == 'p' && __DATE__[2] == 'r')
|
||||
#define CFG_VER_IS_MAY (__DATE__[0] == 'M' && __DATE__[1] == 'a' && __DATE__[2] == 'y')
|
||||
#define CFG_VER_IS_JUN (__DATE__[0] == 'J' && __DATE__[1] == 'u' && __DATE__[2] == 'n')
|
||||
#define CFG_VER_IS_JUL (__DATE__[0] == 'J' && __DATE__[1] == 'u' && __DATE__[2] == 'l')
|
||||
#define CFG_VER_IS_AUG (__DATE__[0] == 'A' && __DATE__[1] == 'u' && __DATE__[2] == 'g')
|
||||
#define CFG_VER_IS_SEP (__DATE__[0] == 'S' && __DATE__[1] == 'e' && __DATE__[2] == 'p')
|
||||
#define CFG_VER_IS_OCT (__DATE__[0] == 'O' && __DATE__[1] == 'c' && __DATE__[2] == 't')
|
||||
#define CFG_VER_IS_NOV (__DATE__[0] == 'N' && __DATE__[1] == 'o' && __DATE__[2] == 'v')
|
||||
#define CFG_VER_IS_DEC (__DATE__[0] == 'D' && __DATE__[1] == 'e' && __DATE__[2] == 'c')
|
||||
#define CFG_VER_MONTH \
|
||||
(CFG_VER_IS_JAN ? 1 \
|
||||
: CFG_VER_IS_FEB ? 2 \
|
||||
: CFG_VER_IS_MAR ? 3 \
|
||||
: CFG_VER_IS_APR ? 4 \
|
||||
: CFG_VER_IS_MAY ? 5 \
|
||||
: CFG_VER_IS_JUN ? 6 \
|
||||
: CFG_VER_IS_JUL ? 7 \
|
||||
: CFG_VER_IS_AUG ? 8 \
|
||||
: CFG_VER_IS_SEP ? 9 \
|
||||
: CFG_VER_IS_OCT ? 10 \
|
||||
: CFG_VER_IS_NOV ? 11 \
|
||||
: CFG_VER_IS_DEC ? 12 \
|
||||
: 0)
|
||||
/* 日字段两位，个位数时首字符为空格（如 "Aug 5 2026"） */
|
||||
#define CFG_VER_DAY \
|
||||
((__DATE__[4] == ' ') ? (__DATE__[5] - '0') \
|
||||
: ((__DATE__[4] - '0') * 10 + (__DATE__[5] - '0')))
|
||||
|
||||
constexpr u16 SOFTWARE_VERSION_MAJOR = CFG_VER_YEAR % 100; // 年后两位，如 26
|
||||
constexpr u16 SOFTWARE_VERSION_MINOR = CFG_VER_MONTH; // 月
|
||||
constexpr u16 SOFTWARE_VERSION_PATCH = CFG_VER_DAY; // 日
|
||||
constexpr u16 SOFTWARE_VERSION_BUILD = 1; // 当天第几次构建，手动递增
|
||||
|
||||
// 硬件版本：全手写，仅在 PCB 改版时修改（编译 APP 不影响）
|
||||
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
|
||||
constexpr u16 HARDWARE_VERSION_MINOR = 12;
|
||||
constexpr u16 HARDWARE_VERSION_PATCH = 01;
|
||||
constexpr u16 HARDWARE_VERSION_PATCH = 1;
|
||||
constexpr u16 HARDWARE_VERSION_BUILD = 0;
|
||||
|
||||
// BOOT 版本：全手写，仅在 bootloader 更新时修改（APP 编译不影响）
|
||||
constexpr u16 BOOT_VERSION_MAJOR = 26;
|
||||
constexpr u16 BOOT_VERSION_MINOR = 1;
|
||||
constexpr u16 BOOT_VERSION_PATCH = 12;
|
||||
constexpr u16 BOOT_VERSION_BUILD = 0;
|
||||
|
||||
// ===================== 功率级门驱动芯片选择 =====================
|
||||
// 1 = 新板 DRV8323RS（外置 WSD6035DN33）：使能前经 SPI 配置 3x PWM 模式、
|
||||
// CSA 增益、VDS 过流门限；3 路 PWM 接 INHA/B/C（CH2/CH1/CH0，
|
||||
// 固件内已做通道重映射）；PB12(DRVSLP) 同时是使能脚与 INLA/B/C 电平
|
||||
// （高=正常运转，低=三相 Hi-Z 睡眠滑行）。
|
||||
// 0 = 老板 DRV8313（集成 FET 三半桥）：无 SPI，保持原使能时序与相序。
|
||||
#define GATE_DRIVER_DRV8323 0
|
||||
|
||||
#if GATE_DRIVER_DRV8323
|
||||
// DRV8323 片上 CSA 增益（V/V），经 SPI 写入 0x06[7:6]，仅 5/10/20/40 有效。
|
||||
// 双极性量程 = ±1.655V/(G×Rshunt)：G=10、R=10mΩ 时约 ±16.5A。
|
||||
// 修改后电流换算系数 CURRENT_FACTOR（ADC.cpp）自动跟随，但软件限流标定需复核。
|
||||
constexpr float DRV8323_CSA_GAIN_VV = 10.0f;
|
||||
constexpr float DRV8323_SHUNT_OHM = 0.010f;
|
||||
#endif
|
||||
// 编译时间（AC6 编译器自动生成）
|
||||
#define APP_BUILD_DATE __DATE__
|
||||
#define APP_BUILD_TIME __TIME__
|
||||
|
||||
// Flash 分区配置
|
||||
constexpr u32 BOOTLOADER_SIZE = 0x6000; // 24KB
|
||||
constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT 起始地址
|
||||
constexpr u32 APPLATEST_START_ADDR = 0x08012000; // APP 起始地址
|
||||
constexpr u32 APPBACK_START_ADDR = 0x08006000; // APPBACK 起始地址
|
||||
@@ -50,33 +116,33 @@ constexpr u32 SLEEP_CONFIG_START = 0x0801E400; // 睡眠配置起始地址
|
||||
constexpr u32 SLEEP_CONFIG_END = 0x0801E800; // 睡眠配置结束地址
|
||||
constexpr u32 FLASH_EMPTY = 0xFFFFFFFF; // 空数据
|
||||
constexpr u32 SLEEP_ENABLE = 0x11111111; // 睡眠使能
|
||||
constexpr u32 SLEEP_DISABLE = 0x00000000; // 睡眠禁止
|
||||
|
||||
// 节点 ID 配置 (用于多电机 CAN 总线隔离)
|
||||
// 使用 CAN ID 高 16 位 (bits 31-16) 存储节点 ID，低 16 位 (bits 15-0) 存储基础 ID
|
||||
// 格式：[节点 ID(16 位)][基础 ID(16 位)]，例如：node_id=1, base_id=0x0200 → 0x00010200
|
||||
//
|
||||
// 节点 ID 存储方案：
|
||||
// - 使用独立 Flash 页 (NODE_ID_PAGE_ADDR) 存储节点 ID，与EEPROM完全隔离
|
||||
// - 采用双重备份机制 (主备份 + 备用备份)
|
||||
// - 使用独立 Flash 页存储节点 ID，与EEPROM完全隔离
|
||||
// - 采用双重备份机制：主备份与备用备份分属两个独立 1KB 页，
|
||||
// 擦写其一不影响另一份，掉电时至少保留一份有效数据
|
||||
// - 写入采用乒乓方式：总是写入"非最新"的那一页，另一页全程保有旧值
|
||||
// - 使用魔数 (NODE_ID_MAGIC) 验证数据有效性，防止位翻转导致错读
|
||||
// - 使用写入计数器区分最新数据
|
||||
constexpr u32 NODE_ID_PAGE_ADDR = 0x0801E800; // 节点 ID 存储页 (1KB，独立区域)
|
||||
constexpr u32 NODE_ID_PRIMARY_ADDR = 0x0801E800; // 主存储地址
|
||||
constexpr u32 NODE_ID_BACKUP_ADDR = 0x0801E840; // 备份存储地址 (偏移64字节)
|
||||
// - 使用写入计数器区分最新数据（计数器跳过 0，比较用有符号差值容忍回绕）
|
||||
constexpr u32 NODE_ID_PAGE_ADDR = 0x0801E800; // 节点 ID 主存储页 (1KB，独立区域)
|
||||
constexpr u32 NODE_ID_PRIMARY_ADDR = 0x0801E800; // 主备份地址
|
||||
constexpr u32 NODE_ID_BACKUP_ADDR = 0x0801EC00; // 备用备份地址（独立 1KB 页）
|
||||
constexpr u32 NODE_ID_MAGIC = 0x4E4F4445; // 魔数 'NODE' 用于验证数据有效性
|
||||
constexpr u16 DEFAULT_NODE_ID = 1; // 默认节点 ID
|
||||
constexpr u16 BROADCAST_NODE_ID = 0x1FFF; // 广播 ID (所有节点响应)
|
||||
constexpr u16 MAX_NODE_ID = 0x1FFE; // 最大有效节点 ID (0~0x1FFE)
|
||||
constexpr u16 SETTABLE_NODE_ID_MAX = 0x00FF; // SET_NODE_ID 命令允许设置的上限 (1~0xFF)
|
||||
|
||||
// 节点 ID 存储结构 (存储在Flash中的格式)
|
||||
// 节点 ID 存储结构 (主备份/备用备份各一份，每份 16 字节，位于各自页首)
|
||||
// 地址偏移 0x00: 魔数 (0x4E4F4445 = 'NODE')
|
||||
// 地址偏移 0x04: 节点 ID (u16)
|
||||
// 地址偏移 0x06: 保留 (u16，填充为0xFFFF)
|
||||
// 地址偏移 0x08: 写入计数器 (u32，用于区分最新数据)
|
||||
// 地址偏移 0x0C: CRC32 校验 (u32)
|
||||
constexpr u8 NODE_ID_STRUCT_SIZE = 0x10; // 结构体大小 16 字节
|
||||
constexpr u8 NODE_ID_MAGIC_OFFSET = 0x00; // 魔数偏移
|
||||
constexpr u8 NODE_ID_VALUE_OFFSET = 0x04; // 节点ID值偏移
|
||||
constexpr u8 NODE_ID_COUNTER_OFFSET = 0x08; // 写入计数器偏移
|
||||
@@ -136,14 +202,24 @@ constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪
|
||||
// PID 配置
|
||||
constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流 P
|
||||
constexpr float current_loop_Ki = 0.55f; // 电流 P
|
||||
constexpr float current_loop_Kd = 3e-4f; // 电流 P
|
||||
// 电流反馈噪声大（CurrentKalman 稳态增益≈1，几乎无滤波），D 项只放大噪声，置零
|
||||
constexpr float current_loop_Kd = 0.0f;
|
||||
|
||||
constexpr float DEFAULT_vel_loop_p = 0.005f / 100 * 12; // 速度 P
|
||||
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
|
||||
constexpr float vel_loop_Tu = 17.0f * 1e-3f;
|
||||
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
|
||||
// 速度 D 恢复（提供加速度阻尼）；现走"测量微分+1ms低通"路径，无目标阶跃踢、
|
||||
// 噪声被低通抑制，比旧的裸误差微分安全
|
||||
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
|
||||
|
||||
// PID 抗饱和与微分滤波（负载稳定性优化，2026-08）
|
||||
// 抗饱和主体为条件积分（饱和方向冻结，见 pid.cpp）；此增益仅用于
|
||||
// bleed_integral() 外部饱和泄放（前馈叠加超限时），且带同号保护不过零
|
||||
constexpr float ANTI_WINDUP_GAIN = 0.3f; // 外部饱和泄放比例
|
||||
constexpr float DERIV_LPF_TAU = 0.001f; // 微分项一阶低通时间常数 (s)
|
||||
constexpr float MIN_LIMIT_VOLTAGE = 0.1f; // P 增益换算分母下限，防 limit_voltage=0 除零
|
||||
|
||||
// mode2 角度同步 Uq 前馈补偿：固定 Uq 下的真实速度波动不应从反馈中滤掉，
|
||||
// 而应按物理 360° 提前补偿负载/阻力差异，降低速度 PID 的周期性纠偏量。
|
||||
constexpr bool MODE2_UQ_FF_ENABLE = true;
|
||||
@@ -155,6 +231,7 @@ constexpr float MODE2_UQ_FF_GAIN = 0.003f; // 速度误差(deg/s)到前馈
|
||||
constexpr float MODE2_UQ_FF_MAX = 3.5f; // 前馈补偿电压限幅(V)
|
||||
constexpr float MODE2_UQ_FF_STOP_SPEED = 2.0f; // 目标/实际速度低于该值时不输出前馈
|
||||
constexpr float MODE2_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error
|
||||
constexpr float MODE2_UQ_FF_LEARN_ERR_MAX = 375.0f; // 速度误差带内才学习(≈15°/s×25)，防启停瞬态污染表
|
||||
|
||||
// mode3 角度同步 Uq 前馈补偿：单独建表，避免和 mode2 互相污染。
|
||||
constexpr bool MODE3_UQ_FF_ENABLE = true;
|
||||
@@ -166,16 +243,21 @@ constexpr float MODE3_UQ_FF_GAIN = 0.3f; // mode3 速度误差未乘减
|
||||
constexpr float MODE3_UQ_FF_MAX = 3.5f;
|
||||
constexpr float MODE3_UQ_FF_STOP_SPEED = 2.0f; // mode3 内部目标/实际速度低于该值时不输出前馈
|
||||
constexpr float MODE3_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error
|
||||
constexpr float MODE3_UQ_FF_LEARN_ERR_MAX = 15.0f; // 速度误差带内(°/s)才学习，防启停瞬态污染表
|
||||
|
||||
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度 - 角度 P
|
||||
constexpr float vel_angloop_Ki = 5.0f; // 速度 速度 - 角度 I
|
||||
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度 - 角度 D
|
||||
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度 - 角度 D（恢复，走测量微分+低通）
|
||||
|
||||
// mode3 低速/静止附近速度 P 降低，减少速度反馈噪声、齿槽和摩擦导致的来回拉扯。
|
||||
// mode3 低速/静止附近速度内环增益调度（D 保持满值）：
|
||||
// P/I 各自独立地板——I 地板更低，停止瞬间积分充电速率仅为全速的 15%，
|
||||
// 与静摩擦相互作用（到位 hunting：停下瞬间小幅来回抖动）的能量被大幅压缩；
|
||||
// P 地板保持足够刚度，减小突破静摩擦所需的误差。
|
||||
constexpr bool MODE3_LOW_SPEED_P_SCALE_ENABLE = true;
|
||||
constexpr float MODE3_LOW_SPEED_P_MIN_SCALE = 0.2f;
|
||||
constexpr float MODE3_LOW_SPEED_P_MIN_SCALE = 0.5f;
|
||||
constexpr float MODE3_LOW_SPEED_I_MIN_SCALE = 0.15f;
|
||||
constexpr float MODE3_LOW_SPEED_P_LOW = 1.0f;
|
||||
constexpr float MODE3_LOW_SPEED_P_FULL = 5.0f;
|
||||
constexpr float MODE3_LOW_SPEED_P_FULL = 8.0f;
|
||||
|
||||
constexpr float DEFAULT_angle_angloop_p =
|
||||
150.0f / 100 * 12; // 角度 速度 - 角度 P
|
||||
@@ -190,41 +272,26 @@ constexpr float angle_MIT_Kd = 8e-04f;
|
||||
|
||||
constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
|
||||
constexpr float vel_MIT_Ki = 15.0f;
|
||||
constexpr float vel_MIT_Kd = 1e-04f;
|
||||
constexpr float vel_MIT_Kd = 1e-04f; // 速度 MIT D（恢复，走测量微分+低通）
|
||||
|
||||
constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
|
||||
constexpr float current_MIT_Ki = 0.1f;
|
||||
constexpr float current_MIT_Kd = 3e-4f;
|
||||
constexpr float current_MIT_Kd = 0.0f; // 电流 MIT D（置零，同 current_loop_Kd）
|
||||
// 常量定义
|
||||
constexpr u8 SIZE_FLOAT = sizeof(float);
|
||||
constexpr u8 SIZE_U32 = sizeof(uint32_t);
|
||||
constexpr float ADC_MAX = 65536.0f;
|
||||
constexpr float PI = 3.14159265358979323846f;
|
||||
constexpr float INV_PI_3 = 3.0f / PI;
|
||||
constexpr float PI_3 = PI / 3.0f;
|
||||
constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
|
||||
constexpr float _3PI_2 = 4.71238898038f;
|
||||
constexpr float SQRT3 = 1.7320508075688772f;
|
||||
constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
|
||||
constexpr float COS_PI_3 = 0.5f;
|
||||
constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
|
||||
constexpr float DEG2RAD = 1.0f / 180.0f * PI;
|
||||
constexpr float RAD2DEG = 180.0f / PI; // 修正 RAD2DEG 定义，应该是 180/π
|
||||
constexpr u8 DEFAULT_VALUE = 0;
|
||||
constexpr u8 CONFIG_SAVED_NUM = 6;
|
||||
|
||||
// 三角函数快速计算 LUT
|
||||
constexpr u16 LUT_SIZE = 1024;
|
||||
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
|
||||
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
|
||||
constexpr u16 DEV_CAN0_ID = 0x0000;
|
||||
constexpr u16 DEV_CAN0_MASK = 0x0000;
|
||||
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
|
||||
// 反馈基础 ID
|
||||
constexpr u16 CANFD_STATUS_ID = 0x0100; // 状态反馈基础 ID
|
||||
constexpr u16 CANFD_CONFIG_ID = 0x0101; // 配置信息基础 ID
|
||||
@@ -238,14 +305,10 @@ constexpr u16 CANFD_GET_NODE_ID = 0x0401; // 查询节点 ID 基础 ID
|
||||
|
||||
// OTA 相关基础 ID 不受到node ID影响
|
||||
constexpr u16 CANFD_BOOT_ID = 0x0000; // BOOT 模式基础 ID (进入 BOOT 使用此 ID)
|
||||
constexpr u16 CANFD_OTA_REQ_ID = 0x0100; // OTA 请求基础 ID
|
||||
constexpr u16 CANFD_OTA_DATA_ID = 0x0101; // OTA 数据基础 ID
|
||||
constexpr u16 CANFD_OTA_ACK_ID = 0x0102; // OTA 确认基础 ID
|
||||
constexpr u16 CANFD_OTA_WAIT_ID = 0x0103; // OTA 等待基础 ID
|
||||
|
||||
// ADC
|
||||
constexpr u8 ADC_FILTER_SIZE = 6;
|
||||
constexpr u8 BLOCK_SIZE = ADC_FILTER_SIZE * SIZE_FLOAT;
|
||||
constexpr float ERROR_ADC_VAL = 255.0f;
|
||||
constexpr float MAX_TEMP = 150.0f;
|
||||
constexpr float MIN_TEMP = -40.0f;
|
||||
@@ -255,12 +318,10 @@ constexpr float MAX_VCC = 65.0f;
|
||||
constexpr float MIN_VCC = -0.1f;
|
||||
|
||||
// ang_sensor
|
||||
constexpr u16 VEL_FILTER_SIZE = 4;
|
||||
constexpr u16 DEFAULT_COMMAND = 0x0000;
|
||||
constexpr u16 FAULT_RECEIVE = 0xFFFF;
|
||||
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
|
||||
constexpr float ANGLE_WRAP_VALUE = 360.0f;
|
||||
constexpr float RAD_FACTOR = 1.0f / ADC_MAX * 2.0f * PI;
|
||||
constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
|
||||
|
||||
// TIMER2-任务调度
|
||||
@@ -269,7 +330,6 @@ constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us
|
||||
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; // 频率
|
||||
constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
|
||||
|
||||
// TIMER1-PWM
|
||||
|
||||
+140
-68
@@ -491,17 +491,6 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
|
||||
return FLASH_EEPROM_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取角度数据从 Flash
|
||||
* @return float: 角度数据
|
||||
*/
|
||||
float FlashEEPROM::read_angle() {
|
||||
float angle = DEFAULT_VALUE;
|
||||
if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) {
|
||||
angle = DEFAULT_VALUE;
|
||||
}
|
||||
return angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 保存电角度零位校准角到 Flash
|
||||
@@ -530,6 +519,35 @@ static u32 calculate_node_id_crc(u16 node_id, u32 counter) {
|
||||
return crc_block_data_calculate(crc_data, 2);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief 比较写入计数器新旧（有符号差值比较，容忍 u32 回绕）
|
||||
* @param a: 计数器 a
|
||||
* @param b: 计数器 b
|
||||
* @retval bool: a 比 b 新则为 true
|
||||
* @note 计数器写入时跳过 0，只要两次写入间隔不超过 2^31 次即可正确比较
|
||||
*/
|
||||
static bool node_id_counter_newer(u32 a, u32 b) {
|
||||
return (int32_t)(a - b) > 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief 擦除单个 1KB Flash 页
|
||||
* @param page_addr: 页起始地址（1KB 对齐）
|
||||
* @retval FlashEepromStatus: 操作状态
|
||||
*/
|
||||
static FlashEepromStatus erase_flash_page(u32 page_addr) {
|
||||
fmc_unlock();
|
||||
fmc_state_enum fmc_status = fmc_page_erase(page_addr);
|
||||
if (fmc_status != FMC_READY) {
|
||||
fmc_lock();
|
||||
EEP_DEBUG_PRINT("Page erase failed at 0x%08X, status=%d\r\n",
|
||||
page_addr, fmc_status);
|
||||
return FLASH_EEPROM_ERASE_ERROR;
|
||||
}
|
||||
fmc_lock();
|
||||
return FLASH_EEPROM_OK;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief 从指定地址读取节点 ID 数据结构
|
||||
* @param addr: Flash 地址
|
||||
@@ -618,60 +636,87 @@ static FlashEepromStatus write_node_id_struct(u32 addr, u16 node_id, u32 counter
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief 擦除节点 ID 存储页
|
||||
* @brief 擦除节点 ID 存储区（主备份页 + 备用备份页，恢复默认值）
|
||||
* @retval FlashEepromStatus: 操作状态
|
||||
* @note 仅用于恢复出厂/调试场景，会同时清除两份备份
|
||||
*/
|
||||
FlashEepromStatus FlashEEPROM::eraseNodeId() {
|
||||
fmc_state_enum fmc_status;
|
||||
EEP_DEBUG_PRINT("Erasing Node ID pages at 0x%08X / 0x%08X\r\n",
|
||||
NODE_ID_PRIMARY_ADDR, NODE_ID_BACKUP_ADDR);
|
||||
|
||||
EEP_DEBUG_PRINT("Erasing Node ID page at 0x%08X\r\n", NODE_ID_PAGE_ADDR);
|
||||
|
||||
fmc_unlock();
|
||||
fmc_status = fmc_page_erase(NODE_ID_PAGE_ADDR);
|
||||
if (fmc_status != FMC_READY) {
|
||||
fmc_lock();
|
||||
EEP_DEBUG_PRINT("Node ID page erase failed, status=%d\r\n", fmc_status);
|
||||
return FLASH_EEPROM_ERASE_ERROR;
|
||||
FlashEepromStatus status = erase_flash_page(NODE_ID_PRIMARY_ADDR);
|
||||
if (status != FLASH_EEPROM_OK) {
|
||||
return status;
|
||||
}
|
||||
fmc_lock();
|
||||
|
||||
return FLASH_EEPROM_OK;
|
||||
return erase_flash_page(NODE_ID_BACKUP_ADDR);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief 从 Flash 读取节点 ID
|
||||
* @brief 从 Flash 读取节点 ID（双重备份回退机制）
|
||||
* @param node_id: 输出节点 ID 值
|
||||
* @retval FlashEepromStatus: 操作状态
|
||||
* @note 读取主备份与备用备份两份记录：
|
||||
* - 两份均有效：取写入计数器更新的一份
|
||||
* - 仅一份有效：回退使用该份（另一份损坏或未写入）
|
||||
* - 均无效：返回 DATA_NOT_FOUND，由调用方使用默认值
|
||||
*/
|
||||
FlashEepromStatus FlashEEPROM::readNodeId(u16 *node_id) const {
|
||||
if (node_id == nullptr) {
|
||||
return FLASH_EEPROM_INVALID_ADDR;
|
||||
}
|
||||
|
||||
u16 id_value = 0;
|
||||
u32 counter = 0;
|
||||
u16 id_primary = 0, id_backup = 0;
|
||||
u32 counter_primary = 0, counter_backup = 0;
|
||||
|
||||
// 从主存储区读取
|
||||
FlashEepromStatus status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &id_value, &counter);
|
||||
FlashEepromStatus status_primary = read_node_id_struct(
|
||||
NODE_ID_PRIMARY_ADDR, &id_primary, &counter_primary);
|
||||
FlashEepromStatus status_backup = read_node_id_struct(
|
||||
NODE_ID_BACKUP_ADDR, &id_backup, &counter_backup);
|
||||
|
||||
if (status == FLASH_EEPROM_OK) {
|
||||
*node_id = id_value;
|
||||
EEP_DEBUG_PRINT("Node ID read from primary (counter=%u): %d\r\n", counter, *node_id);
|
||||
return FLASH_EEPROM_OK;
|
||||
if (status_primary == FLASH_EEPROM_OK &&
|
||||
status_backup == FLASH_EEPROM_OK) {
|
||||
// 双备份均有效：取计数器更新的一份（计数器相同时优先主备份）
|
||||
if (node_id_counter_newer(counter_backup, counter_primary)) {
|
||||
*node_id = id_backup;
|
||||
EEP_DEBUG_PRINT("Node ID read from backup (counter=%u): %d\r\n",
|
||||
counter_backup, *node_id);
|
||||
} else {
|
||||
EEP_DEBUG_PRINT("Node ID read failed from primary: status=%d\r\n", status);
|
||||
*node_id = id_primary;
|
||||
EEP_DEBUG_PRINT("Node ID read from primary (counter=%u): %d\r\n",
|
||||
counter_primary, *node_id);
|
||||
}
|
||||
} else if (status_primary == FLASH_EEPROM_OK) {
|
||||
// 主备份有效，备用备份损坏/为空：回退主备份
|
||||
*node_id = id_primary;
|
||||
EEP_DEBUG_PRINT(
|
||||
"Node ID fallback to primary (backup status=%d): %d\r\n",
|
||||
status_backup, *node_id);
|
||||
} else if (status_backup == FLASH_EEPROM_OK) {
|
||||
// 主备份损坏/为空，备用备份有效：回退备用备份
|
||||
*node_id = id_backup;
|
||||
EEP_DEBUG_PRINT(
|
||||
"Node ID fallback to backup (primary status=%d): %d\r\n",
|
||||
status_primary, *node_id);
|
||||
} else {
|
||||
EEP_DEBUG_PRINT("Node ID not found in both slots (primary=%d, backup=%d)\r\n",
|
||||
status_primary, status_backup);
|
||||
return FLASH_EEPROM_DATA_NOT_FOUND;
|
||||
}
|
||||
return FLASH_EEPROM_OK;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief 保存节点 ID 到 Flash（每次写入前先擦除整个页面，写入后校验）
|
||||
* @brief 保存节点 ID 到 Flash（乒乓式双重备份，写入后校验）
|
||||
* @param node_id: 要保存的节点 ID 值
|
||||
* @retval FlashEepromStatus: 操作状态
|
||||
* @note 由于Flash只能将1写为0，不能将0写为1，因此每次写入新数据前
|
||||
* 需要先擦除整个页面。简化方案：不再使用备用备份区，
|
||||
* 每次写入前擦除页面，然后写入主存储区。
|
||||
* 写入后立即校验数据，如果不对则再次写入，最多3次。
|
||||
* @note 主备份与备用备份分属两个独立 1KB Flash 页。写入流程：
|
||||
* 1. 读取两份备份，定位计数器最新的有效记录作为基准
|
||||
* 2. 选择"非最新"的一页作为写入目标（乒乓交替）
|
||||
* 3. 只擦除并写入目标页，另一页全程保有旧的有效记录，
|
||||
* 掉电时至少一份数据存活
|
||||
* 4. 写入后立即校验，失败则重试，最多3次
|
||||
* 某一页损坏时无需专门修复：下一次写入目标即切换到该页，
|
||||
* 擦除+重写天然完成自愈。
|
||||
*/
|
||||
FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
|
||||
if (node_id > MAX_NODE_ID) {
|
||||
@@ -679,34 +724,59 @@ FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
|
||||
return FLASH_EEPROM_INVALID_ADDR;
|
||||
}
|
||||
|
||||
// 获取当前写入计数器（从主存储区读取）
|
||||
u16 current_id = 0;
|
||||
u32 current_counter = 0;
|
||||
// 读取两份备份，确定当前最新有效记录（计数器基准 + 乒乓目标选择）
|
||||
u16 id_primary = 0, id_backup = 0;
|
||||
u32 counter_primary = 0, counter_backup = 0;
|
||||
|
||||
FlashEepromStatus primary_status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, ¤t_id, ¤t_counter);
|
||||
FlashEepromStatus status_primary = read_node_id_struct(
|
||||
NODE_ID_PRIMARY_ADDR, &id_primary, &counter_primary);
|
||||
FlashEepromStatus status_backup = read_node_id_struct(
|
||||
NODE_ID_BACKUP_ADDR, &id_backup, &counter_backup);
|
||||
|
||||
// 计数器 +1
|
||||
u32 new_counter = current_counter + 1;
|
||||
// 如果计数器溢出（0xFFFFFFFF + 1 = 0），则使用1
|
||||
bool primary_latest = false;
|
||||
bool backup_latest = false;
|
||||
if (status_primary == FLASH_EEPROM_OK &&
|
||||
status_backup == FLASH_EEPROM_OK) {
|
||||
// 两份均有效：计数器新的为最新（相同时视为主备份最新）
|
||||
if (node_id_counter_newer(counter_backup, counter_primary)) {
|
||||
backup_latest = true;
|
||||
} else {
|
||||
primary_latest = true;
|
||||
}
|
||||
} else if (status_primary == FLASH_EEPROM_OK) {
|
||||
primary_latest = true;
|
||||
} else if (status_backup == FLASH_EEPROM_OK) {
|
||||
backup_latest = true;
|
||||
}
|
||||
|
||||
// 计数器 +1（跳过 0；两份均无效时从 1 开始）
|
||||
u32 new_counter = 1;
|
||||
if (primary_latest) {
|
||||
new_counter = counter_primary + 1;
|
||||
} else if (backup_latest) {
|
||||
new_counter = counter_backup + 1;
|
||||
}
|
||||
if (new_counter == 0) {
|
||||
new_counter = 1;
|
||||
}
|
||||
|
||||
// 先擦除整个页面
|
||||
EEP_DEBUG_PRINT("Erasing Node ID page before write\r\n");
|
||||
FlashEepromStatus status = eraseNodeId();
|
||||
if (status != FLASH_EEPROM_OK) {
|
||||
return status;
|
||||
}
|
||||
// 乒乓写入：目标为"非最新有效"的一页，最新有效的一页保持不动
|
||||
u32 target_addr = primary_latest ? NODE_ID_BACKUP_ADDR : NODE_ID_PRIMARY_ADDR;
|
||||
|
||||
// 写入并校验，最多3次
|
||||
// 写入并校验，最多3次（重试期间另一页的旧记录始终有效）
|
||||
/** @brief MAX_WRITE_RETRY 宏定义 */
|
||||
#define MAX_WRITE_RETRY 3
|
||||
for (u8 retry = 0; retry < MAX_WRITE_RETRY; retry++) {
|
||||
// 写入主存储区
|
||||
EEP_DEBUG_PRINT("Writing to primary (counter=%u, retry=%d)\r\n", new_counter, retry);
|
||||
FlashEepromStatus write_status = write_node_id_struct(NODE_ID_PRIMARY_ADDR, node_id, new_counter);
|
||||
EEP_DEBUG_PRINT("Writing to 0x%08X (counter=%u, retry=%d)\r\n",
|
||||
target_addr, new_counter, retry);
|
||||
|
||||
FlashEepromStatus status = erase_flash_page(target_addr);
|
||||
if (status != FLASH_EEPROM_OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
FlashEepromStatus write_status =
|
||||
write_node_id_struct(target_addr, node_id, new_counter);
|
||||
if (write_status != FLASH_EEPROM_OK) {
|
||||
EEP_DEBUG_PRINT("Node ID write failed: status=%d\r\n", write_status);
|
||||
continue;
|
||||
@@ -715,20 +785,18 @@ FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
|
||||
// 立即校验数据
|
||||
u16 read_id = 0;
|
||||
u32 read_counter = 0;
|
||||
FlashEepromStatus read_status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &read_id, &read_counter);
|
||||
FlashEepromStatus read_status =
|
||||
read_node_id_struct(target_addr, &read_id, &read_counter);
|
||||
|
||||
if (read_status == FLASH_EEPROM_OK && read_id == node_id && read_counter == new_counter) {
|
||||
EEP_DEBUG_PRINT("Node ID saved and verified successfully: %d (counter=%u)\r\n", node_id, new_counter);
|
||||
if (read_status == FLASH_EEPROM_OK && read_id == node_id &&
|
||||
read_counter == new_counter) {
|
||||
EEP_DEBUG_PRINT("Node ID saved and verified: %d (counter=%u, slot=0x%08X)\r\n",
|
||||
node_id, new_counter, target_addr);
|
||||
return FLASH_EEPROM_OK;
|
||||
} else {
|
||||
EEP_DEBUG_PRINT("Node ID verification failed: read_id=%d (expected=%d), read_counter=%u (expected=%u)\r\n",
|
||||
}
|
||||
EEP_DEBUG_PRINT("Node ID verify failed: read_id=%d (expected=%d), "
|
||||
"read_counter=%u (expected=%u)\r\n",
|
||||
read_id, node_id, read_counter, new_counter);
|
||||
// 校验失败，重新擦除并写入
|
||||
if (retry < MAX_WRITE_RETRY - 1) {
|
||||
EEP_DEBUG_PRINT("Retrying...\r\n");
|
||||
eraseNodeId();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
EEP_DEBUG_PRINT("Node ID save failed after %d retries\r\n", MAX_WRITE_RETRY);
|
||||
@@ -741,7 +809,9 @@ FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
|
||||
* @return FlashEepromStatus: 状态
|
||||
*/
|
||||
FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
|
||||
static u32 next_address;
|
||||
// 每次调用重新扫描可写入位置，不跨调用残留旧地址
|
||||
// （循环至少执行一次，next_address 必被赋值）
|
||||
u32 next_address = EEPROM_START_ADDRESS;
|
||||
// 遍历 Flash 页面寻找下一个可写入 crc 的位置
|
||||
for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32;
|
||||
address > EEPROM_START_ADDRESS; address -= SIZE_U32) {
|
||||
@@ -933,6 +1003,8 @@ void jumpToBOOT() {
|
||||
uint32_t reset_handler = *(__IO uint32_t *)(BOOTLOADER_START_ADDR + 4);
|
||||
typedef void (*AppEntryFunc)(void);
|
||||
AppEntryFunc app_entry = (AppEntryFunc)reset_handler;
|
||||
// 跳转前喂狗，给 BOOT 留出一个完整看门狗周期做早期初始化
|
||||
fwdgt_counter_reload();
|
||||
__set_MSP(msp_value);
|
||||
// 7. 跳转
|
||||
app_entry();
|
||||
|
||||
+6
-10
@@ -7,6 +7,8 @@
|
||||
#ifndef __FLASH_H
|
||||
#define __FLASH_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用
|
||||
#include <cstdint>
|
||||
|
||||
@@ -56,7 +58,6 @@ class FlashEEPROM {
|
||||
static constexpr u8 ADDR_NODE_ID = 0x06; // 节点 ID 存储地址
|
||||
static constexpr u8 ADDR_CALIBRATION_ANGLE = 0x07; // 电角度零位校准角存储地址
|
||||
|
||||
bool angle_saved = false;
|
||||
bool system_config_saved = false;
|
||||
u8 EEPROMStatus;
|
||||
// 构造函数
|
||||
@@ -120,11 +121,6 @@ class FlashEEPROM {
|
||||
* @return u8 返回值
|
||||
*/
|
||||
u8 read_saved_system_config(void);
|
||||
/**
|
||||
* @brief 读取保存的角度值
|
||||
* @return float 返回值
|
||||
*/
|
||||
float read_angle();
|
||||
/**
|
||||
* @brief 保存电角度零位校准角到 Flash
|
||||
* @param[in] angle 校准角（度）
|
||||
@@ -132,22 +128,22 @@ class FlashEEPROM {
|
||||
*/
|
||||
FlashEepromStatus save_calibration_angle(float angle);
|
||||
|
||||
// 节点 ID 独立读写函数（使用魔数+双重备份+计数器机制）
|
||||
// 从 Flash 读取节点 ID（带魔数验证和双重备份回退）
|
||||
// 节点 ID 独立读写函数（主/备双备份分属独立 Flash 页 + 魔数 + 计数器 + CRC）
|
||||
// 从 Flash 读取节点 ID（双备份均有效取计数器最新，单份有效则回退）
|
||||
/**
|
||||
* @brief 读取节点 ID 配置
|
||||
* @param[in] node_id 参数
|
||||
* @return FlashEepromStatus 返回值
|
||||
*/
|
||||
FlashEepromStatus readNodeId(u16 *node_id) const;
|
||||
// 保存节点 ID 到 Flash（写入主备份和备用备份）
|
||||
// 保存节点 ID 到 Flash（乒乓写入非最新页，另一页保有旧值，掉电安全）
|
||||
/**
|
||||
* @brief 写入节点 ID 配置
|
||||
* @param[in] node_id 参数
|
||||
* @return FlashEepromStatus 返回值
|
||||
*/
|
||||
FlashEepromStatus writeNodeId(u16 node_id);
|
||||
// 擦除节点 ID 存储区（恢复默认值）
|
||||
// 擦除节点 ID 双备份存储页（恢复默认值）
|
||||
/**
|
||||
* @brief 擦除节点 ID 配置
|
||||
* @return FlashEepromStatus 返回值
|
||||
|
||||
+65
-19
@@ -10,6 +10,8 @@
|
||||
#include "FOC.h"
|
||||
#include "adc.h"
|
||||
#include "flash.h"
|
||||
#include "motor.h"
|
||||
#include <cmath>
|
||||
|
||||
|
||||
extern sampler sampler;
|
||||
@@ -19,11 +21,10 @@ extern FlashEEPROM flashEEP;
|
||||
// 全局热管理控制器单例：由 main 任务周期调用 update()，用于维护过热保护状态。
|
||||
ThermalController thermalController; // 热量控制器初始化
|
||||
|
||||
/** @brief max 宏定义 */
|
||||
#define max(a, b, c) \
|
||||
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
|
||||
/** @brief PROTECT_TIME 常量定义 */
|
||||
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
|
||||
// 三相电流连续全部失效帧数（×10ms）阈值，达到后按过热关机处理
|
||||
constexpr u16 CURRENT_ERROR_SHUTDOWN_COUNT = 100;
|
||||
|
||||
// 电压管理状态机：仅 power_manage() 慢速任务访问，文件内封装。
|
||||
static VoltageState voltage_state = VoltageState::NORMAL;
|
||||
@@ -34,6 +35,14 @@ static float last_vbus_slow = 0.0f; // 上次电压值（慢速任务用）
|
||||
** @retval 热量值
|
||||
*/
|
||||
u32 ThermalController::update() {
|
||||
#if GATE_DRIVER_DRV8323
|
||||
// nFAULT 快速保护（低有效，1ms 轮询）：GDF/UVLO/OTSD 等硬件故障
|
||||
// → 立即封 PWM 并睡眠，锁存到 CAN 重新下发恢复命令为止。
|
||||
// VDS 过流当前配置为"仅上报不动作"，不会拉低 nFAULT，不在此误触发。
|
||||
if (drv_nfault_active()) {
|
||||
FOC.shut_down();
|
||||
}
|
||||
#endif
|
||||
thermal_value = Q16_16_MUL(thermal_value, thermalRate);
|
||||
// 添加温度积累
|
||||
int drv_temp = (int)sampler.get_drv_temp();
|
||||
@@ -60,28 +69,65 @@ u32 ThermalController::update() {
|
||||
return --remainingTime10Ms;
|
||||
} else {
|
||||
// 当前没有过热停机
|
||||
// 添加电流热积累
|
||||
u32 currents[3] = {(u32)(sampler.get_current('U') * 1000),
|
||||
(u32)(sampler.get_current('V') * 1000),
|
||||
(u32)(sampler.get_current('W') * 1000)};
|
||||
// 添加电流热积累：取三相有效相的最大绝对值（热量与电流方向无关，
|
||||
// 负半周同样发热）。采样失效帧(ERROR_ADC_VAL)不参与积累：
|
||||
// 失效值 255 按 mA 参与平方运算会超出 int/u32 范围，热积累不可控；
|
||||
// 三相持续全部失效说明采样链异常，按过热关机 fail-safe。
|
||||
static u16 current_error_frames = 0;
|
||||
const float raw_current[3] = {sampler.get_current('U'),
|
||||
sampler.get_current('V'),
|
||||
sampler.get_current('W')};
|
||||
u32 currents[3] = {0, 0, 0};
|
||||
bool valid[3] = {false, false, false};
|
||||
for (u8 i = 0; i < 3; i++) {
|
||||
if (raw_current[i] == ERROR_ADC_VAL) {
|
||||
continue;
|
||||
}
|
||||
// get_current 已保证有效值 |I| ≤ MAX_CURRENT(40A)，转 mA ≤ 40000
|
||||
currents[i] = (u32)(fabsf(raw_current[i]) * 1000.0f);
|
||||
valid[i] = true;
|
||||
}
|
||||
|
||||
// 找最大电流（原始ADC值）
|
||||
current_max = max(currents[0], currents[1], currents[2]);
|
||||
if (!(valid[0] || valid[1] || valid[2])) {
|
||||
if (++current_error_frames >= CURRENT_ERROR_SHUTDOWN_COUNT) {
|
||||
current_error_frames = 0;
|
||||
FOC.shut_down_thermal();
|
||||
remainingTime10Ms = PROTECT_TIME;
|
||||
CAN0_transmiter.ThermalStatus = 1;
|
||||
return remainingTime10Ms;
|
||||
}
|
||||
return thermal_value;
|
||||
}
|
||||
current_error_frames = 0;
|
||||
|
||||
// 找最大电流（mA，绝对值）；个别相失效时用剩余有效相
|
||||
u32 max_current = 0;
|
||||
for (u8 i = 0; i < 3; i++) {
|
||||
if (valid[i] && currents[i] > max_current) {
|
||||
max_current = currents[i];
|
||||
}
|
||||
}
|
||||
current_max = (u16)max_current;
|
||||
// 平方运算先转 float：40A 档 40000²×10 超 int 范围，整数运算会溢出
|
||||
const float imax = (float)current_max;
|
||||
// 电流系数按"绝对值全周期"口径标定：取 fabs 后正负半周都积累，
|
||||
// 交流负载下积累速度约为旧版(仅正半周)的 2 倍，故系数统一取旧值一半，
|
||||
// 保持原实测触发时机不变；温度积累与恢复阈值不受此补偿影响。
|
||||
if (current_max < 2000) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.00007f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.000035f); // 计算当前电流热积累值
|
||||
} else if (current_max > 3800) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.01f; // 添加当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.005f); // 添加当前电流热积累值
|
||||
} else if (current_max > 3400) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.005f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.0025f); // 计算当前电流热积累值
|
||||
} else if (current_max > 3000) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.0002f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.0001f); // 计算当前电流热积累值
|
||||
} else if (current_max > 2000) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.0001f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.00005f); // 计算当前电流热积累值
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
#ifndef _HEALTH_H
|
||||
#define _HEALTH_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#include <cstdint>
|
||||
typedef int32_t q16_16_t; // Q16.16格式
|
||||
// 定点数运算宏
|
||||
@@ -36,7 +38,6 @@ enum class VoltageState : uint8_t {
|
||||
*/
|
||||
class ThermalController {
|
||||
private:
|
||||
u8 temperature;
|
||||
u16 current_max;
|
||||
|
||||
q16_16_t thermalRate; // 热耗散系数（每单位时间散热比例）
|
||||
|
||||
@@ -231,9 +231,6 @@ void VersionManager::printVersions() const {
|
||||
versionToString(getBootVersion(), versionStr, sizeof(versionStr)));
|
||||
}
|
||||
|
||||
const char* VersionManager::getAppBuildTime() const {
|
||||
return APP_BUILD_DATE " " APP_BUILD_TIME;
|
||||
}
|
||||
|
||||
/* retarget the C library printf function to the USART */
|
||||
/**
|
||||
@@ -286,47 +283,6 @@ void unused_pin_init() {
|
||||
gpio_bit_reset(GPIOC, GPIO_PIN_14 | GPIO_PIN_15);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 检测并打印复位源信息
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
u16 get_reset_cause(void) {
|
||||
u16 cause = 0;
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_FWDGTRST)) {
|
||||
// OTA_DEBUG_PRINT("Free Watchdog Timer Reset\r\n");
|
||||
cause = RCU_FLAG_FWDGTRST;
|
||||
}
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_EPRST)) {
|
||||
// OTA_DEBUG_PRINT("External Pin Reset\r\n");
|
||||
cause = RCU_FLAG_EPRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_PORRST)) {
|
||||
// OTA_DEBUG_PRINT("Power Reset\r\n");
|
||||
cause = RCU_FLAG_PORRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_WWDGTRST)) {
|
||||
// OTA_DEBUG_PRINT("Window Watchdog Timer Reset\r\n");
|
||||
cause = RCU_FLAG_WWDGTRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_LPRST)) {
|
||||
// OTA_DEBUG_PRINT("Low Power Reset\r\n");
|
||||
cause = RCU_FLAG_LPRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_SWRST)) {
|
||||
// OTA_DEBUG_PRINT("Software Reset\r\n");
|
||||
cause = RCU_FLAG_SWRST;
|
||||
}
|
||||
// 清除所有复位标志
|
||||
rcu_all_reset_flag_clear();
|
||||
// OTA_DEBUG_PRINT("cause:%d\r\n", cause);
|
||||
return cause;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化 OTA 与 A/B 分区状态
|
||||
|
||||
@@ -59,11 +59,6 @@ u8 USART0_init(void);
|
||||
* @return u8 返回值
|
||||
*/
|
||||
u8 watchdog_init(u32 time);
|
||||
/**
|
||||
* @brief 获取复位原因
|
||||
* @return u16 返回值
|
||||
*/
|
||||
u16 get_reset_cause(void);
|
||||
/**
|
||||
* @brief 初始化未使用引脚状态
|
||||
* @return void
|
||||
@@ -155,8 +150,6 @@ class VersionManager {
|
||||
char *versionToString(const VersionInfo &version, char *buffer,
|
||||
size_t bufSize) const;
|
||||
|
||||
// 获取编译时间
|
||||
const char* getAppBuildTime() const;
|
||||
|
||||
// 打印所有版本信息
|
||||
/**
|
||||
|
||||
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "gd32c10x.h"
|
||||
#include "gd32c10x_fmc.h"
|
||||
#include "motor.h"
|
||||
#include "tasks.h"
|
||||
#include <cstdio>
|
||||
|
||||
// SLEEP 调试宏定义
|
||||
@@ -149,18 +150,8 @@ extern "C" void SysTick_Handler(void) {
|
||||
|
||||
busy_cycles_accum = 0;
|
||||
sample_seconds = 0;
|
||||
}
|
||||
}
|
||||
/*!
|
||||
\brief delay a time in seconds
|
||||
\param[in] count: count in seconds
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
void delay_1s(uint32_t count) {
|
||||
uint32_t i;
|
||||
for (i = 0; i < count; i++) {
|
||||
delay_1ms(1000);
|
||||
// 按任务负载统计与全局口径同步：同一秒窗口
|
||||
task_busy_cycles_reset();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -59,13 +59,6 @@ uint32_t get_systick_ms(void);
|
||||
* @return void
|
||||
*/
|
||||
void delay_1ms(uint32_t count);
|
||||
/* delay a time in seconds */
|
||||
/**
|
||||
* @brief 秒级阻塞延时
|
||||
* @param[in] count 参数
|
||||
* @return void
|
||||
*/
|
||||
void delay_1s(uint32_t count);
|
||||
/* delay decrement */
|
||||
/**
|
||||
* @brief 递减延时计数器
|
||||
|
||||
+38
-10
@@ -11,6 +11,7 @@
|
||||
#include "ang_sensor.h"
|
||||
#include "health.h"
|
||||
#include "systick.h"
|
||||
#include "gd32c10x.h"
|
||||
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
extern sampler sampler;
|
||||
@@ -45,6 +46,29 @@ static bool has_task(u32 currentTasks, SystemTask task) {
|
||||
return (currentTasks & (1u << task)) != 0U;
|
||||
}
|
||||
|
||||
// 按任务 DWT 负载计量：定位 CPU 占用分布（与全局 cpu_usage 同 1s 口径，
|
||||
// 由 SysTick 统计块调用 task_busy_cycles_reset() 同步清零）。
|
||||
// 120MHz 下 120000 周期 ≈ 1% CPU。仅统计任务本体，不含中断抢占时间。
|
||||
static volatile u32 task_busy_cycles[TASK_COUNT] = {0};
|
||||
|
||||
u32 get_task_busy_cycles(u8 task) {
|
||||
return (task < TASK_COUNT) ? task_busy_cycles[task] : 0U;
|
||||
}
|
||||
|
||||
void task_busy_cycles_reset(void) {
|
||||
for (u8 i = 0; i < TASK_COUNT; i++) {
|
||||
task_busy_cycles[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 执行任务并累计其忙碌周期
|
||||
#define RUN_TIMED(task_enum, call) \
|
||||
do { \
|
||||
u32 t0_ = DWT->CYCCNT; \
|
||||
call; \
|
||||
task_busy_cycles[task_enum] += DWT->CYCCNT - t0_; \
|
||||
} while (0)
|
||||
|
||||
/**
|
||||
* @brief 控制模式变化时复位所有控制环，避免积分项跨模式继承
|
||||
* @return void
|
||||
@@ -96,17 +120,19 @@ static void run_motor_control_task(void) {
|
||||
*/
|
||||
static void run_fast_sample_tasks(u32 currentTasks) {
|
||||
if (has_task(currentTasks, TASK_CURRENT_UPDATE)) {
|
||||
sampler.update_current();
|
||||
RUN_TIMED(TASK_CURRENT_UPDATE, sampler.update_current());
|
||||
}
|
||||
if (has_task(currentTasks, TASK_ANGLE_UPDATE)) {
|
||||
KTH7823.update();
|
||||
FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
|
||||
RUN_TIMED(TASK_ANGLE_UPDATE, KTH7823.update());
|
||||
RUN_TIMED(TASK_ANGLE_UPDATE,
|
||||
FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()));
|
||||
}
|
||||
if (has_task(currentTasks, TASK_VELOCITY_UPDATE)) {
|
||||
FOC.rt_velocity = Angle_kalman.getVelocity();
|
||||
RUN_TIMED(TASK_VELOCITY_UPDATE,
|
||||
FOC.rt_velocity = Angle_kalman.getVelocity());
|
||||
}
|
||||
if (has_task(currentTasks, TASK_MOTOR_CONTROL)) {
|
||||
run_motor_control_task();
|
||||
RUN_TIMED(TASK_MOTOR_CONTROL, run_motor_control_task());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -117,19 +143,19 @@ static void run_fast_sample_tasks(u32 currentTasks) {
|
||||
*/
|
||||
static void run_housekeeping_tasks(u32 currentTasks) {
|
||||
if (has_task(currentTasks, TASK_MCU_TEMP_UPDATE)) {
|
||||
sampler.update_mcu_temp();
|
||||
RUN_TIMED(TASK_MCU_TEMP_UPDATE, sampler.update_mcu_temp());
|
||||
}
|
||||
if (has_task(currentTasks, TASK_DRV_TEMP_UPDATE)) {
|
||||
sampler.update_drv_temp();
|
||||
RUN_TIMED(TASK_DRV_TEMP_UPDATE, sampler.update_drv_temp());
|
||||
}
|
||||
if (has_task(currentTasks, TASK_THERMAL_CONTROL)) {
|
||||
thermalController.update();
|
||||
RUN_TIMED(TASK_THERMAL_CONTROL, thermalController.update());
|
||||
}
|
||||
if (has_task(currentTasks, TASK_MAGNETIC_CHECK)) {
|
||||
angsens_MG_check();
|
||||
RUN_TIMED(TASK_MAGNETIC_CHECK, angsens_MG_check());
|
||||
}
|
||||
if (has_task(currentTasks, TASK_VBUS_UPDATE)) {
|
||||
power_manage();
|
||||
RUN_TIMED(TASK_VBUS_UPDATE, power_manage());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -138,11 +164,13 @@ static void run_housekeeping_tasks(u32 currentTasks) {
|
||||
* @return void
|
||||
*/
|
||||
static void run_can_task(void) {
|
||||
u32 t0 = DWT->CYCCNT;
|
||||
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();
|
||||
task_busy_cycles[TASK_CAN_TRANSMIT] += DWT->CYCCNT - t0;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
#ifndef TASKS_H
|
||||
#define TASKS_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#include "initial.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
@@ -14,6 +16,19 @@ extern "C" {
|
||||
*/
|
||||
void do_task(u32 currentTasks);
|
||||
|
||||
/**
|
||||
* @brief 读取上一统计周期(1s)内指定任务累计消耗的 CPU 周期数
|
||||
* @param[in] task SystemTask 枚举值
|
||||
* @return u32 周期数（120MHz 下 120000 = 1% CPU）
|
||||
*/
|
||||
u32 get_task_busy_cycles(u8 task);
|
||||
|
||||
/**
|
||||
* @brief 清零按任务负载统计（SysTick 每秒统计 cpu_usage 时同步调用）
|
||||
* @return void
|
||||
*/
|
||||
void task_busy_cycles_reset(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -13,7 +13,6 @@
|
||||
以下 `constexpr` 对象为编译期常量，非可变全局状态，无运行期写入、并发访问或动态初始化顺序风险：
|
||||
|
||||
- `device/ADC.cpp`: `ADC_SCALE`, `TEMP_FACTOR`, `VCC_FACTOR`, `CURRENT_FACTOR`
|
||||
- `device/Can.cpp`: `CANX`
|
||||
- `sys/health.cpp`: `PROTECT_TIME`
|
||||
- `sys/config.h`: 版本号、Flash 地址、协议 ID、滤波/PID 参数、数学常量、Timer 参数等 `constexpr` 配置项
|
||||
|
||||
|
||||
Reference in New Issue
Block a user