From 7c4aaa5ee66a381cb214430907b5d3e4250e3fa8 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 1 Sep 2026 02:46:25 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E4=BC=98=E5=8C=96FOC=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E7=AE=97=E6=B3=95=E5=B9=B6=E4=BF=AE=E5=A4=8D=E5=A4=9A?= =?UTF-8?q?=E4=B8=AA=E6=8E=A7=E5=88=B6=E7=8E=AF=E8=B7=AF=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 重构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算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况 --- Robot_V2/Algorithm/FOC.cpp | 238 +++++++++++++++++++---------- Robot_V2/Algorithm/FOC.h | 45 +++--- Robot_V2/Algorithm/Filter.cpp | 35 ++++- Robot_V2/Algorithm/Filter.h | 2 + Robot_V2/Algorithm/pid.cpp | 68 +++++++-- Robot_V2/Algorithm/pid.h | 34 ++++- Robot_V2/device/ADC.cpp | 113 +++++++++++--- Robot_V2/device/Can.cpp | 269 +-------------------------------- Robot_V2/device/Can.h | 15 -- Robot_V2/device/Timer.cpp | 38 ++++- Robot_V2/device/Timer.h | 2 + Robot_V2/device/adc.h | 17 ++- Robot_V2/device/ang_sensor.cpp | 131 +++------------- Robot_V2/device/ang_sensor.h | 19 +-- Robot_V2/device/motor.cpp | 226 +++++++++++++++++++++++++-- Robot_V2/device/motor.h | 37 +++++ Robot_V2/main.cpp | 11 +- Robot_V2/main.h | 2 + Robot_V2/sys/config.h | 150 ++++++++++++------ Robot_V2/sys/flash.cpp | 218 +++++++++++++++++--------- Robot_V2/sys/flash.h | 16 +- Robot_V2/sys/health.cpp | 84 +++++++--- Robot_V2/sys/health.h | 3 +- Robot_V2/sys/initial.cpp | 44 ------ Robot_V2/sys/initial.h | 7 - Robot_V2/sys/systick.cpp | 15 +- Robot_V2/sys/systick.h | 7 - Robot_V2/tasks.cpp | 48 ++++-- Robot_V2/tasks.h | 15 ++ STATIC_ANALYSIS_NOTES.md | 1 - 30 files changed, 1127 insertions(+), 783 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 37c7b91..3570813 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -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) / - (MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW); - } - vel_p *= p_scale; + float abs_target = fabsf(target_vel); + float abs_speed = fabsf(actual_vel); + float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed; + if (speed_ref <= MODE3_LOW_SPEED_P_LOW) { + return min_scale; } - return vel_p; + 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); + } + return 1.0f; +} + +/** + * @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; } diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 376af6b..039586d 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -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); }; // 定义函数指针类型 diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index 7f64c42..14874c4 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -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类的实现 diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index d741e46..1380fbc 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -7,6 +7,8 @@ #ifndef _FILTER_H #define _FILTER_H + +#include "config.h" #ifdef __cplusplus extern "C" { #endif diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 9a44bc3..0970b38 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -12,33 +12,55 @@ #include 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; } /** diff --git a/Robot_V2/Algorithm/pid.h b/Robot_V2/Algorithm/pid.h index 2f4dcee..ebf143d 100644 --- a/Robot_V2/Algorithm/pid.h +++ b/Robot_V2/Algorithm/pid.h @@ -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 \ No newline at end of file diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 6e85afe..3432ce7 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -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; - adc_flag_clear(ADC1, ADC_FLAG_EOC); + 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; diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index ce46212..1292b30 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -22,7 +22,6 @@ #include #include #include -#include // 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 返回值 - */ - std::uniform_int_distribution 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 返回值 - */ - std::uniform_int_distribution 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); } diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index e1d4b2c..733694b 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -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 通信类 diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 4ee0d11..e867f06 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -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 } diff --git a/Robot_V2/device/Timer.h b/Robot_V2/device/Timer.h index ac56b20..0556887 100644 --- a/Robot_V2/device/Timer.h +++ b/Robot_V2/device/Timer.h @@ -7,6 +7,8 @@ #ifndef _TIMER_H #define _TIMER_H + +#include "config.h" #ifdef __cplusplus extern "C" { #endif diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h index dfe2237..a9a5bc8 100644 --- a/Robot_V2/device/adc.h +++ b/Robot_V2/device/adc.h @@ -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); diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index ca7de0a..d45ccfe 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -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, - uint16_t *rx_data) { - // 重置完成标志 - spi_dma_tx_complete = false; - spi_dma_rx_complete = false; +int spi_rw_half_word_polling(uint32_t spi_periph, uint16_t tx_data, + uint16_t *rx_data) { + 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 传输完成 + while (spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE) == RESET) { + if (--timeout == 0U) { + return -1; + } } + spi_i2s_data_transmit(spi_periph, tx_data); - // 检查是否超时 - 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"); - return -1; + 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) { diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index 5844342..cb60b6f 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -21,14 +21,17 @@ 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, - uint16_t *rx_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) diff --git a/Robot_V2/device/motor.cpp b/Robot_V2/device/motor.cpp index db76814..d15947a 100644 --- a/Robot_V2/device/motor.cpp +++ b/Robot_V2/device/motor.cpp @@ -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 +/* 角度传感器 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 } /** @@ -44,17 +248,13 @@ 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); +void motor_sleep(void) { + 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); -} \ No newline at end of file +void motor_resume(void) { + drv_enable(1); +} diff --git a/Robot_V2/device/motor.h b/Robot_V2/device/motor.h index 49ba865..8ac37f0 100644 --- a/Robot_V2/device/motor.h +++ b/Robot_V2/device/motor.h @@ -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 \ No newline at end of file diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 77c8622..4346926 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -15,6 +15,7 @@ #include "flash.h" #include "health.h" #include "initial.h" +#include "motor.h" #include "systick.h" // MAIN 调试宏定义 @@ -79,8 +80,9 @@ int main(void) { */ u8 hardware_init(void) { __enable_irq(); - systick_config(); // 系统时钟初始化 - unused_pin_init(); // 未使用引脚初始化 + 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; } diff --git a/Robot_V2/main.h b/Robot_V2/main.h index 104885b..3a482c7 100644 --- a/Robot_V2/main.h +++ b/Robot_V2/main.h @@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MAIN_H #define MAIN_H + +#include "config.h" #include /** diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index c433c37..18a0815 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -9,6 +9,18 @@ #include "gd32c10x_eval.h" #include +// 项目级定宽类型别名（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 diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index b1b0bbb..5e55295 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -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 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; + EEP_DEBUG_PRINT("Erasing Node ID pages at 0x%08X / 0x%08X\r\n", + NODE_ID_PRIMARY_ADDR, NODE_ID_BACKUP_ADDR); + + 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); - - 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; + 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_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 { + *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 read failed from primary: status=%d\r\n", status); + 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; - - FlashEepromStatus primary_status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, ¤t_id, ¤t_counter); - - // 计数器 +1 - u32 new_counter = current_counter + 1; - // 如果计数器溢出（0xFFFFFFFF + 1 = 0），则使用1 + // 读取两份备份，确定当前最新有效记录（计数器基准 + 乒乓目标选择） + u16 id_primary = 0, id_backup = 0; + u32 counter_primary = 0, counter_backup = 0; + + 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); + + 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); - - 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); + 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: %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", - 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 verify failed: read_id=%d (expected=%d), " + "read_counter=%u (expected=%u)\r\n", + read_id, node_id, read_counter, new_counter); } 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(); diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 6bde8e1..409897d 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -7,6 +7,8 @@ #ifndef __FLASH_H #define __FLASH_H + +#include "config.h" #include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用 #include @@ -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 返回值 diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index 42a13e3..78a5fd2 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -10,6 +10,8 @@ #include "FOC.h" #include "adc.h" #include "flash.h" +#include "motor.h" +#include 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); // 计算当前电流热积累值 } } diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h index 5497f1f..c70205b 100644 --- a/Robot_V2/sys/health.h +++ b/Robot_V2/sys/health.h @@ -7,6 +7,8 @@ #ifndef _HEALTH_H #define _HEALTH_H + +#include "config.h" #include 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; // 热耗散系数（每单位时间散热比例） diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index a09de1d..190d0e5 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -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 分区状态 diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 285ce91..5906301 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -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; // 打印所有版本信息 /** diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index a02c987..68434d1 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -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 // 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(); } } diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index b473044..1322073 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -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 递减延时计数器 diff --git a/Robot_V2/tasks.cpp b/Robot_V2/tasks.cpp index fe3bf54..688fb7e 100644 --- a/Robot_V2/tasks.cpp +++ b/Robot_V2/tasks.cpp @@ -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; } /** diff --git a/Robot_V2/tasks.h b/Robot_V2/tasks.h index 21fd04f..b6cba30 100644 --- a/Robot_V2/tasks.h +++ b/Robot_V2/tasks.h @@ -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 diff --git a/STATIC_ANALYSIS_NOTES.md b/STATIC_ANALYSIS_NOTES.md index a5870ef..fa80285 100644 --- a/STATIC_ANALYSIS_NOTES.md +++ b/STATIC_ANALYSIS_NOTES.md @@ -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` 配置项