feat(FOC): 优化FOC控制算法并修复多个控制环路问题
- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值 以避免重复三角函数计算，提升性能约15-300周期 - 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的 震荡问题和正转正常反转震荡的根本原因 - 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦 hunting抑制效果 - 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待， 提升实时性能 - 修复初始化期间电压分母为零的问题，添加下限保护防止P增益 无穷大 - 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击 - 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定 性能 perf(Filter): 优化中值滤波算法性能 - 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络， 用12次比较替代通用std::nth_element算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
This commit is contained in:
+105
-45
@@ -9,6 +9,18 @@
|
||||
#include "gd32c10x_eval.h"
|
||||
#include <cstdint>
|
||||
|
||||
// 项目级定宽类型别名（u8/u16/u32 等）：整个工程统一从这里取，
|
||||
// 重复 typedef 同一类型在 C++ 中合法，包含多次无副作用
|
||||
typedef uint8_t u8;
|
||||
typedef uint16_t u16;
|
||||
typedef uint32_t u32;
|
||||
typedef int8_t s8;
|
||||
typedef int16_t s16;
|
||||
typedef int32_t s32;
|
||||
typedef volatile uint8_t vu8;
|
||||
typedef volatile uint16_t vu16;
|
||||
typedef volatile uint32_t vu32;
|
||||
|
||||
// 如果使用 CMSIS DSP 库，需要定义 ARM_MATH_CM4
|
||||
#define ARM_MATH_CM4
|
||||
|
||||
@@ -20,27 +32,81 @@
|
||||
#define MAIN_DEBUG 0
|
||||
#define FOC_DEBUG 0
|
||||
|
||||
// 版本号定义 - 可根据实际项目进行调整
|
||||
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
|
||||
constexpr u16 SOFTWARE_VERSION_MINOR = 8;
|
||||
constexpr u16 SOFTWARE_VERSION_PATCH = 16;
|
||||
constexpr u16 SOFTWARE_VERSION_BUILD = 7;
|
||||
// ===================== 版本号定义 =====================
|
||||
// 软件版本 = 编译日期自动生成（年.月.日）+ 当天构建小版本号（手动递增）
|
||||
// __DATE__ 形如 "Mmm dd yyyy"（如 "Aug 29 2026"）：2026-08-29 当天第一次
|
||||
// 构建得到 26.8.29.1；同一天多次出固件时把 SOFTWARE_VERSION_BUILD +1
|
||||
#define CFG_VER_YEAR \
|
||||
((__DATE__[7] - '0') * 1000 + (__DATE__[8] - '0') * 100 + \
|
||||
(__DATE__[9] - '0') * 10 + (__DATE__[10] - '0'))
|
||||
#define CFG_VER_IS_JAN (__DATE__[0] == 'J' && __DATE__[1] == 'a' && __DATE__[2] == 'n')
|
||||
#define CFG_VER_IS_FEB (__DATE__[0] == 'F' && __DATE__[1] == 'e' && __DATE__[2] == 'b')
|
||||
#define CFG_VER_IS_MAR (__DATE__[0] == 'M' && __DATE__[1] == 'a' && __DATE__[2] == 'r')
|
||||
#define CFG_VER_IS_APR (__DATE__[0] == 'A' && __DATE__[1] == 'p' && __DATE__[2] == 'r')
|
||||
#define CFG_VER_IS_MAY (__DATE__[0] == 'M' && __DATE__[1] == 'a' && __DATE__[2] == 'y')
|
||||
#define CFG_VER_IS_JUN (__DATE__[0] == 'J' && __DATE__[1] == 'u' && __DATE__[2] == 'n')
|
||||
#define CFG_VER_IS_JUL (__DATE__[0] == 'J' && __DATE__[1] == 'u' && __DATE__[2] == 'l')
|
||||
#define CFG_VER_IS_AUG (__DATE__[0] == 'A' && __DATE__[1] == 'u' && __DATE__[2] == 'g')
|
||||
#define CFG_VER_IS_SEP (__DATE__[0] == 'S' && __DATE__[1] == 'e' && __DATE__[2] == 'p')
|
||||
#define CFG_VER_IS_OCT (__DATE__[0] == 'O' && __DATE__[1] == 'c' && __DATE__[2] == 't')
|
||||
#define CFG_VER_IS_NOV (__DATE__[0] == 'N' && __DATE__[1] == 'o' && __DATE__[2] == 'v')
|
||||
#define CFG_VER_IS_DEC (__DATE__[0] == 'D' && __DATE__[1] == 'e' && __DATE__[2] == 'c')
|
||||
#define CFG_VER_MONTH \
|
||||
(CFG_VER_IS_JAN ? 1 \
|
||||
: CFG_VER_IS_FEB ? 2 \
|
||||
: CFG_VER_IS_MAR ? 3 \
|
||||
: CFG_VER_IS_APR ? 4 \
|
||||
: CFG_VER_IS_MAY ? 5 \
|
||||
: CFG_VER_IS_JUN ? 6 \
|
||||
: CFG_VER_IS_JUL ? 7 \
|
||||
: CFG_VER_IS_AUG ? 8 \
|
||||
: CFG_VER_IS_SEP ? 9 \
|
||||
: CFG_VER_IS_OCT ? 10 \
|
||||
: CFG_VER_IS_NOV ? 11 \
|
||||
: CFG_VER_IS_DEC ? 12 \
|
||||
: 0)
|
||||
/* 日字段两位，个位数时首字符为空格（如 "Aug 5 2026"） */
|
||||
#define CFG_VER_DAY \
|
||||
((__DATE__[4] == ' ') ? (__DATE__[5] - '0') \
|
||||
: ((__DATE__[4] - '0') * 10 + (__DATE__[5] - '0')))
|
||||
|
||||
constexpr u16 SOFTWARE_VERSION_MAJOR = CFG_VER_YEAR % 100; // 年后两位，如 26
|
||||
constexpr u16 SOFTWARE_VERSION_MINOR = CFG_VER_MONTH; // 月
|
||||
constexpr u16 SOFTWARE_VERSION_PATCH = CFG_VER_DAY; // 日
|
||||
constexpr u16 SOFTWARE_VERSION_BUILD = 1; // 当天第几次构建，手动递增
|
||||
|
||||
// 硬件版本：全手写，仅在 PCB 改版时修改（编译 APP 不影响）
|
||||
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
|
||||
constexpr u16 HARDWARE_VERSION_MINOR = 12;
|
||||
constexpr u16 HARDWARE_VERSION_PATCH = 01;
|
||||
constexpr u16 HARDWARE_VERSION_PATCH = 1;
|
||||
constexpr u16 HARDWARE_VERSION_BUILD = 0;
|
||||
|
||||
// BOOT 版本：全手写，仅在 bootloader 更新时修改（APP 编译不影响）
|
||||
constexpr u16 BOOT_VERSION_MAJOR = 26;
|
||||
constexpr u16 BOOT_VERSION_MINOR = 1;
|
||||
constexpr u16 BOOT_VERSION_PATCH = 12;
|
||||
constexpr u16 BOOT_VERSION_BUILD = 0;
|
||||
|
||||
// ===================== 功率级门驱动芯片选择 =====================
|
||||
// 1 = 新板 DRV8323RS（外置 WSD6035DN33）：使能前经 SPI 配置 3x PWM 模式、
|
||||
// CSA 增益、VDS 过流门限；3 路 PWM 接 INHA/B/C（CH2/CH1/CH0，
|
||||
// 固件内已做通道重映射）；PB12(DRVSLP) 同时是使能脚与 INLA/B/C 电平
|
||||
// （高=正常运转，低=三相 Hi-Z 睡眠滑行）。
|
||||
// 0 = 老板 DRV8313（集成 FET 三半桥）：无 SPI，保持原使能时序与相序。
|
||||
#define GATE_DRIVER_DRV8323 0
|
||||
|
||||
#if GATE_DRIVER_DRV8323
|
||||
// DRV8323 片上 CSA 增益（V/V），经 SPI 写入 0x06[7:6]，仅 5/10/20/40 有效。
|
||||
// 双极性量程 = ±1.655V/(G×Rshunt)：G=10、R=10mΩ 时约 ±16.5A。
|
||||
// 修改后电流换算系数 CURRENT_FACTOR（ADC.cpp）自动跟随，但软件限流标定需复核。
|
||||
constexpr float DRV8323_CSA_GAIN_VV = 10.0f;
|
||||
constexpr float DRV8323_SHUNT_OHM = 0.010f;
|
||||
#endif
|
||||
// 编译时间（AC6 编译器自动生成）
|
||||
#define APP_BUILD_DATE __DATE__
|
||||
#define APP_BUILD_TIME __TIME__
|
||||
|
||||
// Flash 分区配置
|
||||
constexpr u32 BOOTLOADER_SIZE = 0x6000; // 24KB
|
||||
constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT 起始地址
|
||||
constexpr u32 APPLATEST_START_ADDR = 0x08012000; // APP 起始地址
|
||||
constexpr u32 APPBACK_START_ADDR = 0x08006000; // APPBACK 起始地址
|
||||
@@ -50,33 +116,33 @@ constexpr u32 SLEEP_CONFIG_START = 0x0801E400; // 睡眠配置起始地址
|
||||
constexpr u32 SLEEP_CONFIG_END = 0x0801E800; // 睡眠配置结束地址
|
||||
constexpr u32 FLASH_EMPTY = 0xFFFFFFFF; // 空数据
|
||||
constexpr u32 SLEEP_ENABLE = 0x11111111; // 睡眠使能
|
||||
constexpr u32 SLEEP_DISABLE = 0x00000000; // 睡眠禁止
|
||||
|
||||
// 节点 ID 配置 (用于多电机 CAN 总线隔离)
|
||||
// 使用 CAN ID 高 16 位 (bits 31-16) 存储节点 ID，低 16 位 (bits 15-0) 存储基础 ID
|
||||
// 格式：[节点 ID(16 位)][基础 ID(16 位)]，例如：node_id=1, base_id=0x0200 → 0x00010200
|
||||
//
|
||||
// 节点 ID 存储方案：
|
||||
// - 使用独立 Flash 页 (NODE_ID_PAGE_ADDR) 存储节点 ID，与EEPROM完全隔离
|
||||
// - 采用双重备份机制 (主备份 + 备用备份)
|
||||
// - 使用独立 Flash 页存储节点 ID，与EEPROM完全隔离
|
||||
// - 采用双重备份机制：主备份与备用备份分属两个独立 1KB 页，
|
||||
// 擦写其一不影响另一份，掉电时至少保留一份有效数据
|
||||
// - 写入采用乒乓方式：总是写入"非最新"的那一页，另一页全程保有旧值
|
||||
// - 使用魔数 (NODE_ID_MAGIC) 验证数据有效性，防止位翻转导致错读
|
||||
// - 使用写入计数器区分最新数据
|
||||
constexpr u32 NODE_ID_PAGE_ADDR = 0x0801E800; // 节点 ID 存储页 (1KB，独立区域)
|
||||
constexpr u32 NODE_ID_PRIMARY_ADDR = 0x0801E800; // 主存储地址
|
||||
constexpr u32 NODE_ID_BACKUP_ADDR = 0x0801E840; // 备份存储地址 (偏移64字节)
|
||||
// - 使用写入计数器区分最新数据（计数器跳过 0，比较用有符号差值容忍回绕）
|
||||
constexpr u32 NODE_ID_PAGE_ADDR = 0x0801E800; // 节点 ID 主存储页 (1KB，独立区域)
|
||||
constexpr u32 NODE_ID_PRIMARY_ADDR = 0x0801E800; // 主备份地址
|
||||
constexpr u32 NODE_ID_BACKUP_ADDR = 0x0801EC00; // 备用备份地址（独立 1KB 页）
|
||||
constexpr u32 NODE_ID_MAGIC = 0x4E4F4445; // 魔数 'NODE' 用于验证数据有效性
|
||||
constexpr u16 DEFAULT_NODE_ID = 1; // 默认节点 ID
|
||||
constexpr u16 BROADCAST_NODE_ID = 0x1FFF; // 广播 ID (所有节点响应)
|
||||
constexpr u16 MAX_NODE_ID = 0x1FFE; // 最大有效节点 ID (0~0x1FFE)
|
||||
constexpr u16 SETTABLE_NODE_ID_MAX = 0x00FF; // SET_NODE_ID 命令允许设置的上限 (1~0xFF)
|
||||
|
||||
// 节点 ID 存储结构 (存储在Flash中的格式)
|
||||
// 节点 ID 存储结构 (主备份/备用备份各一份，每份 16 字节，位于各自页首)
|
||||
// 地址偏移 0x00: 魔数 (0x4E4F4445 = 'NODE')
|
||||
// 地址偏移 0x04: 节点 ID (u16)
|
||||
// 地址偏移 0x06: 保留 (u16，填充为0xFFFF)
|
||||
// 地址偏移 0x08: 写入计数器 (u32，用于区分最新数据)
|
||||
// 地址偏移 0x0C: CRC32 校验 (u32)
|
||||
constexpr u8 NODE_ID_STRUCT_SIZE = 0x10; // 结构体大小 16 字节
|
||||
constexpr u8 NODE_ID_MAGIC_OFFSET = 0x00; // 魔数偏移
|
||||
constexpr u8 NODE_ID_VALUE_OFFSET = 0x04; // 节点ID值偏移
|
||||
constexpr u8 NODE_ID_COUNTER_OFFSET = 0x08; // 写入计数器偏移
|
||||
@@ -136,14 +202,24 @@ constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪
|
||||
// PID 配置
|
||||
constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流 P
|
||||
constexpr float current_loop_Ki = 0.55f; // 电流 P
|
||||
constexpr float current_loop_Kd = 3e-4f; // 电流 P
|
||||
// 电流反馈噪声大（CurrentKalman 稳态增益≈1，几乎无滤波），D 项只放大噪声，置零
|
||||
constexpr float current_loop_Kd = 0.0f;
|
||||
|
||||
constexpr float DEFAULT_vel_loop_p = 0.005f / 100 * 12; // 速度 P
|
||||
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
|
||||
constexpr float vel_loop_Tu = 17.0f * 1e-3f;
|
||||
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
|
||||
// 速度 D 恢复（提供加速度阻尼）；现走"测量微分+1ms低通"路径，无目标阶跃踢、
|
||||
// 噪声被低通抑制，比旧的裸误差微分安全
|
||||
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
|
||||
|
||||
// PID 抗饱和与微分滤波（负载稳定性优化，2026-08）
|
||||
// 抗饱和主体为条件积分（饱和方向冻结，见 pid.cpp）；此增益仅用于
|
||||
// bleed_integral() 外部饱和泄放（前馈叠加超限时），且带同号保护不过零
|
||||
constexpr float ANTI_WINDUP_GAIN = 0.3f; // 外部饱和泄放比例
|
||||
constexpr float DERIV_LPF_TAU = 0.001f; // 微分项一阶低通时间常数 (s)
|
||||
constexpr float MIN_LIMIT_VOLTAGE = 0.1f; // P 增益换算分母下限，防 limit_voltage=0 除零
|
||||
|
||||
// mode2 角度同步 Uq 前馈补偿：固定 Uq 下的真实速度波动不应从反馈中滤掉，
|
||||
// 而应按物理 360° 提前补偿负载/阻力差异，降低速度 PID 的周期性纠偏量。
|
||||
constexpr bool MODE2_UQ_FF_ENABLE = true;
|
||||
@@ -155,6 +231,7 @@ constexpr float MODE2_UQ_FF_GAIN = 0.003f; // 速度误差(deg/s)到前馈
|
||||
constexpr float MODE2_UQ_FF_MAX = 3.5f; // 前馈补偿电压限幅(V)
|
||||
constexpr float MODE2_UQ_FF_STOP_SPEED = 2.0f; // 目标/实际速度低于该值时不输出前馈
|
||||
constexpr float MODE2_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error
|
||||
constexpr float MODE2_UQ_FF_LEARN_ERR_MAX = 375.0f; // 速度误差带内才学习(≈15°/s×25)，防启停瞬态污染表
|
||||
|
||||
// mode3 角度同步 Uq 前馈补偿：单独建表，避免和 mode2 互相污染。
|
||||
constexpr bool MODE3_UQ_FF_ENABLE = true;
|
||||
@@ -166,16 +243,21 @@ constexpr float MODE3_UQ_FF_GAIN = 0.3f; // mode3 速度误差未乘减
|
||||
constexpr float MODE3_UQ_FF_MAX = 3.5f;
|
||||
constexpr float MODE3_UQ_FF_STOP_SPEED = 2.0f; // mode3 内部目标/实际速度低于该值时不输出前馈
|
||||
constexpr float MODE3_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error
|
||||
constexpr float MODE3_UQ_FF_LEARN_ERR_MAX = 15.0f; // 速度误差带内(°/s)才学习，防启停瞬态污染表
|
||||
|
||||
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度 - 角度 P
|
||||
constexpr float vel_angloop_Ki = 5.0f; // 速度 速度 - 角度 I
|
||||
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度 - 角度 D
|
||||
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度 - 角度 D（恢复，走测量微分+低通）
|
||||
|
||||
// mode3 低速/静止附近速度 P 降低，减少速度反馈噪声、齿槽和摩擦导致的来回拉扯。
|
||||
// mode3 低速/静止附近速度内环增益调度（D 保持满值）：
|
||||
// P/I 各自独立地板——I 地板更低，停止瞬间积分充电速率仅为全速的 15%，
|
||||
// 与静摩擦相互作用（到位 hunting：停下瞬间小幅来回抖动）的能量被大幅压缩；
|
||||
// P 地板保持足够刚度，减小突破静摩擦所需的误差。
|
||||
constexpr bool MODE3_LOW_SPEED_P_SCALE_ENABLE = true;
|
||||
constexpr float MODE3_LOW_SPEED_P_MIN_SCALE = 0.2f;
|
||||
constexpr float MODE3_LOW_SPEED_P_MIN_SCALE = 0.5f;
|
||||
constexpr float MODE3_LOW_SPEED_I_MIN_SCALE = 0.15f;
|
||||
constexpr float MODE3_LOW_SPEED_P_LOW = 1.0f;
|
||||
constexpr float MODE3_LOW_SPEED_P_FULL = 5.0f;
|
||||
constexpr float MODE3_LOW_SPEED_P_FULL = 8.0f;
|
||||
|
||||
constexpr float DEFAULT_angle_angloop_p =
|
||||
150.0f / 100 * 12; // 角度 速度 - 角度 P
|
||||
@@ -190,41 +272,26 @@ constexpr float angle_MIT_Kd = 8e-04f;
|
||||
|
||||
constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
|
||||
constexpr float vel_MIT_Ki = 15.0f;
|
||||
constexpr float vel_MIT_Kd = 1e-04f;
|
||||
constexpr float vel_MIT_Kd = 1e-04f; // 速度 MIT D（恢复，走测量微分+低通）
|
||||
|
||||
constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
|
||||
constexpr float current_MIT_Ki = 0.1f;
|
||||
constexpr float current_MIT_Kd = 3e-4f;
|
||||
constexpr float current_MIT_Kd = 0.0f; // 电流 MIT D（置零，同 current_loop_Kd）
|
||||
// 常量定义
|
||||
constexpr u8 SIZE_FLOAT = sizeof(float);
|
||||
constexpr u8 SIZE_U32 = sizeof(uint32_t);
|
||||
constexpr float ADC_MAX = 65536.0f;
|
||||
constexpr float PI = 3.14159265358979323846f;
|
||||
constexpr float INV_PI_3 = 3.0f / PI;
|
||||
constexpr float PI_3 = PI / 3.0f;
|
||||
constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
|
||||
constexpr float _3PI_2 = 4.71238898038f;
|
||||
constexpr float SQRT3 = 1.7320508075688772f;
|
||||
constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
|
||||
constexpr float COS_PI_3 = 0.5f;
|
||||
constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
|
||||
constexpr float DEG2RAD = 1.0f / 180.0f * PI;
|
||||
constexpr float RAD2DEG = 180.0f / PI; // 修正 RAD2DEG 定义，应该是 180/π
|
||||
constexpr u8 DEFAULT_VALUE = 0;
|
||||
constexpr u8 CONFIG_SAVED_NUM = 6;
|
||||
|
||||
// 三角函数快速计算 LUT
|
||||
constexpr u16 LUT_SIZE = 1024;
|
||||
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
|
||||
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
|
||||
|
||||
// CAN 相关
|
||||
// 注意：以下为 CAN 基础 ID（只保留低 16 位），实际发送/接收时会自动叠加节点 ID
|
||||
// 到高 16 位 格式：[节点 ID(16 位)][基础 ID(16 位)] 例如：节点 ID=1，发送命令
|
||||
// ID = (1 << 16) | CANFD_CMD_ID = 0x00010200
|
||||
constexpr u16 DEV_CAN0_ID = 0x0000;
|
||||
constexpr u16 DEV_CAN0_MASK = 0x0000;
|
||||
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
|
||||
// 反馈基础 ID
|
||||
constexpr u16 CANFD_STATUS_ID = 0x0100; // 状态反馈基础 ID
|
||||
constexpr u16 CANFD_CONFIG_ID = 0x0101; // 配置信息基础 ID
|
||||
@@ -238,14 +305,10 @@ constexpr u16 CANFD_GET_NODE_ID = 0x0401; // 查询节点 ID 基础 ID
|
||||
|
||||
// OTA 相关基础 ID 不受到node ID影响
|
||||
constexpr u16 CANFD_BOOT_ID = 0x0000; // BOOT 模式基础 ID (进入 BOOT 使用此 ID)
|
||||
constexpr u16 CANFD_OTA_REQ_ID = 0x0100; // OTA 请求基础 ID
|
||||
constexpr u16 CANFD_OTA_DATA_ID = 0x0101; // OTA 数据基础 ID
|
||||
constexpr u16 CANFD_OTA_ACK_ID = 0x0102; // OTA 确认基础 ID
|
||||
constexpr u16 CANFD_OTA_WAIT_ID = 0x0103; // OTA 等待基础 ID
|
||||
|
||||
// ADC
|
||||
constexpr u8 ADC_FILTER_SIZE = 6;
|
||||
constexpr u8 BLOCK_SIZE = ADC_FILTER_SIZE * SIZE_FLOAT;
|
||||
constexpr float ERROR_ADC_VAL = 255.0f;
|
||||
constexpr float MAX_TEMP = 150.0f;
|
||||
constexpr float MIN_TEMP = -40.0f;
|
||||
@@ -255,12 +318,10 @@ constexpr float MAX_VCC = 65.0f;
|
||||
constexpr float MIN_VCC = -0.1f;
|
||||
|
||||
// ang_sensor
|
||||
constexpr u16 VEL_FILTER_SIZE = 4;
|
||||
constexpr u16 DEFAULT_COMMAND = 0x0000;
|
||||
constexpr u16 FAULT_RECEIVE = 0xFFFF;
|
||||
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
|
||||
constexpr float ANGLE_WRAP_VALUE = 360.0f;
|
||||
constexpr float RAD_FACTOR = 1.0f / ADC_MAX * 2.0f * PI;
|
||||
constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
|
||||
|
||||
// TIMER2-任务调度
|
||||
@@ -269,7 +330,6 @@ constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us
|
||||
constexpr int MOTOR_PERIOD = 1; // 控制每50us执行一次，无分频
|
||||
constexpr float TIME_SLICE =
|
||||
(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
|
||||
constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率
|
||||
constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
|
||||
|
||||
// TIMER1-PWM
|
||||
|
||||
+145
-73
@@ -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();
|
||||
|
||||
+6
-10
@@ -7,6 +7,8 @@
|
||||
#ifndef __FLASH_H
|
||||
#define __FLASH_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用
|
||||
#include <cstdint>
|
||||
|
||||
@@ -56,7 +58,6 @@ class FlashEEPROM {
|
||||
static constexpr u8 ADDR_NODE_ID = 0x06; // 节点 ID 存储地址
|
||||
static constexpr u8 ADDR_CALIBRATION_ANGLE = 0x07; // 电角度零位校准角存储地址
|
||||
|
||||
bool angle_saved = false;
|
||||
bool system_config_saved = false;
|
||||
u8 EEPROMStatus;
|
||||
// 构造函数
|
||||
@@ -120,11 +121,6 @@ class FlashEEPROM {
|
||||
* @return u8 返回值
|
||||
*/
|
||||
u8 read_saved_system_config(void);
|
||||
/**
|
||||
* @brief 读取保存的角度值
|
||||
* @return float 返回值
|
||||
*/
|
||||
float read_angle();
|
||||
/**
|
||||
* @brief 保存电角度零位校准角到 Flash
|
||||
* @param[in] angle 校准角（度）
|
||||
@@ -132,22 +128,22 @@ class FlashEEPROM {
|
||||
*/
|
||||
FlashEepromStatus save_calibration_angle(float angle);
|
||||
|
||||
// 节点 ID 独立读写函数（使用魔数+双重备份+计数器机制）
|
||||
// 从 Flash 读取节点 ID（带魔数验证和双重备份回退）
|
||||
// 节点 ID 独立读写函数（主/备双备份分属独立 Flash 页 + 魔数 + 计数器 + CRC）
|
||||
// 从 Flash 读取节点 ID（双备份均有效取计数器最新，单份有效则回退）
|
||||
/**
|
||||
* @brief 读取节点 ID 配置
|
||||
* @param[in] node_id 参数
|
||||
* @return FlashEepromStatus 返回值
|
||||
*/
|
||||
FlashEepromStatus readNodeId(u16 *node_id) const;
|
||||
// 保存节点 ID 到 Flash（写入主备份和备用备份）
|
||||
// 保存节点 ID 到 Flash（乒乓写入非最新页，另一页保有旧值，掉电安全）
|
||||
/**
|
||||
* @brief 写入节点 ID 配置
|
||||
* @param[in] node_id 参数
|
||||
* @return FlashEepromStatus 返回值
|
||||
*/
|
||||
FlashEepromStatus writeNodeId(u16 node_id);
|
||||
// 擦除节点 ID 存储区（恢复默认值）
|
||||
// 擦除节点 ID 双备份存储页（恢复默认值）
|
||||
/**
|
||||
* @brief 擦除节点 ID 配置
|
||||
* @return FlashEepromStatus 返回值
|
||||
|
||||
+65
-19
@@ -10,6 +10,8 @@
|
||||
#include "FOC.h"
|
||||
#include "adc.h"
|
||||
#include "flash.h"
|
||||
#include "motor.h"
|
||||
#include <cmath>
|
||||
|
||||
|
||||
extern sampler sampler;
|
||||
@@ -19,11 +21,10 @@ extern FlashEEPROM flashEEP;
|
||||
// 全局热管理控制器单例：由 main 任务周期调用 update()，用于维护过热保护状态。
|
||||
ThermalController thermalController; // 热量控制器初始化
|
||||
|
||||
/** @brief max 宏定义 */
|
||||
#define max(a, b, c) \
|
||||
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
|
||||
/** @brief PROTECT_TIME 常量定义 */
|
||||
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
|
||||
// 三相电流连续全部失效帧数（×10ms）阈值，达到后按过热关机处理
|
||||
constexpr u16 CURRENT_ERROR_SHUTDOWN_COUNT = 100;
|
||||
|
||||
// 电压管理状态机：仅 power_manage() 慢速任务访问，文件内封装。
|
||||
static VoltageState voltage_state = VoltageState::NORMAL;
|
||||
@@ -34,6 +35,14 @@ static float last_vbus_slow = 0.0f; // 上次电压值（慢速任务用）
|
||||
** @retval 热量值
|
||||
*/
|
||||
u32 ThermalController::update() {
|
||||
#if GATE_DRIVER_DRV8323
|
||||
// nFAULT 快速保护（低有效，1ms 轮询）：GDF/UVLO/OTSD 等硬件故障
|
||||
// → 立即封 PWM 并睡眠，锁存到 CAN 重新下发恢复命令为止。
|
||||
// VDS 过流当前配置为"仅上报不动作"，不会拉低 nFAULT，不在此误触发。
|
||||
if (drv_nfault_active()) {
|
||||
FOC.shut_down();
|
||||
}
|
||||
#endif
|
||||
thermal_value = Q16_16_MUL(thermal_value, thermalRate);
|
||||
// 添加温度积累
|
||||
int drv_temp = (int)sampler.get_drv_temp();
|
||||
@@ -60,28 +69,65 @@ u32 ThermalController::update() {
|
||||
return --remainingTime10Ms;
|
||||
} else {
|
||||
// 当前没有过热停机
|
||||
// 添加电流热积累
|
||||
u32 currents[3] = {(u32)(sampler.get_current('U') * 1000),
|
||||
(u32)(sampler.get_current('V') * 1000),
|
||||
(u32)(sampler.get_current('W') * 1000)};
|
||||
// 添加电流热积累：取三相有效相的最大绝对值（热量与电流方向无关，
|
||||
// 负半周同样发热）。采样失效帧(ERROR_ADC_VAL)不参与积累：
|
||||
// 失效值 255 按 mA 参与平方运算会超出 int/u32 范围，热积累不可控；
|
||||
// 三相持续全部失效说明采样链异常，按过热关机 fail-safe。
|
||||
static u16 current_error_frames = 0;
|
||||
const float raw_current[3] = {sampler.get_current('U'),
|
||||
sampler.get_current('V'),
|
||||
sampler.get_current('W')};
|
||||
u32 currents[3] = {0, 0, 0};
|
||||
bool valid[3] = {false, false, false};
|
||||
for (u8 i = 0; i < 3; i++) {
|
||||
if (raw_current[i] == ERROR_ADC_VAL) {
|
||||
continue;
|
||||
}
|
||||
// get_current 已保证有效值 |I| ≤ MAX_CURRENT(40A)，转 mA ≤ 40000
|
||||
currents[i] = (u32)(fabsf(raw_current[i]) * 1000.0f);
|
||||
valid[i] = true;
|
||||
}
|
||||
|
||||
// 找最大电流（原始ADC值）
|
||||
current_max = max(currents[0], currents[1], currents[2]);
|
||||
if (!(valid[0] || valid[1] || valid[2])) {
|
||||
if (++current_error_frames >= CURRENT_ERROR_SHUTDOWN_COUNT) {
|
||||
current_error_frames = 0;
|
||||
FOC.shut_down_thermal();
|
||||
remainingTime10Ms = PROTECT_TIME;
|
||||
CAN0_transmiter.ThermalStatus = 1;
|
||||
return remainingTime10Ms;
|
||||
}
|
||||
return thermal_value;
|
||||
}
|
||||
current_error_frames = 0;
|
||||
|
||||
// 找最大电流（mA，绝对值）；个别相失效时用剩余有效相
|
||||
u32 max_current = 0;
|
||||
for (u8 i = 0; i < 3; i++) {
|
||||
if (valid[i] && currents[i] > max_current) {
|
||||
max_current = currents[i];
|
||||
}
|
||||
}
|
||||
current_max = (u16)max_current;
|
||||
// 平方运算先转 float：40A 档 40000²×10 超 int 范围，整数运算会溢出
|
||||
const float imax = (float)current_max;
|
||||
// 电流系数按"绝对值全周期"口径标定：取 fabs 后正负半周都积累，
|
||||
// 交流负载下积累速度约为旧版(仅正半周)的 2 倍，故系数统一取旧值一半，
|
||||
// 保持原实测触发时机不变；温度积累与恢复阈值不受此补偿影响。
|
||||
if (current_max < 2000) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.00007f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.000035f); // 计算当前电流热积累值
|
||||
} else if (current_max > 3800) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.01f; // 添加当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.005f); // 添加当前电流热积累值
|
||||
} else if (current_max > 3400) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.005f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.0025f); // 计算当前电流热积累值
|
||||
} else if (current_max > 3000) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.0002f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.0001f); // 计算当前电流热积累值
|
||||
} else if (current_max > 2000) {
|
||||
thermal_value += current_max * current_max * gapTimeMs *
|
||||
0.0001f; // 计算当前电流热积累值
|
||||
thermal_value += (u32)(imax * imax * gapTimeMs *
|
||||
0.00005f); // 计算当前电流热积累值
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
#ifndef _HEALTH_H
|
||||
#define _HEALTH_H
|
||||
|
||||
|
||||
#include "config.h"
|
||||
#include <cstdint>
|
||||
typedef int32_t q16_16_t; // Q16.16格式
|
||||
// 定点数运算宏
|
||||
@@ -36,7 +38,6 @@ enum class VoltageState : uint8_t {
|
||||
*/
|
||||
class ThermalController {
|
||||
private:
|
||||
u8 temperature;
|
||||
u16 current_max;
|
||||
|
||||
q16_16_t thermalRate; // 热耗散系数（每单位时间散热比例）
|
||||
|
||||
@@ -231,9 +231,6 @@ void VersionManager::printVersions() const {
|
||||
versionToString(getBootVersion(), versionStr, sizeof(versionStr)));
|
||||
}
|
||||
|
||||
const char* VersionManager::getAppBuildTime() const {
|
||||
return APP_BUILD_DATE " " APP_BUILD_TIME;
|
||||
}
|
||||
|
||||
/* retarget the C library printf function to the USART */
|
||||
/**
|
||||
@@ -286,47 +283,6 @@ void unused_pin_init() {
|
||||
gpio_bit_reset(GPIOC, GPIO_PIN_14 | GPIO_PIN_15);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 检测并打印复位源信息
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
u16 get_reset_cause(void) {
|
||||
u16 cause = 0;
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_FWDGTRST)) {
|
||||
// OTA_DEBUG_PRINT("Free Watchdog Timer Reset\r\n");
|
||||
cause = RCU_FLAG_FWDGTRST;
|
||||
}
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_EPRST)) {
|
||||
// OTA_DEBUG_PRINT("External Pin Reset\r\n");
|
||||
cause = RCU_FLAG_EPRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_PORRST)) {
|
||||
// OTA_DEBUG_PRINT("Power Reset\r\n");
|
||||
cause = RCU_FLAG_PORRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_WWDGTRST)) {
|
||||
// OTA_DEBUG_PRINT("Window Watchdog Timer Reset\r\n");
|
||||
cause = RCU_FLAG_WWDGTRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_LPRST)) {
|
||||
// OTA_DEBUG_PRINT("Low Power Reset\r\n");
|
||||
cause = RCU_FLAG_LPRST;
|
||||
}
|
||||
|
||||
if (RESET != rcu_flag_get(RCU_FLAG_SWRST)) {
|
||||
// OTA_DEBUG_PRINT("Software Reset\r\n");
|
||||
cause = RCU_FLAG_SWRST;
|
||||
}
|
||||
// 清除所有复位标志
|
||||
rcu_all_reset_flag_clear();
|
||||
// OTA_DEBUG_PRINT("cause:%d\r\n", cause);
|
||||
return cause;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化 OTA 与 A/B 分区状态
|
||||
|
||||
@@ -59,11 +59,6 @@ u8 USART0_init(void);
|
||||
* @return u8 返回值
|
||||
*/
|
||||
u8 watchdog_init(u32 time);
|
||||
/**
|
||||
* @brief 获取复位原因
|
||||
* @return u16 返回值
|
||||
*/
|
||||
u16 get_reset_cause(void);
|
||||
/**
|
||||
* @brief 初始化未使用引脚状态
|
||||
* @return void
|
||||
@@ -155,8 +150,6 @@ class VersionManager {
|
||||
char *versionToString(const VersionInfo &version, char *buffer,
|
||||
size_t bufSize) const;
|
||||
|
||||
// 获取编译时间
|
||||
const char* getAppBuildTime() const;
|
||||
|
||||
// 打印所有版本信息
|
||||
/**
|
||||
|
||||
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "gd32c10x.h"
|
||||
#include "gd32c10x_fmc.h"
|
||||
#include "motor.h"
|
||||
#include "tasks.h"
|
||||
#include <cstdio>
|
||||
|
||||
// SLEEP 调试宏定义
|
||||
@@ -149,18 +150,8 @@ extern "C" void SysTick_Handler(void) {
|
||||
|
||||
busy_cycles_accum = 0;
|
||||
sample_seconds = 0;
|
||||
}
|
||||
}
|
||||
/*!
|
||||
\brief delay a time in seconds
|
||||
\param[in] count: count in seconds
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
void delay_1s(uint32_t count) {
|
||||
uint32_t i;
|
||||
for (i = 0; i < count; i++) {
|
||||
delay_1ms(1000);
|
||||
// 按任务负载统计与全局口径同步：同一秒窗口
|
||||
task_busy_cycles_reset();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -59,13 +59,6 @@ uint32_t get_systick_ms(void);
|
||||
* @return void
|
||||
*/
|
||||
void delay_1ms(uint32_t count);
|
||||
/* delay a time in seconds */
|
||||
/**
|
||||
* @brief 秒级阻塞延时
|
||||
* @param[in] count 参数
|
||||
* @return void
|
||||
*/
|
||||
void delay_1s(uint32_t count);
|
||||
/* delay decrement */
|
||||
/**
|
||||
* @brief 递减延时计数器
|
||||
|
||||
Reference in New Issue
Block a user