From 59f161bd73bbd7777a4423724c99736069c4a9ac Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sat, 22 Nov 2025 23:59:43 +0800 Subject: [PATCH] =?UTF-8?q?refactor(angle):=20=E5=BC=95=E5=85=A5=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E5=8D=A1=E5=B0=94=E6=9B=BC=E6=BB=A4=E6=B3=A2=E5=99=A8?= =?UTF-8?q?=20EncoderAngleKalman=20=E6=96=B0=E5=A2=9E=20EncoderAngleKalman?= =?UTF-8?q?=20=E7=B1=BB=E4=BB=A5=E5=A4=84=E7=90=86=E7=BC=96=E7=A0=81?= =?UTF-8?q?=E5=99=A8=E8=A7=92=E5=BA=A6=E5=99=AA=E5=A3=B0=EF=BC=8C=E6=94=B9?= =?UTF-8?q?=E5=96=84=E8=A7=92=E5=BA=A6=E5=8F=8D=E9=A6=88=E8=B4=A8=E9=87=8F?= =?UTF-8?q?=EF=BC=8C=E5=B9=B6=E9=85=8D=E5=90=88=E9=80=9F=E5=BA=A6=E6=BB=A4?= =?UTF-8?q?=E6=B3=A2=E5=99=A8=E6=8F=90=E4=BE=9B=E6=9B=B4=E5=87=86=E7=A1=AE?= =?UTF-8?q?=E7=9A=84=E9=80=9F=E5=BA=A6=E4=BC=B0=E7=AE=97=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit perf(CAN): 优化CAN数据打包与状态标识字段 改进 pack_u16_to_bytes 等函数实现方式，使用 reinterpret_cast 提高效率； 扩展 Status_code 为多个独立状态位，增强诊断能力。 feat(FOC): 添加电机锁止和停止功能 新增 drv_stop 和 drv_hold 方法用于控制电机占空比，支持紧急制动与角度保持。优化了FOC控制器的接口设计，提升安全性和可控性。 fix(filter): 修正中值滤波器数组大小动态分配问题 将 medium_filter 函数中的排序数组改为使用传入参数 size 动态定义，避免固定大小带来的资源浪费或越界风险，提高滤波精度和稳定性。 chore(project): 移除冗余文件及代码清理 移除了 thermal.cpp/h 文件及相关引用，调整项目结构，删除未使用的全局变量定义、注释和头文件包含，精简系统初始化流程。 --- Robot_V2/Algorithm/DengFOC.cpp | 20 +++++- Robot_V2/Algorithm/DengFOC.h | 6 +- Robot_V2/Algorithm/Filter.cpp | 123 +++++++++++++++++++++------------ Robot_V2/Algorithm/Filter.h | 43 +++++++----- Robot_V2/Algorithm/thermal.cpp | 41 ----------- Robot_V2/Algorithm/thermal.h | 20 ------ Robot_V2/Robot_V2.cproject.yml | 4 +- Robot_V2/device/ADC.cpp | 2 +- Robot_V2/device/Can.cpp | 15 ++-- Robot_V2/device/Can.h | 45 +++++------- Robot_V2/device/Timer.cpp | 17 ++--- Robot_V2/device/ang_sensor.cpp | 13 ++-- Robot_V2/main.cpp | 42 +++++------ Robot_V2/sys/health.cpp | 58 ++++++++++++++++ Robot_V2/sys/health.h | 29 ++++++++ Robot_V2/sys/initial.cpp | 7 +- Robot_V2/sys/initial.h | 2 +- Robot_V2/sys/systick.cpp | 20 ++++++ Robot_V2/sys/systick.h | 2 + 19 files changed, 310 insertions(+), 199 deletions(-) create mode 100644 Robot_V2/sys/health.cpp create mode 100644 Robot_V2/sys/health.h diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 7a1bac0..0e7000d 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -20,6 +20,7 @@ constexpr float DIR = 1.0; constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值 constexpr float zero_electric_angle = 0.0f; +FOC_Controlroller FOC; // FOC控制初始化 extern sampler sampler; extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; @@ -93,6 +94,22 @@ void FOC_Controlroller::shut_down() { is_shut_down = 1; gpio_bit_reset(GPIOB, GPIO_PIN_12); } +/** + * @brief 占空比置0 + * + */ +void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); } +/** + * @brief 电机锁止 + */ +void FOC_Controlroller::drv_hold(float duty) { + if (duty > 0.95f) { + duty = 0.95f; + } else if (duty < 0.0f) { + duty = 0.0f; + } + Set_3Pwm(duty, duty, duty); +} /** * @brief 电机恢复 * @@ -113,8 +130,7 @@ void FOC_Controlroller::update_power(void) { voltage_error = false; v_half = voltage_power_supply * 0.5f; v_safe = voltage_power_supply * 0.95f; - } - else { + } else { voltage_error = true; } } diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index 2b77a14..89e9471 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -28,6 +28,8 @@ class FOC_Controlroller { void DFOC_alignSensor(int _PP); void shut_down(void); void resume(void); + void drv_stop(void); + void drv_hold(float duty = 0.5f); protected: std::u64 last_time_us = 0; @@ -42,9 +44,9 @@ class FOC_Controlroller { // voltage_power_supply};//8V // PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000, // voltage_power_supply}; // 8V - PIDController vel_loop = PIDController{1.0f, 0.85f, 0.0002f, 1000000, + PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000, voltage_power_supply}; // 12V - PIDController angle_loop = PIDController{70.0f, 75.0f, 0.0001f, 10000000, + PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000, voltage_power_supply}; // 12V void setPhaseVoltage(float Uq, float Ud); diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index dcae90f..c234abf 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -1,53 +1,14 @@ #include "Filter.h" +#include "Can.h" #include "ang_sensor.h" +#include "gd32c10x_can.h" #include -#include -#include #include #include - extern angle_sensor KTH7823; - -EncoderVelocityKalman Vel_kalman(KTH7823.getVelocity(), 1e-5f, 1e-6f, 0.05f); - -// 队列实现 -void CircularBuffer::enqueue(const float value) { - buffer[head] = value; - head = (head + 1) % HISTORY_SIZE; - if (count < HISTORY_SIZE) - count++; -} - -/*! - \brief 获取操作 - O(1) - \param[out] float 最新的元素 - \retval 最新的元素值 - \note 注意：此操作不删除元素，只是返回最新的元素 -*/ -float CircularBuffer::get(size_t index) const { - if (index >= count) - return buffer[head]; // 返回最新元素 - size_t pos = (head + HISTORY_SIZE - 1 - index) % HISTORY_SIZE; - return buffer[pos]; -} - -/*! - \brief 获取最新的5个数值 - \param[out] latest_values: 指向存储最新5个数值的数组的指针 - \retval none - \note 数组必须至少能容纳5个float元素 -*/ -void CircularBuffer::getLatestFive(float *latest_values) const { - for (size_t i = 0; i < 5 && i < count; ++i) { - latest_values[i] = get(i); - } - - // 如果历史数据不足5个，用同样的数字填充剩余位置 - for (size_t i = count; i < 5; ++i) { - latest_values[i] = latest_values[count - 1]; - } -} +extern CAN_transmiter CAN0_transmiter; +constexpr u8 SIZE_FLOAT = sizeof(float); /*! \brief 中值滤波器函数 @@ -58,8 +19,8 @@ void CircularBuffer::getLatestFive(float *latest_values) const { */ float medium_filter(float values[], u8 size) { // 使用局部数组避免动态内存分配 - float sorted[HISTORY_SIZE]; - memcpy(sorted, values, size * sizeof(float)); + float sorted[size]; + memcpy(sorted, values, size * SIZE_FLOAT); // 这比完整排序更高效，时间复杂度从O(n log n)降低到O(n) std::nth_element(sorted, sorted + size / 2, sorted + size); @@ -168,4 +129,76 @@ void EncoderVelocityKalman::setProcessNoise(float q) { */ void EncoderVelocityKalman::setMeasurementNoise(float r) { kalman.setMeasurementNoise(r); +} + +// EncoderAngleKalman类的实现 +/*! + \brief 构造函数，初始化编码器角度卡尔曼滤波器参数 + \param[in] initial_angle 初始角度值 (degree) + \param[in] initial_error_covariance 初始误差协方差 + \param[in] process_noise 过程噪声协方差 + \param[in] measurement_noise 测量噪声协方差 + \retval none +*/ +EncoderAngleKalman::EncoderAngleKalman(float initial_angle, + float initial_error_covariance, + float process_noise, + float measurement_noise) + : kalman(initial_angle, initial_error_covariance, process_noise, + measurement_noise), + last_angle(initial_angle), initialized(true) {} + + +/*! + \brief 更新卡尔曼滤波器状态 + \param[in] measured_angle 测量的角度值 (degree) + \retval 估计的角度值 (degree) + \note 实现卡尔曼滤波的预测和更新步骤，处理角度环绕问题 +*/ +float EncoderAngleKalman::update(float measured_angle) { + // 更新last_angle以保持同步 + last_angle = kalman.getState(); + // 1. 预测步骤 - 假设角度符合随机游走模型（角速度为0） + // 由于角度变化相对缓慢，我们可以简单预测角度保持不变 + kalman.predict(kalman.getState()); + + // 2. 更新步骤 - 使用处理后的角度值更新 + float filtered_angle = kalman.update(measured_angle); + + + return filtered_angle; +} + +/*! + \brief 获取当前角度估计值 + \retval 当前角度估计值 (degree) +*/ +float EncoderAngleKalman::getAngle() const { return kalman.getState(); } +/*! + \brief 获取当前角度估计值 + \retval 当前角度估计值 (degree) +*/ +float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*1e4; } +/*! + \brief 获取前一个角度估计值 + \retval 前一个角度估计值 (degree) +*/ +float EncoderAngleKalman::getlastAngle() const { return last_angle; } + +/*! + \brief 设置过程噪声协方差 + \param[in] q 过程噪声协方差值 + \retval none +*/ +void EncoderAngleKalman::setProcessNoise(float q) { + kalman.setProcessNoise(q); +} + +/*! + \brief 设置测量噪声协方差 + \param[in] r 测量噪声协方差值 + \retval none +*/ +void EncoderAngleKalman::setMeasurementNoise(float r) { + kalman.setMeasurementNoise(r); } \ No newline at end of file diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index 022c9dd..9d8dbc5 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -18,21 +18,6 @@ extern "C" { #include #include -constexpr u8 HISTORY_SIZE = 10; // 历史数据大小 - -// 环形缓冲区类，用于存储历史数据 -class CircularBuffer { - private: - std::array buffer; - size_t head = 0; - size_t count = 0; - - public: - void enqueue(float value); - float get(size_t index) const; - void getLatestFive(float *latest_values) const; -}; - float medium_filter(float values[], u8 size); /*! @@ -125,7 +110,7 @@ template class KalmanFilter { \note 基于电机模型的卡尔曼滤波器实现，用于电流状态估计 \note // 创建卡尔曼滤波器实例 - CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); + CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.02f, 1e-6f, 0.02f); // 更新滤波器状态并获取估计值 float estimatedCurrent = kalman.update(measuredCurrent, appliedVoltage); */ @@ -171,6 +156,32 @@ class EncoderVelocityKalman { void setMeasurementNoise(float r); }; +/*! + \brief 编码器角度卡尔曼滤波器类 + \note 用于滤除编码器角度测量噪声的卡尔曼滤波器实现 +*/ +class EncoderAngleKalman { +private: + KalmanFilter kalman; + float last_angle; + bool initialized; + +public: + EncoderAngleKalman(float initial_angle, float initial_error_covariance, + float process_noise, float measurement_noise); + + float update(float measured_angle); + + float getAngle() const; + float getlastAngle() const; + float getVelocity() const; + + + void setProcessNoise(float q); + + void setMeasurementNoise(float r); +}; + #endif // __cplusplus #endif // _FILTER_H \ No newline at end of file diff --git a/Robot_V2/Algorithm/thermal.cpp b/Robot_V2/Algorithm/thermal.cpp index 709769e..e69de29 100644 --- a/Robot_V2/Algorithm/thermal.cpp +++ b/Robot_V2/Algorithm/thermal.cpp @@ -1,41 +0,0 @@ -#include "thermal.h" -#include "Can.h" -#include "DengFOC.h" -#include "adc.h" - -extern sampler sampler; -extern CAN_transmiter CAN0_transmiter; -extern FOC_Controlroller FOC; -#define SQUARE(x) ((x) * (x)) -#define max(a, b, c) \ - ((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c))) - -constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms -u32 ThermalController::update() { - thermal_value = thermal_value * thermalRate; - thermal_value += - SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp()); - - if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热 - if (thermal_value < thermal_capacity * recoveryRate) { - remainingTime10Ms = 1; // 可以恢复工作 - } - if (remainingTime10Ms == 1) { // 过热恢复 - FOC.resume(); - return --remainingTime10Ms; - } - return --remainingTime10Ms; - } else { // 当前没有过热停机 - current_max = - 1000 * max(sampler.get_current('U'), sampler.get_current('V'), - sampler.get_current('W')); // 获取电流最大值 - thermal_value += - SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值 - } - if (thermal_value > thermal_capacity) { - FOC.shut_down(); - remainingTime10Ms = PROTECT_TIME; // 过热停机10秒 - return remainingTime10Ms; - } - return thermal_value; -} \ No newline at end of file diff --git a/Robot_V2/Algorithm/thermal.h b/Robot_V2/Algorithm/thermal.h index db05cec..ed7041f 100644 --- a/Robot_V2/Algorithm/thermal.h +++ b/Robot_V2/Algorithm/thermal.h @@ -1,27 +1,7 @@ #ifndef THERMAL_H #define THERMAL_H -#include "stdint.h" -class ThermalController { - private: - u8 temperature; - u16 current_max; - float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例） - float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作） - - u32 thermal_value = 0; - u32 thermal_capacity = 2250000; // 热容量 - - u16 gapTimeMs = 10; // 单次计算间隔ms - u16 remainingTime10Ms = 0; // 过热冷却剩余ms - public: - u32 update(); - void reset() { - thermal_value = 0; - remainingTime10Ms = 0; - } -}; #endif \ No newline at end of file diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index c3ff611..ce727a6 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -10,6 +10,8 @@ project: - file: sys/systick.h - file: sys/initial.cpp - file: sys/initial.h + - file: sys/health.h + - file: sys/health.cpp - group: device files: - file: device/Can.cpp @@ -29,8 +31,6 @@ project: - file: Algorithm/DengFOC.h - file: Algorithm/pid.cpp - file: pid.h - - file: Algorithm/thermal.cpp - - file: Algorithm/thermal.h components: - component: ARM::CMSIS:CORE - component: Device:Startup diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 374ee10..8c797fc 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -10,7 +10,6 @@ constexpr float ERROR_ADC_VAL = 255.0f; constexpr float ADC_SCALE = 3.3f / 4096.0f; constexpr float TEMP_FACTOR = 1000.0f / 4.3f; constexpr float VCC_FACTOR = 11.0f; -// constexpr float CURRENT_OFFSET = 0 - 4096.0f / 2.0f; constexpr float CURRENT_OFFSET = -1.60f; constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA @@ -21,6 +20,7 @@ constexpr float MIN_CURRENT = -40.0f; constexpr float MAX_VCC = 65.0f; constexpr float MIN_VCC = -0.1f; extern CAN_transmiter CAN0_transmiter; +sampler sampler; // 采样器初始化 u16 adc_value[6] = {0}; // 显式初始化数组元素为0 /*! diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 16642c1..dc362ac 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -20,7 +20,7 @@ constexpr u16 DEV_CAN0_MASK = 0x0000; constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000; constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f; -extern CAN_transmiter CAN0_transmiter; +CAN_transmiter CAN0_transmiter; // CAN初始化 extern FOC_Controlroller FOC; extern sampler sampler; extern angle_sensor KTH7823; @@ -432,10 +432,10 @@ void CAN_transmiter::update(void) { mcu_temp = sampler.get_mcu_temp() * 100; motor_temp = sampler.get_drv_temp() * 100; Power_Voltage = sampler.get_vbus_voltage() * 1000; - rt_angle_union.f = KTH7823.getAngle(); + rt_angle_union.f = FOC.rt_angle; // rt_velocity_union.f = KTH7823.getVelocity(); rt_velocity_union.f = FOC.rt_velocity; - Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1; + MotorStatus = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1; Target_Control_Mode = FOC.control_mode; controller_Mode = FOC.control_mode; @@ -591,7 +591,7 @@ u8 CAN_transmiter::CAN_all_Send() { success_flag += CAN_Send(Current_FEED, 6, transmit_data); // 打包Status_FEED - transmit_data[0] = Status_code; + transmit_data[0] = MotorStatus; transmit_data[1] = controller_Mode; pack_u16_to_bytes(transmit_data, 2, motor_temp); pack_u16_to_bytes(transmit_data, 4, mcu_temp); @@ -621,7 +621,11 @@ u8 CAN_transmiter::CANFD_all_Send() { pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f)); pack_u16_to_bytes(transmit_data, 6, u16(w_current_union.f)); pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f)); - transmit_data[10] = Status_code; + transmit_data[10] = (transmit_data[10] & ~0x07) | + ((MotorStatus & 0x01) | + ((MagStatus & 0x01) << 1) | + ((ThermalStatus & 0x01) << 2)); + pack_u16_to_bytes(transmit_data, 11, Power_Voltage); pack_float_to_bytes(transmit_data, 13, rt_angle_union); pack_float_to_bytes(transmit_data, 17, rt_velocity_union); @@ -645,7 +649,6 @@ u8 CAN_transmiter::CANFD_all_Send() { * 1: 有新消息 */ void CAN_transmiter::communication_check() { - uint8_t i = 0; // CAN0 发送数据标志 if (SET == can0_send_flag) { // printf("\r\n can0 %X receive data:", receive_message.rx_efid); diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 727193d..bba05cf 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -17,39 +17,30 @@ typedef union { } float_union_t; // 添加辅助函数用于将u16类型数据打包到u8数组中 -static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) { - buffer[offset] = (u8)(value & 0xFF); - buffer[offset + 1] = (u8)((value >> 8) & 0xFF); +static inline void pack_u16_to_bytes(u8 * const buffer, const u16 offset, const u16 value) { + *(reinterpret_cast(buffer + offset)) = value; } // 添加辅助函数用于将float类型数据打包到u8数组中 -static inline void pack_float_to_bytes(u8 *buffer, u16 offset, - float_union_t &float_union) { - buffer[offset] = float_union.b[0]; - buffer[offset + 1] = float_union.b[1]; - buffer[offset + 2] = float_union.b[2]; - buffer[offset + 3] = float_union.b[3]; +static inline void pack_float_to_bytes(u8 * const buffer, const u16 offset, + const float_union_t &float_union) { + *(reinterpret_cast(buffer + offset)) = *(reinterpret_cast(float_union.b)); } // 添加辅助函数用于从u8数组中解包short类型数据 -static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) { - short value = (short)(buffer[offset] | (buffer[offset + 1] << 8)); - return value; +static inline short unpack_bytes_to_short(const u8 * const buffer, const u16 offset) { + return *(reinterpret_cast(buffer + offset)); } // 添加辅助函数用于从u8数组中解包u16类型数据 -static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) { - u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8)); - return value; +static inline u16 unpack_bytes_to_u16(const u8 * const buffer, const u16 offset) { + return *(reinterpret_cast(buffer + offset)); } // 添加辅助函数用于从u8数组中解包float类型数据 -static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) { - float_union_t float_union; - float_union.b[0] = buffer[offset]; - float_union.b[1] = buffer[offset + 1]; - float_union.b[2] = buffer[offset + 2]; - float_union.b[3] = buffer[offset + 3]; +static inline float_union_t unpack_bytes_to_float(const u8 * const buffer, const u16 offset) { + static float_union_t float_union; + *(reinterpret_cast(float_union.b)) = *(reinterpret_cast(buffer + offset)); return float_union; } @@ -79,7 +70,7 @@ class CAN_transmiter { u16 Power_Voltage = 0; // 电源电压 float_union_t rt_angle_union = {0}; // 实时角度 float_union_t rt_velocity_union = {0}; // 实时速度 - u8 Status_code = 0; // 状态码 + bool MotorStatus = 0; // 电机状态码 float_union_t target_angle_union = {0}; // 目标角度 u8 Target_Control_Mode = 0; // 目标控制模式 float_union_t target_velocity_union = {0}; // 目标速度 @@ -103,12 +94,14 @@ class CAN_transmiter { // 添加构造函数声明 CAN_transmiter(); - can_receive_message_struct receive_message; // 接收消息结构体 - FlagStatus can0_receive_flag; // CAN0接收标志 - FlagStatus can0_send_flag; // CAN0发送标志 + can_receive_message_struct receive_message; // 接收消息结构体 + FlagStatus can0_receive_flag; // CAN0接收标志 + FlagStatus can0_send_flag; // CAN0发送标志 float_union_t testfloat_union = {0}; // 测试用 - void communication_check(); // 通信检查 + bool MagStatus = 0; // 磁状态码 + bool ThermalStatus = 0; // 热能状态码 + void communication_check(); // 通信检查 }; #ifdef __cplusplus diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index b871816..ec34b61 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -1,4 +1,5 @@ #include "Timer.h" +#include "Can.h" #include "DengFOC.h" #include "Timer.h" #include "gd32c10x.h" @@ -28,6 +29,7 @@ static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1 extern u16 adc_value[7]; extern FOC_Controlroller FOC; extern TaskScheduler scheduler; +extern CAN_transmiter CAN0_transmiter; // 添加系统运行时间计数器 static volatile std::u64 system_runtime_us = 0; @@ -179,6 +181,7 @@ void TIMER2_IRQHandler(void) { break; } case 4: { + CAN0_transmiter.can0_send_flag = SET; scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT); break; } @@ -198,6 +201,7 @@ void TIMER2_IRQHandler(void) { break; } case 2: { + scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK); break; } } @@ -275,7 +279,7 @@ void timer1_gpio_config(void) { gpio_bit_set(GPIOB, GPIO_PIN_0); gpio_bit_set(GPIOB, GPIO_PIN_12); - /* 配置TIMER1完全重映射 - 应该使用DISABLE，因为我们使用的是默认引脚 */ + /* 配置TIMER1完全重映射*/ gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE); } @@ -358,7 +362,6 @@ extern "C" void TIMER1_IRQHandler(void) { timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3); scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE); - // 在这里添加需要在PWM周期90%位置执行的代码 // 例如触发ADC采样或其他操作 } } @@ -391,10 +394,8 @@ 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) { - timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, - duty_cycle0 * TIMER1_PERIOD); - timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, - duty_cycle1 * TIMER1_PERIOD); - timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2, - duty_cycle2 * TIMER1_PERIOD); + // 直接写寄存器 + 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); } diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 0ce93fd..5206dc5 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -1,11 +1,13 @@ #include "ang_sensor.h" +#include "Filter.h" #include "gd32c10x_dma.h" #include "gd32c10x_gpio.h" #include "gd32c10x_spi.h" +#include "initial.h" #include #include #include -#include "initial.h" + #define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15) @@ -17,12 +19,16 @@ constexpr u16 FAULT_RECEIVE = 0xFFFF; constexpr float VELOCITY_SCALE = 1e4f; constexpr float ANGLE_WRAP_THRESHOLD = 300.0f; constexpr float ANGLE_WRAP_VALUE = 360.0f; -constexpr int SIZE_FLOAT = sizeof(float); +constexpr u8 SIZE_FLOAT = sizeof(float); // DMA传输完成标志 volatile bool spi_dma_tx_complete = false; volatile bool spi_dma_rx_complete = false; +angle_sensor KTH7823; // 角度传感器初始化 +EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f); +EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f); + /** * @brief SPI DMA initialization */ @@ -293,11 +299,10 @@ float angle_sensor::getRawVelocity(void) const { * @brief 获取角度值(原始级) * @return 角度值(原始级) */ -float angle_sensor::getcurrent_sensorangle(void) const { +float angle_sensor::getcurrent_sensorangle(void) const { return current_angle_deg; } - /** * @brief 重置累积角度值 */ diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index cc50189..4b01bf2 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -3,36 +3,35 @@ #include "DengFOC.h" #include "Filter.h" #include "Timer.h" -#include "adc.h" -#include "ang_sensor.h" #include "core_cm4.h" +#include "health.h" #include "initial.h" +#include "sys/health.h" +#include "sys/systick.h" #include "systick.h" -#include "thermal.h" +#include "ang_sensor.h" +#include "adc.h" TaskScheduler scheduler = {0}; -// 全局变量定义 -CAN_transmiter CAN0_transmiter; // CAN初始化 -FOC_Controlroller FOC; // FOC控制初始化 -sampler sampler; // 采样器初始化 -angle_sensor KTH7823; // 角度传感器初始化 -ThermalController thermalController; // 热量控制器初始化 -extern EncoderVelocityKalman Vel_kalman; int main(void) { systick_config(); // 系统时钟初始化 USART0_init(); // 调试串口初始化 User_timer_init(); // Timer初始化 driver_init(); // 电机驱动初始化 + extern FOC_Controlroller FOC; // FOC控制 + extern sampler sampler; // 采样器 + extern EncoderVelocityKalman Vel_kalman; // 速度滤波 + extern EncoderAngleKalman Angle_kalman; // 角度滤波 + extern angle_sensor KTH7823; // 角度传感器 + extern CAN_transmiter CAN0_transmiter; // CAN + extern ThermalController thermalController; // 热量控制器 - delay_1ms(10); - angsens_MG_test(); // 角度传感器磁感应强度测试 - watchdog_init(5000); // 看门狗初始化 - + delay_1ms(5); + angsens_MG_check(); // 角度传感器磁感应强度测试 + watchdog_init(5000); // 看门狗初始化 while (1) { - // delay_1ms(100); fwdgt_counter_reload(); // 看门狗复位 - // std::printf("1\n"); // 保存当前任务标志快照以提高效率并确保一致性 u32 currentTasks = scheduler.taskFlags; @@ -46,15 +45,16 @@ int main(void) { } if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值 KTH7823.update(); - FOC.rt_angle = KTH7823.getAngle(); + FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()); } if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值 - FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity()); + FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity()); } if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) switch (FOC.control_mode) { case 0: { + FOC.drv_hold(0.95); // 占空比置0 break; } case 1: { @@ -83,14 +83,16 @@ int main(void) { sampler.update_drv_temp(); } if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制 - CAN0_transmiter.testfloat_union.f = thermalController.update(); + thermalController.update(); + } + if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测 + angsens_MG_check(); } if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 sampler.update_vbus_voltage(); FOC.update_power(); } if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 - CAN0_transmiter.can0_send_flag = SET; CAN0_transmiter.communication_check(); } } diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp new file mode 100644 index 0000000..e931133 --- /dev/null +++ b/Robot_V2/sys/health.cpp @@ -0,0 +1,58 @@ +#include "health.h" +#include "Can.h" +#include "DengFOC.h" +#include "adc.h" + +extern sampler sampler; +extern CAN_transmiter CAN0_transmiter; +extern FOC_Controlroller FOC; +ThermalController thermalController; // 热量控制器初始化 +#define SQUARE(x) ((x) * (x)) +#define max(a, b, c) \ + ((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c))) + +constexpr u16 PROTECT_TIME = 3000; // 过热保护时间，单位*10ms +/** +** @brief 热能计算 +** @param None +** @retval 热量值 +*/ +u32 ThermalController::update() { + thermal_value = thermal_value * thermalRate; + thermal_value += + SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp()); + + if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热 + if (thermal_value < thermal_capacity * recoveryRate) { + remainingTime10Ms = 1; // 可以恢复工作 + } + if (remainingTime10Ms == 1) { // 过热恢复 + FOC.resume(); + CAN0_transmiter.ThermalStatus = 0; + return --remainingTime10Ms; + } + return --remainingTime10Ms; + } else { // 当前没有过热停机 + current_max = + 1000 * max(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); // 获取电流最大值 + thermal_value += + SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值 + } + if (thermal_value > thermal_capacity) { + FOC.shut_down(); + remainingTime10Ms = PROTECT_TIME; // 过热停机 + CAN0_transmiter.ThermalStatus = 1; + return remainingTime10Ms; + } + return thermal_value; +} +/** +** @brief 温度检测 +** @param None +*/ +void angsens_MG_check(void) { + bool MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10); + bool MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11); + CAN0_transmiter.MagStatus = MGH or MGL; +} \ No newline at end of file diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h new file mode 100644 index 0000000..66bbb65 --- /dev/null +++ b/Robot_V2/sys/health.h @@ -0,0 +1,29 @@ +#ifndef _HEALTH_H +#define _HEALTH_H + +#include "stdint.h" +class ThermalController { + private: + u8 temperature; + u16 current_max; + + float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例） + float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作） + + u32 thermal_value = 0; + u32 thermal_capacity = 2250000; // 热容量 + + u16 gapTimeMs = 10; // 单次计算间隔ms + u16 remainingTime10Ms = 0; // 过热冷却剩余ms + public: + u32 update(); + void reset() { + thermal_value = 0; + remainingTime10Ms = 0; + } +}; + +void angsens_MG_check(void); + + +#endif diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index f829dbf..ef129e5 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -3,6 +3,7 @@ #include "ang_sensor.h" #include + void GPIO_config(void) { // 使能GPIOB时钟 rcu_periph_clock_enable(RCU_GPIOB); @@ -31,11 +32,7 @@ u8 watchdog_init(u32 time) { return status; } -void angsens_MG_test(void) { - u8 MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10); - u8 MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11); - std::printf("MGH:%d MGL:%d\r\n", MGH, MGL); -} + /** * @brief driver输出初始化 */ diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index ac00eca..38ea28c 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -21,7 +21,6 @@ u8 tle5210b_init(void); u8 USART0_init(void); u8 watchdog_init(u32 time); void driver_init(void); -void angsens_MG_test(void); // 任务标志定义 typedef enum { TASK_MOTOR_CONTROL = 0, @@ -33,6 +32,7 @@ typedef enum { TASK_VBUS_UPDATE, TASK_THERMAL_CONTROL, TASK_CAN_TRANSMIT, + TASK_MAGNETIC_CHECK, TASK_COUNT } SystemTask; diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index 2731f85..cab013a 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -94,4 +94,24 @@ void delay_decrement(void) if (0U != delay){ delay--; } +} + +/*! + \brief system soft reset + \param[in] none + \param[out] none + \retval none +*/ +void system_soft_reset(void) +{ + /* Set the software reset bit */ + SCB->AIRCR = (0x05FA0000U | (SCB->AIRCR & (0x700U)) | (1U << 2)); + + /* Data Synchronization Barrier */ + __DSB(); + + /* Wait for reset */ + while (1){ + __NOP(); + } } \ No newline at end of file diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index 2c0f4e3..5294ccb 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -52,6 +52,8 @@ void delay_decrement(void); void idle_task_count(void); /* 获取CPU使用率 */ uint8_t get_cpu_usage(void); +/* 系统软复位 */ +void system_soft_reset(void); #ifdef __cplusplus }