From 93cbf18f1f3f2940da8c6c104ca0c141cf05613b Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Fri, 5 Dec 2025 00:46:19 +0800 Subject: [PATCH 01/16] =?UTF-8?q?feat(FOC):=20=E5=AE=9E=E7=8E=B0=E7=94=B5?= =?UTF-8?q?=E6=B5=81=E7=8E=AF=E6=8E=A7=E5=88=B6=E5=B9=B6=E4=BC=98=E5=8C=96?= =?UTF-8?q?=E7=9B=B8=E5=85=B3=E9=80=BB=E8=BE=91=20=E6=96=B0=E5=A2=9E?= =?UTF-8?q?=E4=BA=86=E7=94=B5=E6=B5=81=E7=8E=AF=E6=8E=A7=E5=88=B6=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=EF=BC=8C=E5=8C=85=E6=8B=AC=E9=80=9A=E8=BF=87=E4=B8=89?= =?UTF-8?q?=E7=9B=B8=E7=94=B5=E6=B5=81=E8=AE=A1=E7=AE=97=20Iq=20=E7=9A=84?= =?UTF-8?q?=20`cal=5FIq=5FId`=20=E6=96=B9=E6=B3=95=EF=BC=8C=E5=B9=B6?= =?UTF-8?q?=E5=9C=A8=20`current=5Fcontrol`=20=E4=B8=AD=E5=AE=9E=E7=8E=B0?= =?UTF-8?q?=E4=BA=86=E5=9F=BA=E4=BA=8E=20PID=20=E7=9A=84=E7=94=B5=E6=B5=81?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E9=80=BB=E8=BE=91=E3=80=82=E5=90=8C=E6=97=B6?= =?UTF-8?q?=E5=AF=B9=E7=94=B5=E5=8E=8B=E8=AE=BE=E7=BD=AE=E3=80=81=E5=8D=A0?= =?UTF-8?q?=E7=A9=BA=E6=AF=94=E8=AE=A1=E7=AE=97=E7=AD=89=E9=83=A8=E5=88=86?= =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E4=BA=86=E8=B0=83=E6=95=B4=E4=B8=8E=E4=BC=98?= =?UTF-8?q?=E5=8C=96=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit refactor(filter): 移除冗余的电流卡尔曼滤波器实现 删除了 `Filter.cpp` 和 `Filter.h` 中关于 `CurrentKalman` 类的冗余代码，统一外部定义，简化结构。 refactor(health): 改进热量控制器算法和数据类型 将热累积计算改为更精确的分段处理方式，并引入 Q16.16 定点数提升运算精度与性能；增加对异常温度值的判断修正。 fix(CAN): 调整电流发送顺序及标志位设置位置 修改 CAN 发送中 V 相与 W 相电流打包顺序，确保通信正确性；调整发送标志设置逻辑至主循环任务内触发。 chore(headers): 清理无用头文件引用和结构调整 移除了多个源文件中不必要的头文件包含（如 ``、``），并对部分全局变量声明做了 extern 处理以避免重复定义问题。 --- Robot_V2/Algorithm/DengFOC.cpp | 58 ++++++++++++++++++++++++-------- Robot_V2/Algorithm/DengFOC.h | 25 ++++++-------- Robot_V2/Algorithm/Filter.cpp | 61 ++++------------------------------ Robot_V2/Algorithm/Filter.h | 47 +++++--------------------- Robot_V2/device/ADC.cpp | 10 +++++- Robot_V2/device/Can.cpp | 6 ++-- Robot_V2/device/Timer.cpp | 2 -- Robot_V2/device/ang_sensor.cpp | 4 +-- Robot_V2/main.cpp | 11 ++++-- Robot_V2/sys/health.cpp | 58 ++++++++++++++++++++++++-------- Robot_V2/sys/health.h | 20 ++++++++--- 11 files changed, 152 insertions(+), 150 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 3580c04..9fa6d01 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -1,6 +1,5 @@ #include "DengFOC.h" #include "Can.h" -#include "Filter.h" #include "Timer.h" #include "adc.h" #include "ang_sensor.h" @@ -27,9 +26,6 @@ extern sampler sampler; extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; -CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); -CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); -CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); // 初始变量及函数定义 #define _constrain(amt, low, high) \ ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) @@ -47,6 +43,27 @@ inline float _normalizeAngle(float angle) { // 取余运算可以用于归一化，列出特殊值例子算便知 return a >= 0 ? a : (a + 2 * PI); } + +// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq) +inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b, + float current_c) { + // 计算电气角度 + float angle_el = _normalizeAngle(shaft_angle * PP); + + // Clarke变换 (使用三相电流) + float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b - + (1.0f / 3.0f) * current_c; + float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c; + + // Park变换 + float sin_el, cos_el; + cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK]; + sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK]; + float I_q = I_beta * cos_el - I_alpha * sin_el; + + return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA +} + /** * @brief 电机控制类构造函数 * @@ -77,15 +94,15 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half; // 计算占空比并限制范围 - float dc_a = _constrain(Ua / v_safe, 0.0f, 1.0f); - float dc_b = _constrain(Ub / v_safe, 0.0f, 1.0f); - float dc_c = _constrain(Uc / v_safe, 0.0f, 1.0f); + duty_a = _constrain(Ua / v_safe, 0.0f, 1.0f); + duty_b = _constrain(Ub / v_safe, 0.0f, 1.0f); + duty_c = _constrain(Uc / v_safe, 0.0f, 1.0f); // 设置PWM输出 float voltage_exchange = limit_voltage / v_safe; - dc_a = dc_a * voltage_exchange; - dc_b = dc_b * voltage_exchange; - dc_c = dc_c * voltage_exchange; - Set_3Pwm(dc_a, dc_b, dc_c); + duty_a = duty_a * voltage_exchange; + duty_b = duty_b * voltage_exchange; + duty_c = duty_c * voltage_exchange; + Set_3Pwm(duty_a, duty_b, duty_c); // std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c); } @@ -94,7 +111,6 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { * */ void FOC_Controlroller::shut_down() { - thermal_uncontrol = 0; is_shut_down = 1; gpio_bit_reset(GPIOB, GPIO_PIN_12); } @@ -119,7 +135,7 @@ void FOC_Controlroller::drv_hold(float duty) { * */ void FOC_Controlroller::resume() { - thermal_uncontrol = false; + over_current = false; voltage_error = false; is_shut_down = false; gpio_bit_set(GPIOB, GPIO_PIN_12); @@ -155,6 +171,8 @@ float FOC_Controlroller::velocity_open_loop() { const float Uq = voltage_power_supply / 2.0f; setPhaseVoltage(Uq, 0.0f); + CAN0_transmiter.testfloat_union.f = + cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); return limit_voltage; } @@ -223,5 +241,19 @@ float FOC_Controlroller::velocity_control(float target_velocity) { shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; // 设置相电压 setPhaseVoltage(Uq, 0.0f); + CAN0_transmiter.testfloat_union.f = + cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); return rt_velocity; +} + +float FOC_Controlroller::current_control(float target_current) { + // PID 控制计算 + float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); + float Uq = current_loop(target_current - Iq); + CAN0_transmiter.testfloat_union.f = Iq; + // 获取电机轴角度 + shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; + // 设置相电压 + setPhaseVoltage(Uq, 0.0f); + return 1; } \ No newline at end of file diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index 446c891..b5dbde9 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -5,7 +5,7 @@ #include class FOC_Controlroller { - + public: FOC_Controlroller(); @@ -16,14 +16,17 @@ class FOC_Controlroller { float target_velocity = 0.0f; float rt_angle = 0.0f; float rt_velocity = 0.0f; - bool thermal_uncontrol = false; + bool over_current = false; bool voltage_error = false; bool is_shut_down = false; 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 velocity_open_loop(); float angle_control(float target_angle); float velocity_control(float target_velocity); + float current_control(float target_current); float angle_control_NPID(float target_angle); void update_power(void); @@ -34,31 +37,25 @@ class FOC_Controlroller { void drv_hold(float duty = 0.5f); protected: - std::u64 last_time_us = 0; - std::u64 now_us = 0; float shaft_angle = 0; - float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0; + float Ualpha, Ubeta = 0; float zero_electric_angle = 0; float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - float v_safe = voltage_power_supply * 0.95f; - // PIDController vel_loop =PIDController{1.8f, 1.2f, 0.0002f, 1000000, - // voltage_power_supply};//8V - // PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000, - // voltage_power_supply}; // 8V + PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000, voltage_power_supply}; // 12V PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000, voltage_power_supply}; // 12V + PIDController current_loop = + PIDController{0.01f, 0.001f, 0, 100000, voltage_power_supply}; // 12V void setPhaseVoltage(float Uq, float Ud); + float cal_Iq_Id(float current_a, float current_b, float current_c); }; float _normalizeAngle(float angle); -void DFOC_Vbus(float power_supply); inline float _electricalAngle(); -float DFOC_M0_Angle(void); - constexpr u16 LUT_SIZE = 1024; constexpr u16 LUT_MASK = (LUT_SIZE - 1); diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index c234abf..ca40507 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -1,13 +1,15 @@ #include "Filter.h" #include "Can.h" #include "ang_sensor.h" -#include "gd32c10x_can.h" #include #include #include extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; +EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f); +EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f); + constexpr u8 SIZE_FLOAT = sizeof(float); /*! @@ -28,56 +30,6 @@ float medium_filter(float values[], u8 size) { return sorted[size / 2]; // 返回中值 } -// CurrentKalman类的实现 -/*! - \brief 构造函数，初始化卡尔曼滤波器参数 - \param[in] initial_current 初始电流值 - \param[in] L 电机电感(H) 0.0026H - \param[in] R 电机电阻(Ω) 4.8 Ω - \param[in] T 控制周期(s) 1e -4 - \param[in] Q 过程噪声协方差(典型值: 1e-6 ~ 1e-4) - \param[in] R_noise 测量噪声协方差(典型值: 电流范围的0.5%~2%) - \retval none -*/ -CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T, - float Q, float R_noise) - : kalman(initial_current, 1.0f, Q, R_noise), L(L), R_m(R), T(T) {} -/*! - \brief 更新卡尔曼滤波器状态 - \param[in] measured_value 测量值 - \param[in] applied_voltage 应用电压值 - \retval 估计的电流值 - \note 实现卡尔曼滤波的预测和更新步骤 -*/ -float CurrentKalman::update(float measured_value, float applied_voltage) { - // 1. 预测步骤 - 基于电机模型 - float predicted_current = kalman.getState() + T * (applied_voltage - R_m * kalman.getState()) / L; - - // 2. 预测误差协方差已经在kalman.predict中处理 - - // 3. 执行预测 - kalman.predict(predicted_current); - - // 4. 更新步骤 - 直接使用测量值更新 - return kalman.update(measured_value); -} -/*! - \brief 获取当前电流估计值 - \retval 当前电流估计值 -*/ -float CurrentKalman::getCurrent() const { return kalman.getState(); } -/*! - \brief 设置过程噪声协方差 - \param[in] q 过程噪声协方差值 - \retval none -*/ -void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); } -/*! - \brief 设置测量噪声协方差 - \param[in] r 测量噪声协方差值 - \retval none -*/ -void CurrentKalman::setMeasurementNoise(float r) { kalman.setMeasurementNoise(r); } // EncoderVelocityKalman类的实现 /*! @@ -109,7 +61,7 @@ float EncoderVelocityKalman::update(float measured_velocity) { /*! \brief 获取当前速度估计值 - \retval 当前速度估计值 (rad/s) + \retval 当前速度估计值 (deg/s) */ float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); } @@ -158,9 +110,8 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle, float EncoderAngleKalman::update(float measured_angle) { // 更新last_angle以保持同步 last_angle = kalman.getState(); - // 1. 预测步骤 - 假设角度符合随机游走模型（角速度为0） - // 由于角度变化相对缓慢，我们可以简单预测角度保持不变 - kalman.predict(kalman.getState()); + // 1. 预测步骤 - 速度估计 + kalman.predict(last_angle); // 2. 更新步骤 - 使用处理后的角度值更新 float filtered_angle = kalman.update(measured_angle); diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index 9d8dbc5..d34cc47 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -1,7 +1,6 @@ #ifndef _FILTER_H #define _FILTER_H -#include #ifdef __cplusplus extern "C" { #endif @@ -14,7 +13,6 @@ extern "C" { #endif #ifdef __cplusplus -#include #include #include @@ -26,7 +24,7 @@ float medium_filter(float values[], u8 size); */ template class KalmanFilter { private: - T x; // 状态估计 + T xp; // 状态估计 T P; // 估计误差协方差 T Q; // 过程噪声协方差 T R; // 测量噪声协方差 @@ -41,7 +39,7 @@ template class KalmanFilter { */ KalmanFilter(T initial_state, T initial_error_covariance, T process_noise, T measurement_noise) - : x(initial_state), P(initial_error_covariance), Q(process_noise), + : xp(initial_state), P(initial_error_covariance), Q(process_noise), R(measurement_noise) {} /*! @@ -50,7 +48,7 @@ template class KalmanFilter { \retval none */ void predict(T state_prediction) { - x = state_prediction; + xp = state_prediction; P += Q; } @@ -65,10 +63,10 @@ template class KalmanFilter { T S = P + R; // 残差协方差 T K = P / S; // 卡尔曼增益 - x += K * y; // 更新状态估计 + xp += K * y; // 更新状态估计 P = (1 - K) * P; // 更新误差协方差 - return x; + return xp; } /*! @@ -79,16 +77,16 @@ template class KalmanFilter { T update(T measurement) { T K = P / (P + R); // 卡尔曼增益 - x += K * (measurement - x); // 更新状态估计 + xp += K * (measurement - xp); // 更新状态估计 P = (1 - K) * P; // 更新误差协方差 - return x; + return xp; } /*! \brief 获取当前状态估计值 \retval 当前状态估计值 */ - T getState() const { return x; } + T getState() const { return xp; } /*! \brief 设置过程噪声协方差 @@ -105,34 +103,6 @@ template class KalmanFilter { void setMeasurementNoise(T measurement_noise) { R = measurement_noise; } }; -/*! - \brief 电流卡尔曼滤波器类 - \note 基于电机模型的卡尔曼滤波器实现，用于电流状态估计 - \note - // 创建卡尔曼滤波器实例 - CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.02f, 1e-6f, 0.02f); - // 更新滤波器状态并获取估计值 - float estimatedCurrent = kalman.update(measuredCurrent, appliedVoltage); -*/ -class CurrentKalman { - private: - KalmanFilter kalman; - float L; // 电机电感(H) - float R_m; // 电机电阻(Ω) - float T; // 控制周期(s) - - public: - CurrentKalman(float initial_current, float L, float R, float T, float Q, - float R_noise); - - float update(float measured_current, float applied_voltage); - - float getCurrent() const; - - void setProcessNoise(float q); - - void setMeasurementNoise(float r); -}; /*! \brief 编码器速度卡尔曼滤波器类 @@ -182,6 +152,7 @@ public: void setMeasurementNoise(float r); }; + #endif // __cplusplus #endif // _FILTER_H \ No newline at end of file diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 32017f1..eb11ec8 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -1,5 +1,6 @@ #include "adc.h" #include "Can.h" +#include "DengFOC.h" #include "Filter.h" #include "math.h" #include @@ -20,8 +21,8 @@ constexpr float MIN_CURRENT = -40.0f; constexpr float MAX_VCC = 65.0f; constexpr float MIN_VCC = -0.1f; extern CAN_transmiter CAN0_transmiter; +extern FOC_Controlroller FOC; sampler sampler; // 采样器初始化 - u16 adc_value[6] = {0}; // 显式初始化数组元素为0 /*! \brief configure the DMA peripheral @@ -171,14 +172,17 @@ float sampler::get_drv_temp() { float sampler::get_current(char type) { switch (type) { case 'U': { + // return currU_samples[current_sample_index]; tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1); break; } case 'V': { + // return currV_samples[current_sample_index]; tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1); break; } case 'W': { + // return currW_samples[current_sample_index]; tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1); break; } @@ -189,6 +193,7 @@ float sampler::get_current(char type) { if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) { return ERROR_ADC_VAL; } + return tempora_sample; } @@ -238,6 +243,7 @@ void sampler::update_drv_temp() { */ void sampler::update_current() { u8 idx = current_sample_index; + extern FOC_Controlroller FOC; // 优化计算 const float scale = ADC_SCALE * CURRENT_FACTOR; const float offset = CURRENT_OFFSET * CURRENT_FACTOR; @@ -251,6 +257,8 @@ void sampler::update_current() { if (current_sample_index >= FILTER_SIZE - 1) { current_sample_index = 0; } + // 卡尔曼滤波更新 + float voltage = FOC.voltage_power_supply; } /** diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 28647eb..cc4a8a0 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -445,10 +445,10 @@ void CAN_transmiter::update(void) { Target_Control_Mode = FOC.control_mode; controller_Mode = FOC.control_mode; + // 使用简单平均滤波的电流值 u_current_union.f = sampler.get_current('U') * 1000; w_current_union.f = sampler.get_current('W') * 1000; v_current_union.f = sampler.get_current('V') * 1000; - // testfloat_union.f = KTH7823.getVelocity(); } /*! @@ -624,8 +624,8 @@ u8 CAN_transmiter::CANFD_all_Send() { pack_u16_to_bytes(transmit_data, 0, mcu_temp); pack_u16_to_bytes(transmit_data, 2, motor_temp); 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)); + pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f)); + pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f)); transmit_data[10] = (transmit_data[10] & ~0x07) | ((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) | diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 431df26..d8a18f1 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -8,7 +8,6 @@ #include "gd32c10x_rcu.h" #include "gd32c10x_timer.h" #include "initial.h" -#include #include #include #include @@ -139,7 +138,6 @@ void TIMER2_IRQHandler(void) { break; } case 9: { - CAN0_transmiter.can0_send_flag = SET; scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT); break; } diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index a82d181..4f39520 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -27,8 +27,8 @@ 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); +extern EncoderAngleKalman Angle_kalman; +extern EncoderVelocityKalman Vel_kalman; /** * @brief SPI DMA initialization diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 0468896..72bb07e 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -12,9 +12,9 @@ #include "sys/health.h" #include "sys/systick.h" #include "systick.h" +#include TaskScheduler scheduler = {0}; - int main(void) { systick_config(); // 系统时钟初始化 USART0_init(); // 调试串口初始化 @@ -28,6 +28,7 @@ int main(void) { extern CAN_transmiter CAN0_transmiter; // CAN extern ThermalController thermalController; // 热量控制器 + u32 cnt_test = 0; delay_1ms(5); angsens_MG_check(); // 角度传感器磁感应强度测试 watchdog_init(5000); // 看门狗初始化 @@ -38,7 +39,7 @@ int main(void) { // 一次性清除所有已处理的任务标志 scheduler.taskFlags &= ~currentTasks; __enable_irq(); - + cnt_test++; // 按照优先级和重要性处理任务 if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 sampler.update_current(); @@ -66,7 +67,8 @@ int main(void) { break; } case 3: { - FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 + // FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 + FOC.current_control(FOC.limit_velocity); // 电流闭环控制 break; } case 4: { @@ -93,6 +95,9 @@ int main(void) { FOC.update_power(); } if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 + // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 + cnt_test = 0; + CAN0_transmiter.can0_send_flag = SET; CAN0_transmiter.communication_check(); fwdgt_counter_reload(); // 看门狗复位 } diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index e931133..4ef4277 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -2,28 +2,38 @@ #include "Can.h" #include "DengFOC.h" #include "adc.h" +#include 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 +constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*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()); + thermal_value = Q16_16_MUL(thermal_value, thermalRate); + // 添加温度积累 + int drv_temp = (int)sampler.get_drv_temp(); + int mcu_temp = (int)sampler.get_mcu_temp(); + if (drv_temp == 255) + drv_temp = 0; + if (mcu_temp == 255) + mcu_temp = 0; + thermal_value += drv_temp * drv_temp + mcu_temp * mcu_temp; - if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热 - if (thermal_value < thermal_capacity * recoveryRate) { + if (remainingTime10Ms) { + // 过热停机状态 + // 检查是否可以恢复 + q16_16_t recovery_threshold = + Q16_16_MUL(thermal_capacity, recoveryRate); + if (thermal_value < recovery_threshold) { remainingTime10Ms = 1; // 可以恢复工作 } if (remainingTime10Ms == 1) { // 过热恢复 @@ -32,13 +42,33 @@ u32 ThermalController::update() { 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; // 计算当前电流热积累值 + } else { + // 当前没有过热停机 + // 添加电流热积累 + u32 currents[3] = {(u32)(sampler.get_current('U') * 1000), + (u32)(sampler.get_current('V') * 1000), + (u32)(sampler.get_current('W') * 1000)}; + + // 找最大电流（原始ADC值） + current_max = max(currents[0], currents[1], currents[2]); + if (current_max < 2000) { + thermal_value += current_max * current_max * gapTimeMs * + 0.00007f; // 计算当前电流热积累值 + } else if (current_max > 3800) { + thermal_value += current_max * current_max * gapTimeMs * + 0.01f; // 添加当前电流热积累值 + } else if (current_max > 3400) { + thermal_value += current_max * current_max * gapTimeMs * + 0.005f; // 计算当前电流热积累值 + } else if (current_max > 3000) { + thermal_value += current_max * current_max * gapTimeMs * + 0.0002f; // 计算当前电流热积累值 + } else if (current_max > 2000) { + thermal_value += current_max * current_max * gapTimeMs * + 0.0001f; // 计算当前电流热积累值 + } } + if (thermal_value > thermal_capacity) { FOC.shut_down(); remainingTime10Ms = PROTECT_TIME; // 过热停机 diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h index 66bbb65..3dfd298 100644 --- a/Robot_V2/sys/health.h +++ b/Robot_V2/sys/health.h @@ -2,20 +2,31 @@ #define _HEALTH_H #include "stdint.h" +typedef int32_t q16_16_t; // Q16.16格式 +// 定点数运算宏 +#define Q16_16_ONE 65536 // 1.0 in Q16.16 +#define FLOAT_TO_Q16_16(x) ((q16_16_t)((x) * Q16_16_ONE)) +#define Q16_16_TO_FLOAT(x) ((float)(x) / Q16_16_ONE) +#define Q16_16_MUL(a, b) ((q16_16_t)(((int64_t)(a) * (b)) >> 16)) +#define Q16_16_SQUARE(x) Q16_16_MUL((x), (x)) + class ThermalController { private: u8 temperature; u16 current_max; - float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例） - float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作） + q16_16_t thermalRate; // 热耗散系数（每单位时间散热比例） + q16_16_t recoveryRate; // 自然恢复率（散热到多少以下开始工作） u32 thermal_value = 0; - u32 thermal_capacity = 2250000; // 热容量 + u32 thermal_capacity = 3600000; // 热容量 - u16 gapTimeMs = 10; // 单次计算间隔ms + u16 gapTimeMs = 10; // 单次计算间隔ms u16 remainingTime10Ms = 0; // 过热冷却剩余ms public: + ThermalController() + : thermalRate(FLOAT_TO_Q16_16(0.995f)), + recoveryRate(FLOAT_TO_Q16_16(0.33f)) {} u32 update(); void reset() { thermal_value = 0; @@ -25,5 +36,4 @@ class ThermalController { void angsens_MG_check(void); - #endif From 839a32ff046c67db44ab46268b46c7557548e6b9 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Mon, 8 Dec 2025 02:48:59 +0800 Subject: [PATCH 02/16] =?UTF-8?q?refactor(main):=20=E7=BB=9F=E4=B8=80?= =?UTF-8?q?=E5=A4=96=E8=AE=BE=E5=88=9D=E5=A7=8B=E5=8C=96=E6=B5=81=E7=A8=8B?= =?UTF-8?q?=20=E9=9B=86=E4=B8=AD=E6=89=80=E6=9C=89=E5=A4=96=E8=AE=BE?= =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96=E4=BA=8Emain=E5=87=BD=E6=95=B0?= =?UTF-8?q?=E8=B5=B7=E5=A7=8B=E5=A4=84=EF=BC=8C=E9=80=9A=E8=BF=87=E8=BF=94?= =?UTF-8?q?=E5=9B=9E=E5=80=BC=E7=A1=AE=E8=AE=A4=E5=90=84=E6=A8=A1=E5=9D=97?= =?UTF-8?q?=E6=98=AF=E5=90=A6=E6=88=90=E5=8A=9F=E5=90=AF=E5=8A=A8=EF=BC=9B?= =?UTF-8?q?=20refactor(adc):=20=E4=BF=AE=E6=94=B9=E9=87=87=E6=A0=B7?= =?UTF-8?q?=E5=99=A8=E5=88=9D=E5=A7=8B=E5=8C=96=E8=BF=94=E5=9B=9E=E5=80=BC?= =?UTF-8?q?=20=E5=B0=86sampler=E6=9E=84=E9=80=A0=E5=87=BD=E6=95=B0?= =?UTF-8?q?=E6=9B=BF=E6=8D=A2=E4=B8=BA=E5=B8=A6=E8=BF=94=E5=9B=9E=E5=80=BC?= =?UTF-8?q?=E7=9A=84init=E6=96=B9=E6=B3=95=EF=BC=8C=E5=9C=A8main=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E4=B8=AD=E5=8A=A0=E5=85=A5=E5=88=9D=E5=A7=8B=E5=8C=96?= =?UTF-8?q?=E7=BB=93=E6=9E=9C=E5=88=A4=E6=96=AD=EF=BC=8C=20=E7=A1=AE?= =?UTF-8?q?=E4=BF=9D=E7=B3=BB=E7=BB=9F=E5=90=AF=E5=8A=A8=E6=97=B6=E7=A1=AC?= =?UTF-8?q?=E4=BB=B6=E9=85=8D=E7=BD=AE=E6=AD=A3=E7=A1=AE=E5=8A=A0=E8=BD=BD?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(motor): 引入motor模块并重构电机控制逻辑 将GPIO操作封装进motor_sleep和motor_resume函数，提升代码可读性和维护性。 引入motor.h头文件，并在main.cpp中进行电机初始化统一管理。 修改FOC控制器中的shutdown与resume方法调用新接口。 refactor(filter): 优化卡尔曼滤波器实现 重命名状态变量xp为x，简化访问逻辑；移除冗余注释及未使用的getlastAngle方法； 调整getVelocity计算系数，提高速度估算精度；增加init方法支持初始状态设置。 refactor(angle_sensor): 移除冗余速度计算功能 删除传感器内部的速度估算相关数组和方法，交由外部滤波器处理以降低耦合度。 更新SPI片选宏定义以区分不同设备，并增强错误处理机制。 refactor(can): 调整CAN通信ID及初始化流程 更改状态与配置帧的标识符ID，添加对特定命令ID的响应处理如电机复位指令。 将CAN_transmiter构造函数改为init方法，配合主程序完成模块化初始化检查。 --- Robot_V2/Algorithm/DengFOC.cpp | 9 ++-- Robot_V2/Algorithm/DengFOC.h | 2 +- Robot_V2/Algorithm/Filter.cpp | 13 ++---- Robot_V2/Algorithm/Filter.h | 36 +++++++++------- Robot_V2/Robot_V2.cproject.yml | 2 + Robot_V2/device/ADC.cpp | 6 ++- Robot_V2/device/Can.cpp | 33 ++++++++------ Robot_V2/device/Can.h | 2 +- Robot_V2/device/adc.h | 2 +- Robot_V2/device/ang_sensor.cpp | 78 +++++++++++++--------------------- Robot_V2/device/ang_sensor.h | 4 +- Robot_V2/device/motor.cpp | 38 +++++++++++++++++ Robot_V2/device/motor.h | 11 +++++ Robot_V2/main.cpp | 42 +++++++++++------- Robot_V2/sys/initial.cpp | 16 ------- Robot_V2/sys/initial.h | 2 - 16 files changed, 164 insertions(+), 132 deletions(-) create mode 100644 Robot_V2/device/motor.cpp create mode 100644 Robot_V2/device/motor.h diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 9fa6d01..00d95e5 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -8,6 +8,7 @@ #include #include #include +#include "motor.h" constexpr double PI = 3.1415926535897932384626433832795f; constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f; @@ -112,7 +113,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { */ void FOC_Controlroller::shut_down() { is_shut_down = 1; - gpio_bit_reset(GPIOB, GPIO_PIN_12); + motor_sleep(); } /** * @brief 占空比置0 @@ -138,7 +139,7 @@ void FOC_Controlroller::resume() { over_current = false; voltage_error = false; is_shut_down = false; - gpio_bit_set(GPIOB, GPIO_PIN_12); + motor_resume(); } /** * @brief 更新电源电压 @@ -235,13 +236,13 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) { */ float FOC_Controlroller::velocity_control(float target_velocity) { // PID 控制计算 - float Uq = vel_loop(target_velocity - rt_velocity); + float Uq = vel_loop((target_velocity - rt_velocity)*25); // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; // 设置相电压 setPhaseVoltage(Uq, 0.0f); - CAN0_transmiter.testfloat_union.f = + // CAN0_transmiter.testfloat_union.f = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); return rt_velocity; } diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index b5dbde9..50e5c12 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -43,7 +43,7 @@ class FOC_Controlroller { float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000, + PIDController vel_loop = PIDController{0.13f, 0.4f, 2.8e-4, 1000000, voltage_power_supply}; // 12V PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000, voltage_power_supply}; // 12V diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index ca40507..f1a658b 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -105,18 +105,16 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle, \brief 更新卡尔曼滤波器状态 \param[in] measured_angle 测量的角度值 (degree) \retval 估计的角度值 (degree) - \note 实现卡尔曼滤波的预测和更新步骤，处理角度环绕问题 + \note 实现卡尔曼滤波的预测和更新步骤 */ float EncoderAngleKalman::update(float measured_angle) { - // 更新last_angle以保持同步 + // 更新last_angle以支持速度计算 last_angle = kalman.getState(); // 1. 预测步骤 - 速度估计 kalman.predict(last_angle); // 2. 更新步骤 - 使用处理后的角度值更新 float filtered_angle = kalman.update(measured_angle); - - return filtered_angle; } @@ -129,12 +127,7 @@ 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; } +float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*3.75e3; } /*! \brief 设置过程噪声协方差 diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index d34cc47..1cd1122 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -24,12 +24,12 @@ float medium_filter(float values[], u8 size); */ template class KalmanFilter { private: - T xp; // 状态估计 T P; // 估计误差协方差 T Q; // 过程噪声协方差 T R; // 测量噪声协方差 public: + T x; // 状态估计 /*! \brief 构造函数 \param[in] initial_state 初始状态值 @@ -39,7 +39,7 @@ template class KalmanFilter { */ KalmanFilter(T initial_state, T initial_error_covariance, T process_noise, T measurement_noise) - : xp(initial_state), P(initial_error_covariance), Q(process_noise), + : x(initial_state), P(initial_error_covariance), Q(process_noise), R(measurement_noise) {} /*! @@ -48,7 +48,7 @@ template class KalmanFilter { \retval none */ void predict(T state_prediction) { - xp = state_prediction; + x = state_prediction; P += Q; } @@ -63,10 +63,10 @@ template class KalmanFilter { T S = P + R; // 残差协方差 T K = P / S; // 卡尔曼增益 - xp += K * y; // 更新状态估计 + x += K * y; // 更新状态估计 P = (1 - K) * P; // 更新误差协方差 - return xp; + return x; } /*! @@ -77,16 +77,16 @@ template class KalmanFilter { T update(T measurement) { T K = P / (P + R); // 卡尔曼增益 - xp += K * (measurement - xp); // 更新状态估计 + x += K * (measurement - x); // 更新状态估计 P = (1 - K) * P; // 更新误差协方差 - return xp; + return x; } /*! \brief 获取当前状态估计值 \retval 当前状态估计值 */ - T getState() const { return xp; } + T getState() const { return x; } /*! \brief 设置过程噪声协方差 @@ -103,7 +103,6 @@ template class KalmanFilter { void setMeasurementNoise(T measurement_noise) { R = measurement_noise; } }; - /*! \brief 编码器速度卡尔曼滤波器类 \note 用于滤除编码器速度测量噪声的卡尔曼滤波器实现 @@ -124,6 +123,11 @@ class EncoderVelocityKalman { void setProcessNoise(float q); void setMeasurementNoise(float r); + + u8 init(float init_velocity) { + kalman.x = init_velocity; + return 0; + } }; /*! @@ -131,28 +135,30 @@ class EncoderVelocityKalman { \note 用于滤除编码器角度测量噪声的卡尔曼滤波器实现 */ class EncoderAngleKalman { -private: + private: KalmanFilter kalman; - float last_angle; bool initialized; -public: + public: + float last_angle; 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); + float getstate() const { return kalman.getState(); }; void setMeasurementNoise(float r); + u8 init(float init_angle) { + kalman.x = init_angle; + return 0; + } }; - #endif // __cplusplus #endif // _FILTER_H \ No newline at end of file diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index ce727a6..9c5425b 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -23,6 +23,8 @@ project: - file: device/Timer.cpp - file: device/Timer.h - file: device/test.cpp + - file: device/motor.cpp + - file: device/motor.h - group: Algorithm files: - file: Algorithm/Filter.cpp diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index eb11ec8..afa3e72 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -6,6 +6,7 @@ #include #include #include +#include "systick.h" constexpr float ERROR_ADC_VAL = 255.0f; constexpr float ADC_SCALE = 3.3f / 4096.0f; @@ -110,9 +111,10 @@ void adc_config(void) { /** * @brief 采样类初始化 */ -sampler::sampler() { +u8 sampler::init() { dma_config(); // DMA采样ADC初始化 adc_config(); // ADC初始化 + delay_1ms(1); // 初始采样 for (int i = 0; i < FILTER_SIZE + 1; i++) { mcu_temp_samples[i] = @@ -130,7 +132,9 @@ sampler::sampler() { float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700; drv_temp_samples[i] = 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f; + delay_1ms(1); } + return 0; } /** * @brief 获取MCU温度 diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index cc4a8a0..ea72bb2 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -6,6 +6,7 @@ #include "gd32c10x.h" #include "gd32c10x_can.h" #include "gd32c10x_dma.h" +#include "motor.h" #include "systick.h" #include #include @@ -21,7 +22,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; -CAN_transmiter CAN0_transmiter; // CAN初始化 +CAN_transmiter CAN0_transmiter; // CAN初始化 extern FOC_Controlroller FOC; extern sampler sampler; extern angle_sensor KTH7823; @@ -260,7 +261,7 @@ void can_loopback_init(void) { can_parameter.prescaler = 6; if (can_init(CANX, &can_parameter) != SUCCESS) { u16 i = 200; - while (--i>0){ + while (--i > 0) { delay_1ms(10); printf("can init error \r\n"); } @@ -413,7 +414,7 @@ void can_loopback_test(void) { \brief CAN_transmiter构造函数 \note 初始化CAN通信相关参数和硬件配置 */ -CAN_transmiter::CAN_transmiter() { +u8 CAN_transmiter::init() { // 初始化CAN硬件 // can_config(); canfd_config(); @@ -426,6 +427,7 @@ CAN_transmiter::CAN_transmiter() { // 初始化DMA缓冲区 memset(can_rx_buffer, 0, sizeof(can_rx_buffer)); memset(can_tx_buffer, 0, sizeof(can_tx_buffer)); + return 0; } /*! @@ -440,7 +442,7 @@ void CAN_transmiter::update(void) { rt_angle_union.f = FOC.rt_angle; // rt_velocity_union.f = KTH7823.getVelocity(); rt_velocity_union.f = FOC.rt_velocity; - MotorStatus = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1; + MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET; Target_Control_Mode = FOC.control_mode; controller_Mode = FOC.control_mode; @@ -618,19 +620,18 @@ u8 CAN_transmiter::CAN_all_Send() { u8 CAN_transmiter::CANFD_all_Send() { u32 success_flag = 0; - constexpr u32 STATUS_ID = 0x18FF0100; - constexpr u32 CONFIG_ID = 0x18FF0101; + constexpr u32 STATUS_ID = 0x00010100; + constexpr u32 CONFIG_ID = 0x00010101; // 打包status数据 pack_u16_to_bytes(transmit_data, 0, mcu_temp); pack_u16_to_bytes(transmit_data, 2, motor_temp); pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f)); pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f)); pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f)); - transmit_data[10] = (transmit_data[10] & ~0x07) | - ((MotorStatus & 0x01) | - ((MagStatus & 0x01) << 1) | - ((ThermalStatus & 0x01) << 2)); - + 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); @@ -669,7 +670,7 @@ void CAN_transmiter::communication_check() { // for (i = 0; i < receive_message.rx_dlen; i++) { // printf(" %02x", receive_message.rx_data[i]); // } - if (receive_message.rx_efid == 0x18ff0200) { + if (receive_message.rx_efid == 0x00010200) { FOC.control_mode = receive_message.rx_data[0]; if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 != FOC.limit_voltage) { @@ -689,9 +690,13 @@ void CAN_transmiter::communication_check() { target_velocity_union.f = unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f; - - can0_receive_flag = RESET; + } else if (receive_message.rx_efid == 0x00010301) { + motor_reset(); } + else { + printf("Unknown CAN ID: %X\n", receive_message.rx_efid); + } + can0_receive_flag = RESET; } } diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 56dc00a..5bc1e74 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -101,7 +101,7 @@ class CAN_transmiter { public: // 添加构造函数声明 - CAN_transmiter(); + u8 init(void); can_receive_message_struct receive_message; // 接收消息结构体 FlagStatus can0_receive_flag; // CAN0接收标志 diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h index 9c126e2..3a65951 100644 --- a/Robot_V2/device/adc.h +++ b/Robot_V2/device/adc.h @@ -21,7 +21,7 @@ class sampler { float tempora_sample; public: - sampler(); + u8 init(void); void update_mcu_temp(void); void update_drv_temp(void); void update_vbus_voltage(void); diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 4f39520..19af117 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -9,9 +9,10 @@ #include #include - -#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) -#define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15) +#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) +#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15) +#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14) +#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14) constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f; constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f; @@ -204,37 +205,39 @@ void angle_sensor::KTH7823_init(void) { /** * @brief 角度传感器构造函数 */ -angle_sensor::angle_sensor(void) { KTH7823_init(); } +u8 angle_sensor::init(void) { + KTH7823_init(); + return 0; +} /** * @brief 读取传感器数据 * @return 传感器数据 */ inline float angle_sensor::read_sensorAngle(void) { - uint16_t raw_data; - + uint16_t raw_data; - SPI_CS_ENABLE(); - u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data); - SPI_CS_DISABLE(); - - if (result == 0) { - // 单次浮点乘法优化：只计算一次度数，然后转换为弧度 - float angle_deg = (float)raw_data * ANGLE_FACTOR; - current_angle_deg = angle_deg; - current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f - return angle_deg; - } - - #ifdef DEBUG - // 仅在调试模式下输出错误，避免发布版本中的I/O开销 - if (result != 0) { - // 使用更高效的串口输出替代printf（如果可用） + ANG_SPI_CS_ENABLE(); + u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data); + ANG_SPI_CS_DISABLE(); + + if (result == 0) { + // 单次浮点乘法优化：只计算一次度数，然后转换为弧度 + float angle_deg = (float)raw_data * ANGLE_FACTOR; + current_angle_deg = angle_deg; + current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f + return angle_deg; + } + +#ifdef DEBUG + // 仅在调试模式下输出错误，避免发布版本中的I/O开销 + if (result != 0) { + // 使用更高效的串口输出替代printf（如果可用） printf("Error reading angle from sensor.\n"); - } - #endif - - return (float)FAULT_RECEIVE; + } +#endif + + return (float)FAULT_RECEIVE; } /** * @brief 更新传感器角度（输出级） @@ -262,9 +265,6 @@ float angle_sensor::update(void) { delta += ANGLE_WRAP_VALUE; } - std::memmove(velocity, velocity + 1, SIZE_FLOAT * (VEL_FILTER_SIZE - 1)); - velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE; - m_cumulativeAngle += delta; m_lastAngle = current_angle_deg; @@ -285,26 +285,6 @@ float angle_sensor::getAngle(void) const { */ float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; } -/** - * @brief 获取速度值(输出级) - * @return 速度值(输出级) - */ -float angle_sensor::getVelocity(void) const { - return getRawVelocity() / REDUCTION_RATIO; -} - -/** - * @brief 获取速度值(原始级) - * @return 速度值(原始级) - */ -float angle_sensor::getRawVelocity(void) const { - float velocity_filted = 0; - for (int i = 0; i < VEL_FILTER_SIZE - 1; i++) { - velocity_filted += velocity[i]; - } - return velocity_filted / (VEL_FILTER_SIZE - 1); - // return velocity[VEL_FILTER_SIZE - 1]; -} /** * @brief 获取角度值(原始级) * @return 角度值(原始级) diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index c29a23c..2f86def 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -14,14 +14,12 @@ constexpr u16 VEL_FILTER_SIZE = 4; class angle_sensor { public: - angle_sensor(); + u8 init(void); float read_sensorAngle(void); float update(void); void reset(void); float getAngle(void) const; float getRawAngle(void) const; - float getVelocity(void) const; - float getRawVelocity(void) const; float getcurrent_sensorangle(void) const; protected: diff --git a/Robot_V2/device/motor.cpp b/Robot_V2/device/motor.cpp new file mode 100644 index 0000000..083740e --- /dev/null +++ b/Robot_V2/device/motor.cpp @@ -0,0 +1,38 @@ +#include "motor.h" +#include "Timer.h" +#include "gd32c10x_eval.h" +#include + +/** + * @brief driver输出初始化 + */ +void motor_init(void) { + timer1_gpio_config(); + timer1_pwm_config(); + // 使能GPIOB时钟 + rcu_periph_clock_enable(RCU_GPIOB); + // 配置PB2为输入引脚，用于读取电机状态 + 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); +} + +/** + * @brief 电机复位 + */ +void motor_reset(void) { + motor_sleep(); + printf("motor reseting...\r\n"); + motor_resume(); +} + +/** + * @brief 电机休眠 + */ +void motor_sleep(void) { gpio_bit_reset(GPIOB, GPIO_PIN_12); } + +/** + * @brief 电机启动 + */ +void motor_resume(void) { gpio_bit_set(GPIOB, GPIO_PIN_12); } \ No newline at end of file diff --git a/Robot_V2/device/motor.h b/Robot_V2/device/motor.h new file mode 100644 index 0000000..dc198a3 --- /dev/null +++ b/Robot_V2/device/motor.h @@ -0,0 +1,11 @@ +#ifndef _MOTOR_H +#define _MOTOR_H + + +void motor_init(void); +void motor_reset(void); +void motor_resume(void); +void motor_sleep(void); + + +#endif \ No newline at end of file diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 72bb07e..5d3cfe9 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -7,39 +7,49 @@ #include "ang_sensor.h" #include "core_cm4.h" #include "core_cmFunc.h" +#include "motor.h" #include "health.h" #include "initial.h" -#include "sys/health.h" -#include "sys/systick.h" #include "systick.h" +#include #include TaskScheduler scheduler = {0}; int main(void) { - systick_config(); // 系统时钟初始化 - USART0_init(); // 调试串口初始化 - User_timer_init(); // Timer初始化 - driver_init(); // 电机驱动初始化 - extern FOC_Controlroller FOC; // FOC控制 + systick_config(); // 系统时钟初始化 + USART0_init(); // 调试串口初始化 + User_timer_init(); // 任务Timer初始化 + motor_init(); // 电机驱动初始化 + + extern CAN_transmiter CAN0_transmiter; // CAN初始化 extern sampler sampler; // 采样器 + extern angle_sensor KTH7823; // 角度传感器 extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern EncoderAngleKalman Angle_kalman; // 角度滤波 - extern angle_sensor KTH7823; // 角度传感器 - extern CAN_transmiter CAN0_transmiter; // CAN extern ThermalController thermalController; // 热量控制器 + extern FOC_Controlroller FOC; // FOC控制 + + u8 init_status = 0; + init_status += KTH7823.init(); // 角度传感器初始化 + init_status += sampler.init(); // 采样器初始化 + init_status += CAN0_transmiter.init(); // CAN初始化 + init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 + init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 + while (init_status) { // 初始化检测 + std::printf("初始化失败\r\n"); + } u32 cnt_test = 0; - delay_1ms(5); + watchdog_init(500); // 看门狗初始化 angsens_MG_check(); // 角度传感器磁感应强度测试 - watchdog_init(5000); // 看门狗初始化 while (1) { __disable_irq(); - // 保存当前任务标志快照以提高效率并确保一致性 - u32 currentTasks = scheduler.taskFlags; - // 一次性清除所有已处理的任务标志 - scheduler.taskFlags &= ~currentTasks; + u32 currentTasks = + scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性 + scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志 __enable_irq(); cnt_test++; + // 按照优先级和重要性处理任务 if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 sampler.update_current(); @@ -49,6 +59,7 @@ int main(void) { FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()); } if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值 + CAN0_transmiter.testfloat_union.f = (Angle_kalman.getstate()-Angle_kalman.last_angle)*10000.0f; // 速度滤波前的速度值，单位: deg/s FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity()); } if (currentTasks & @@ -102,4 +113,5 @@ int main(void) { fwdgt_counter_reload(); // 看门狗复位 } } + return 0; } \ No newline at end of file diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index 0b47d45..e574ab2 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,15 +1,7 @@ #include "initial.h" -#include "Timer.h" -#include "ang_sensor.h" #include -void GPIO_config(void) { - // 使能GPIOB时钟 - rcu_periph_clock_enable(RCU_GPIOB); - // 配置PB2为输入引脚，用于读取电机状态 - gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2); -} /* retarget the C library printf function to the USART */ u8 USART0_init() { @@ -33,14 +25,6 @@ u8 watchdog_init(u32 time) { } -/** - * @brief driver输出初始化 - */ -void driver_init(void) { - timer1_gpio_config(); - timer1_pwm_config(); -} - int fputc(int ch, FILE *f) throw() { usart_data_transmit(EVAL_COM0, (uint8_t)ch); while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE)) diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 38ea28c..f2c6e31 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -17,10 +17,8 @@ void rcu_config(void); static volatile uint32_t timer_delay_count = 0; // volatile 很重要！ int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 -u8 tle5210b_init(void); u8 USART0_init(void); u8 watchdog_init(u32 time); -void driver_init(void); // 任务标志定义 typedef enum { TASK_MOTOR_CONTROL = 0, From 7371f97882ec10a7c16e2927abbc7309326bc10a Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 16 Dec 2025 22:39:39 +0800 Subject: [PATCH 03/16] =?UTF-8?q?feat(PID):=20=E8=B0=83=E6=95=B4=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E4=B8=8E=E8=A7=92=E5=BA=A6=E7=8E=AFPID=E5=8F=82?= =?UTF-8?q?=E6=95=B0=20=E6=9B=B4=E6=96=B0=E4=BA=86FOC=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E5=99=A8=E4=B8=AD=E7=9A=84=E9=80=9F=E5=BA=A6=E7=8E=AF=E5=92=8C?= =?UTF-8?q?=E8=A7=92=E5=BA=A6=E7=8E=AFPID=E5=8F=82=E6=95=B0=E4=BB=A5?= =?UTF-8?q?=E6=94=B9=E5=96=84=E6=8E=A7=E5=88=B6=E5=93=8D=E5=BA=94=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E5=BE=AE=E8=B0=83=E7=94=B5=E6=B5=81=E7=8E=AF=E5=8F=82?= =?UTF-8?q?=E6=95=B0=E6=8F=90=E5=8D=87=E7=A8=B3=E5=AE=9A=E6=80=A7=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(Filter): 改进卡尔曼滤波器初始化逻辑及速度计算方式 引入系统运行时间计数器，修正编码器角度和速度卡尔曼滤波器的初始化流程，并基于实际时间间隔优化角速度估算精度。 refactor(Filter): 移动成员变量声明顺序 调整`EncoderAngleKalman`类中`last_angle`成员变量的访问权限和声明位置，增强封装性和可读性。 feat(Flash): 新增Flash EEPROM功能支持 添加FlashEEPROM类及相关头文件，支持电机角度等关键数据掉电保存，并在CAN通信复位时进行角度存储操作。 refactor(Timer): 优化定时器配置和中断优先级设置 删除冗余的TIMER3定义，修改TIMER1中断服务函数可见性，并调整NVIC子优先级分配，确保中断响应合理有序。 feat(AngleSensor): 完善角度传感器初始化与角度设定接口 重构角度传感器初始化方法并新增角度设置函数，支持从Flash读取初始角度值并与减速比结合应用。 feat(Health): 实现母线电压监测与保护策略 新增电源管理函数，在母线电压异常下降时自动关闭电机输出，防止因供电不足导致设备损坏。 --- Robot_V2.csolution.yml | 2 +- Robot_V2/Algorithm/DengFOC.cpp | 2 +- Robot_V2/Algorithm/DengFOC.h | 6 +- Robot_V2/Algorithm/Filter.cpp | 19 +- Robot_V2/Algorithm/Filter.h | 2 +- Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c | 4 +- Robot_V2/Robot_V2.cproject.yml | 2 + Robot_V2/device/Can.cpp | 14 +- Robot_V2/device/Can.h | 1 + Robot_V2/device/Timer.cpp | 17 +- Robot_V2/device/ang_sensor.cpp | 27 +- Robot_V2/device/ang_sensor.h | 2 +- Robot_V2/main.cpp | 35 +- Robot_V2/sys/flash.cpp | 344 ++++++++++++++++++ Robot_V2/sys/flash.h | 64 ++++ Robot_V2/sys/health.cpp | 15 + Robot_V2/sys/health.h | 1 + Robot_V2/sys/initial.cpp | 15 + Robot_V2/sys/initial.h | 1 + 19 files changed, 516 insertions(+), 57 deletions(-) create mode 100644 Robot_V2/sys/flash.cpp create mode 100644 Robot_V2/sys/flash.h diff --git a/Robot_V2.csolution.yml b/Robot_V2.csolution.yml index b255718..5b0406c 100644 --- a/Robot_V2.csolution.yml +++ b/Robot_V2.csolution.yml @@ -30,7 +30,7 @@ solution: target-set: - set: images: - - project-context: Robot_V2.Debug + - project-context: Robot_V2.Release debugger: name: ST-Link@pyOCD port: 3333 diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 00d95e5..4d6ece9 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -172,7 +172,7 @@ float FOC_Controlroller::velocity_open_loop() { const float Uq = voltage_power_supply / 2.0f; setPhaseVoltage(Uq, 0.0f); - CAN0_transmiter.testfloat_union.f = + // CAN0_transmiter.testfloat_union.f = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); return limit_voltage; diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index 50e5c12..cc967e6 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -43,12 +43,12 @@ class FOC_Controlroller { float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - PIDController vel_loop = PIDController{0.13f, 0.4f, 2.8e-4, 1000000, + PIDController vel_loop = PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V - PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000, + PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000, voltage_power_supply}; // 12V PIDController current_loop = - PIDController{0.01f, 0.001f, 0, 100000, voltage_power_supply}; // 12V + PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V void setPhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index f1a658b..01021aa 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -7,8 +7,9 @@ extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; -EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f); -EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f); +extern u32 system_runtime_us; +EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f); +EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f); constexpr u8 SIZE_FLOAT = sizeof(float); @@ -30,7 +31,6 @@ float medium_filter(float values[], u8 size) { return sorted[size / 2]; // 返回中值 } - // EncoderVelocityKalman类的实现 /*! \brief 构造函数，初始化编码器速度卡尔曼滤波器参数 @@ -100,7 +100,6 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle, measurement_noise), last_angle(initial_angle), initialized(true) {} - /*! \brief 更新卡尔曼滤波器状态 \param[in] measured_angle 测量的角度值 (degree) @@ -127,16 +126,20 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); } \brief 获取当前角度估计值 \retval 当前角度估计值 (degree) */ -float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*3.75e3; } +float EncoderAngleKalman::getVelocity() const { + static u32 last_us100_counter = system_runtime_us; + float vel = (kalman.getState() - last_angle) * 1e4 * + (system_runtime_us - last_us100_counter); + last_us100_counter = system_runtime_us; + return vel; +} /*! \brief 设置过程噪声协方差 \param[in] q 过程噪声协方差值 \retval none */ -void EncoderAngleKalman::setProcessNoise(float q) { - kalman.setProcessNoise(q); -} +void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); } /*! \brief 设置测量噪声协方差 diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index 1cd1122..26096bb 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -138,9 +138,9 @@ class EncoderAngleKalman { private: KalmanFilter kalman; bool initialized; + float last_angle; public: - float last_angle; EncoderAngleKalman(float initial_angle, float initial_error_covariance, float process_noise, float measurement_noise); diff --git a/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c b/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c index 3a3eae2..d9d1a58 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c +++ b/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c @@ -33,6 +33,7 @@ OF SUCH DAMAGE. */ #include "gd32c10x_fmc.h" +#include "stdio.h" /* FMC mask */ #define LOW_8BITS_MASK ((uint32_t)0x000000FFU) @@ -329,6 +330,7 @@ fmc_state_enum fmc_doubleword_program(uint32_t address, uint64_t data) fmc_state_enum fmc_word_program(uint32_t address, uint32_t data) { fmc_state_enum fmc_state = fmc_ready_wait(FMC_TIMEOUT_COUNT); + printf("fmc_status: 0x%x\r\n", fmc_state); if(FMC_READY == fmc_state){ /* set the PG bit to start program */ @@ -988,7 +990,7 @@ void fmc_interrupt_flag_clear(uint32_t flag) static fmc_state_enum fmc_state_get(void) { fmc_state_enum fmc_state = FMC_READY; - + if((uint32_t)0x00U != (FMC_STAT & FMC_STAT_BUSY)){ fmc_state = FMC_BUSY; }else{ diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index 9c5425b..e38f8de 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -12,6 +12,8 @@ project: - file: sys/initial.h - file: sys/health.h - file: sys/health.cpp + - file: sys/flash.h + - file: sys/flash.cpp - group: device files: - file: device/Can.cpp diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index ea72bb2..6b1e4cf 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -3,6 +3,8 @@ #include "Filter.h" #include "adc.h" #include "ang_sensor.h" +#include "core_cmFunc.h" +#include "flash.h" #include "gd32c10x.h" #include "gd32c10x_can.h" #include "gd32c10x_dma.h" @@ -27,6 +29,7 @@ extern FOC_Controlroller FOC; extern sampler sampler; extern angle_sensor KTH7823; extern EncoderVelocityKalman Vel_kalman; +extern FlashEEPROM flashEEP; // 添加DMA相关变量 dma_parameter_struct dma_rx_parameter; @@ -628,9 +631,10 @@ 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(v_current_union.f)); pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f)); - transmit_data[10] = (transmit_data[10] & ~0x07) | - ((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) | - ((ThermalStatus & 0x01) << 2)); + transmit_data[10] = + (transmit_data[10] & ~0x07) | + ((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) | + ((ThermalStatus & 0x01) << 2) | ((EEPROMStatus & 0x01) << 3)); pack_u16_to_bytes(transmit_data, 11, Power_Voltage); pack_float_to_bytes(transmit_data, 13, rt_angle_union); @@ -692,8 +696,8 @@ void CAN_transmiter::communication_check() { FOC.target_velocity = target_velocity_union.f; } else if (receive_message.rx_efid == 0x00010301) { motor_reset(); - } - else { + flashEEP.save_angle(666, false); + } else { printf("Unknown CAN ID: %X\n", receive_message.rx_efid); } can0_receive_flag = RESET; diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 5bc1e74..9e17da1 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -110,6 +110,7 @@ class CAN_transmiter { bool MagStatus = 0; // 磁状态码 bool ThermalStatus = 0; // 热能状态码 + bool EEPROMStatus = 0; // EEPROM状态码 void communication_check(); // 通信检查 }; diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index d8a18f1..5d62455 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -14,8 +14,6 @@ constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us -constexpr int TIMER3_PRESCALER = 119; // 120MHz / 120 = 1MHz -constexpr int TIMER3_PERIOD = 9999; // 1MHz / 10000 = 100Hz = 10ms constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz @@ -25,13 +23,13 @@ constexpr int TIMER1_CH2_PULSE = 0; constexpr int TIMER1_CH3_COMPARE_VALUE = (TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断 -static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1 +volatile 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; +extern u32 cnt_test; /** * @brief 设置引脚中断优先级 @@ -44,7 +42,7 @@ void nvic_config(void) { nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2); /* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */ nvic_irq_enable(TIMER2_IRQn, 0, 1); - nvic_irq_enable(TIMER1_IRQn, 0, 1); + nvic_irq_enable(TIMER1_IRQn, 0, 2); } /** @@ -99,13 +97,6 @@ void User_timer_init(void) { nvic_config(); } -/*! - \brief 获取系统运行时间(毫秒) - \param[in] none - \param[out] none - \retval 系统运行时间，单位为毫秒 -*/ -uint64_t get_system_runtime_us(void) { return system_runtime_us; } /** * @brief TIMER2中断处理函数 @@ -277,7 +268,7 @@ void timer1_pwm_config(void) { * @brief TIMER1中断处理函数 * @return void */ -extern "C" void TIMER1_IRQHandler(void) { +void TIMER1_IRQHandler(void) { // 检查CH3比较中断标志并处理 if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) { // 清除CH3比较中断标志 diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 19af117..06d2b1d 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -1,12 +1,15 @@ #include "ang_sensor.h" #include "Filter.h" #include "core_cmFunc.h" +#include "gd32c10x_can.h" #include "gd32c10x_dma.h" #include "gd32c10x_gpio.h" #include "gd32c10x_spi.h" #include "initial.h" +#include "flash.h" #include #include +#include "Can.h" #include #define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) @@ -28,8 +31,10 @@ volatile bool spi_dma_tx_complete = false; volatile bool spi_dma_rx_complete = false; angle_sensor KTH7823; // 角度传感器初始化 +extern FlashEEPROM flashEEP; extern EncoderAngleKalman Angle_kalman; extern EncoderVelocityKalman Vel_kalman; + extern CAN_transmiter CAN0_transmiter; // CAN初始化 /** * @brief SPI DMA initialization @@ -194,21 +199,27 @@ void KTH7823_spi_init(void) { spi_enable(SPI0); } + /** - * @brief KTH7823 初始化 +* @ 角度传感器类 初始化 +* @return 0:初始化成功 1:初始化失败 */ -void angle_sensor::KTH7823_init(void) { +u8 angle_sensor::init(void) { KTH7823_gpio_init(); // GPIO初始化 KTH7823_spi_init(); // SPI初始化 spi_dma_init(); // DMA初始化 -} -/** - * @brief 角度传感器构造函数 - */ -u8 angle_sensor::init(void) { - KTH7823_init(); return 0; } +/** +* @ 角度传感器类 设置当前角度 +* @param angle 输出级角度值 +* @return 0成功/1失败 + */ +u8 angle_sensor::angle_set(float angle){ + m_cumulativeAngle = flashEEP.read_angle()*REDUCTION_RATIO; // 从Flash读取上次保存的角度 + return 0; +} + /** * @brief 读取传感器数据 diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index 2f86def..b2501c0 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -21,9 +21,9 @@ class angle_sensor { float getAngle(void) const; float getRawAngle(void) const; float getcurrent_sensorangle(void) const; + u8 angle_set(float angle); protected: - void KTH7823_init(void); float m_cumulativeAngle; float current_angle_deg; float current_angle_rad; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 5d3cfe9..a6baacf 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -7,16 +7,20 @@ #include "ang_sensor.h" #include "core_cm4.h" #include "core_cmFunc.h" -#include "motor.h" +#include "flash.h" #include "health.h" #include "initial.h" +#include "motor.h" #include "systick.h" #include #include TaskScheduler scheduler = {0}; +volatile u32 system_runtime_us = 0; +volatile u32 cnt_test = 0; int main(void) { systick_config(); // 系统时钟初始化 + unused_pin_init(); // 未使用引脚初始化 USART0_init(); // 调试串口初始化 User_timer_init(); // 任务Timer初始化 motor_init(); // 电机驱动初始化 @@ -28,27 +32,27 @@ int main(void) { extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern ThermalController thermalController; // 热量控制器 extern FOC_Controlroller FOC; // FOC控制 + extern FlashEEPROM flashEEP; // EEPROM类 u8 init_status = 0; - init_status += KTH7823.init(); // 角度传感器初始化 - init_status += sampler.init(); // 采样器初始化 - init_status += CAN0_transmiter.init(); // CAN初始化 - init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 - init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 - while (init_status) { // 初始化检测 + init_status += flashEEP.init(); // EEPROM初始化 + init_status += KTH7823.init(); // 角度传感器初始化 + init_status += sampler.init(); // 采样器初始化 + init_status += CAN0_transmiter.init(); // CAN初始化 + init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 + init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 + while (init_status) { // 初始化检测 std::printf("初始化失败\r\n"); } - u32 cnt_test = 0; watchdog_init(500); // 看门狗初始化 - angsens_MG_check(); // 角度传感器磁感应强度测试 + angsens_MG_check(); // 角度传感器磁感应强度测试 while (1) { __disable_irq(); u32 currentTasks = scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性 scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志 __enable_irq(); - cnt_test++; // 按照优先级和重要性处理任务 if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 @@ -59,11 +63,11 @@ int main(void) { FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()); } if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值 - CAN0_transmiter.testfloat_union.f = (Angle_kalman.getstate()-Angle_kalman.last_angle)*10000.0f; // 速度滤波前的速度值，单位: deg/s FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity()); } if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) + system_runtime_us++; switch (FOC.control_mode) { case 0: { FOC.drv_hold(0.95); // 占空比置0 @@ -71,6 +75,8 @@ int main(void) { } case 1: { FOC.angle_control(FOC.target_angle); // 角度闭环控制 + // FOC.angle_control_with_velocity_limit(FOC.target_angle, + // FOC.limit_velocity); // 角度闭环控制，带速度限制 break; } case 2: { @@ -102,11 +108,10 @@ int main(void) { angsens_MG_check(); } if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 - sampler.update_vbus_voltage(); - FOC.update_power(); + power_manage(); } if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 - // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 + // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 cnt_test = 0; CAN0_transmiter.can0_send_flag = SET; CAN0_transmiter.communication_check(); @@ -114,4 +119,4 @@ int main(void) { } } return 0; -} \ No newline at end of file +} diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp new file mode 100644 index 0000000..f26a71b --- /dev/null +++ b/Robot_V2/sys/flash.cpp @@ -0,0 +1,344 @@ +#include "flash.h" +#include "ang_sensor.h" +#include "core_cmFunc.h" +#include "gd32c10x_fmc.h" +#include +#include +#include + +FlashEEPROM flashEEP = FlashEEPROM(); +extern angle_sensor KTH7823; +extern CAN_transmiter CAN0_transmiter; + +// FlashEEPROM类实现 +FlashEEPROM::FlashEEPROM() { + // 构造函数可以为空，或者做一些初始化工作 +} + +/** + * @brief 初始化Flash EEPROM模拟 + * @param None + * @retval None + */ +u8 FlashEEPROM::init() { + // 解锁Flash + fmc_unlock(); + + // 读取保存的参数 + u16 angle; + + // 尝试从Flash读取参数，如果不存在则使用默认值 + if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { + angle = 0; // 默认值 + std::printf("角度初始化为: %d\r\n", angle); + } else { + std::printf("从Flash读取到角度数: %d\r\n", angle); + } + + KTH7823.angle_set((float)angle); + // save_angle(200, false); // 重置保存标志以允许再次保存 + return 0; +} + +/** + * @brief 从Flash EEPROM读取数据 + * @param virtualAddr: 虚拟地址 + * @param data: 指向存储读取数据的指针 + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::read(uint16_t virtualAddr, + uint16_t *data) const { + // 检查参数有效性 + if (data == nullptr || virtualAddr == 0xFFFF) { + return FLASH_EEPROM_ERROR; + } + + // 从Flash末尾开始向前搜索，找到最新的数据 + for (uint32_t address = EEPROM_END_ADDRESS - sizeof(uint32_t); + address >= EEPROM_START_ADDRESS; address -= sizeof(uint32_t)) { + + uint32_t data_word = *((uint32_t *)address); + + // 检查是否是有效数据（非擦除状态） + if (data_word != 0xFFFFFFFF) { + uint16_t addr = (uint16_t)(data_word & 0xFFFF); + + // 找到匹配的虚拟地址 + if (addr == virtualAddr) { + *data = (uint16_t)((data_word >> 16) & 0xFFFF); + return FLASH_EEPROM_OK; + } + } + } + + // 未找到指定虚拟地址的数据 + return FLASH_EEPROM_NOT_FOUND; +} + +/** + * @brief 向Flash EEPROM写入数据 + * @param virtualAddr: 虚拟地址 + * @param data: 要写入的数据 + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::write(uint16_t virtualAddr, uint16_t data) { + // 参数检查 + if (virtualAddr == 0xFFFF) { + return FLASH_EEPROM_INVALID_ADDR; + } + + // 查找下一个可写入的位置 + uint32_t next_address = EEPROM_START_ADDRESS; + + // 遍历Flash页面寻找下一个可写入位置 + for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; + address += sizeof(uint32_t)) { + + uint32_t data_word = *((uint32_t *)address); + + // 如果找到已使用的地址且与要写入的地址相同，检查数据是否需要更新 + if ((data_word & 0xFFFF) == virtualAddr) { + uint16_t old_data = (uint16_t)((data_word >> 16) & 0xFFFF); + if (old_data == data) { + // 数据相同，无需更新 + return FLASH_EEPROM_OK; + } + } + // 找到空位置 + if (data_word == 0xFFFFFFFF) { + next_address = address; + break; + } + // 更新下一个可能的写入位置（如果需要格式化） + next_address = address + sizeof(uint32_t); + } + + // 如果有足够空间，直接写入 + if (next_address < EEPROM_END_ADDRESS) { + u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF); + fmc_state_enum fmc_status = + fmc_word_program(next_address, new_data_word); + if (fmc_status != FMC_READY) { + return FLASH_EEPROM_WRITE_ERROR; + } + + return FLASH_EEPROM_OK; + } else { + // 空间不足，需要格式化页面 + FlashEepromStatus status = format(); + if (status != FLASH_EEPROM_OK) { + return status; + } + + // 格式化后在页面开头写入数据 + uint32_t new_data_word = + ((uint32_t)data << 16) | (virtualAddr & 0xFFFF); + fmc_state_enum fmc_status = + fmc_word_program(EEPROM_START_ADDRESS, new_data_word); + + if (fmc_status != FMC_READY) { + return FLASH_EEPROM_WRITE_ERROR; + } + + return FLASH_EEPROM_OK; + } +} + +/** + * @brief 格式化Flash EEPROM页面 + * @param None + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::format() { + fmc_state_enum fmc_status; + + // 创建临时缓冲区存储唯一地址的最新数据 + uint32_t valid_entries[EEPROM_MAX_SIZE / sizeof(uint32_t)]; + uint16_t valid_count = 0; + + // 收集有效的数据项（去重，保留最新的） + for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; + address += sizeof(uint32_t)) { + + uint32_t data_word = *((uint32_t *)address); + + // 检查是否是有效数据项 + if (data_word != 0xFFFFFFFF) { + uint16_t addr = (uint16_t)(data_word & 0xFFFF); + uint16_t value = (uint16_t)((data_word >> 16) & 0xFFFF); + + // 查找是否已经有相同虚拟地址的数据项 + uint8_t found = 0; + for (uint16_t i = 0; i < valid_count; i++) { + uint16_t existing_addr = (uint16_t)(valid_entries[i] & 0xFFFF); + if (existing_addr == addr) { + // 更新为最新的数据 + valid_entries[i] = ((uint32_t)value << 16) | addr; + found = 1; + break; + } + } + + // 如果是新的虚拟地址，添加到有效数据列表 + if (!found && valid_count < (EEPROM_MAX_SIZE / sizeof(uint32_t))) { + valid_entries[valid_count] = ((uint32_t)value << 16) | addr; + valid_count++; + } + } + } + + // 擦除整个页面 + fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); + if (fmc_status != FMC_READY) { + return FLASH_EEPROM_ERASE_ERROR; + } + + // 重新写入有效的数据项 + for (uint16_t i = 0; i < valid_count; i++) { + fmc_status = fmc_word_program( + EEPROM_START_ADDRESS + i * sizeof(uint32_t), valid_entries[i]); + if (fmc_status != FMC_READY) { + return FLASH_EEPROM_WRITE_ERROR; + } + } + + return FLASH_EEPROM_OK; +} + +/** + * @brief 更新参数并保存到Flash + * @param kp 新的KP值 + * @param ki 新的KI值 + * @param kd 新的KD值 + */ +void FlashEEPROM::update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd) { + write(ADDR_MOTOR_KP, kp); + write(ADDR_MOTOR_KI, ki); + write(ADDR_MOTOR_KD, kd); +} + +/** + * @brief 更新系统配置并保存到Flash + * @param config 新的系统配置值 + */ +void FlashEEPROM::update_system_config(uint16_t config) { + write(ADDR_SYSTEM_CONFIG, config); +} + +/** + * @brief 保存角度数据到Flash + * @param angle 角度数据 + * @return FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::save_angle(float angle, + bool saved) { // 解锁Flash + // 解锁Flash + fmc_unlock(); + + // 读取保存的参数 + u16 angle2; + + // 尝试从Flash读取参数，如果不存在则使用默认值 + if (read(ADDR_ANGLE_DATA, &angle2) != FLASH_EEPROM_OK) { + angle = 0; // 默认值 + std::printf("角度初始化为: %d\r\n", angle2); + } else { + std::printf("从Flash读取到角度数: %d\r\n", angle2); + } + + // 读取保存的参数 + if (!angle_saved) { + printf("saved_angle: %u\r\n", (u16)angle); + FlashEepromStatus status = write(ADDR_ANGLE_DATA, (u16)angle); + if (status == FLASH_EEPROM_OK) { + angle_saved = saved; // 标记已保存过 + CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态 + } else { + CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态 + } + CAN0_transmiter.testfloat_union.f = status; // CAN发送测试 + return status; + } + return FLASH_EEPROM_OK; // 已保存过，直接返回 +} + +/** + * @brief 读取角度数据从Flash + * @return float: 角度数据 + */ +float FlashEEPROM::read_angle() { + u16 angle = 0; + if (read(ADDR_ANGLE_DATA, &angle) == FLASH_EEPROM_OK) { + return (float)angle; + } + return 0.0f; // 默认返回0度 +} + +/** + * @brief 增加错误计数并保存到Flash + */ +void FlashEEPROM::increment_error_count(void) { + uint16_t error_count = 0; + + // 读取当前错误计数 + FlashEepromStatus read_status = read(ADDR_ERROR_COUNT, &error_count); + if (read_status == FLASH_EEPROM_OK || + read_status == FLASH_EEPROM_NOT_FOUND) { + // 增加错误计数（如果是NOT_FOUND，则error_count已经是0） + error_count++; + + // 保存更新后的错误计数 + write(ADDR_ERROR_COUNT, error_count); + } +} + +/** + * @brief 示例主函数 - 展示Flash EEPROM的使用方法 + */ +void flash_eeprom_example_main(void) { + std::printf("\r\n=== Flash EEPROM 使用示例 ===\r\n"); + + // 初始化Flash EEPROM + flashEEP.init(); + + // 模拟参数更新 + std::printf("\r\n--- 更新PID参数 ---\r\n"); + flashEEP.update_pid_parameters(120, 80, 30); + + // 模拟系统配置更新 + std::printf("\r\n--- 更新系统配置 ---\r\n"); + flashEEP.update_system_config(0x03); + + // 模拟错误发生 + std::printf("\r\n--- 模拟错误计数增加 ---\r\n"); + flashEEP.increment_error_count(); + flashEEP.increment_error_count(); + + // 再次读取参数验证保存是否成功 + std::printf("\r\n--- 验证保存的数据 ---\r\n"); + uint16_t kp_value, ki_value, kd_value; + uint16_t system_config; + uint16_t error_count; + + // if (flashEEP.read(ADDR_MOTOR_KP, &kp_value) == FLASH_EEPROM_OK) { + // std::printf("当前KP值: %d\r\n", kp_value); + // } + + // if (flashEEP.read(ADDR_MOTOR_KI, &ki_value) == FLASH_EEPROM_OK) { + // std::printf("当前KI值: %d\r\n", ki_value); + // } + + // if (flashEEP.read(ADDR_MOTOR_KD, &kd_value) == FLASH_EEPROM_OK) { + // std::printf("当前KD值: %d\r\n", kd_value); + // } + + // if (flashEEP.read(ADDR_SYSTEM_CONFIG, &system_config) == FLASH_EEPROM_OK) + // { std::printf("当前系统配置: 0x%02X\r\n", system_config); + // } + + // if (flashEEP.read(ADDR_ERROR_COUNT, &error_count) == FLASH_EEPROM_OK) { + // std::printf("当前错误计数: %d\r\n", error_count); + // } + + std::printf("\r\n=== 示例结束 ===\r\n"); +} \ No newline at end of file diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h new file mode 100644 index 0000000..c018c7d --- /dev/null +++ b/Robot_V2/sys/flash.h @@ -0,0 +1,64 @@ +#ifndef __FLASH_H +#define __FLASH_H + +#include "gd32c10x.h" +#include + +// EEPROM操作状态 +typedef enum { + FLASH_EEPROM_OK = 0, // 操作成功 + FLASH_EEPROM_ERROR, // 操作失败 + FLASH_EEPROM_NOT_FOUND, // 未找到指定数据 + FLASH_EEPROM_INVALID_ADDR, // 无效地址 + FLASH_EEPROM_WRITE_ERROR, // 写入错误 + FLASH_EEPROM_ERASE_ERROR // 擦除错误 +} FlashEepromStatus; + +// Flash EEPROM类 +class FlashEEPROM { + public: + // EEPROM emulation constants + static constexpr uint32_t EEPROM_START_ADDRESS = + 0x0801F800; // Flash最后一页(2KB) + static constexpr uint32_t EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 + static constexpr uint32_t EEPROM_PAGE_SIZE = 0x800; // 页大小2KB + static constexpr uint32_t EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 + + // 定义用户自定义的虚拟地址 + static constexpr uint16_t ADDR_MOTOR_KP = 0x0010; // 电机KP参数地址 + static constexpr uint16_t ADDR_MOTOR_KI = 0x0011; // 电机KI参数地址 + static constexpr uint16_t ADDR_MOTOR_KD = 0x0012; // 电机KD参数地址 + static constexpr uint16_t ADDR_SYSTEM_CONFIG = 0x0020; // 系统配置参数地址 + static constexpr uint16_t ADDR_ERROR_COUNT = 0x0030; // 错误计数地址 + + // 角度数据存储地址 + static constexpr u16 ADDR_ANGLE_DATA = 0x0021; + bool angle_saved = false; + // 构造函数 + FlashEEPROM(); + + // 初始化Flash EEPROM模拟 + u8 init(); + + // 从Flash EEPROM读取数据 + FlashEepromStatus read(uint16_t virtualAddr, uint16_t *data) const; + + // 向Flash EEPROM写入数据 + FlashEepromStatus write(uint16_t virtualAddr, uint16_t data); + + // 格式化Flash EEPROM页面 + FlashEepromStatus format(); + + void flash_eeprom_example_init(void); + void update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd); + void update_system_config(uint16_t config); + void increment_error_count(void); + FlashEepromStatus save_angle(float angle,bool saved); + float read_angle(); + + private: + +}; + +void flash_eeprom_example_main(void); +#endif /* __FLASH_H */ \ No newline at end of file diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index 4ef4277..10742ab 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -2,11 +2,13 @@ #include "Can.h" #include "DengFOC.h" #include "adc.h" +#include "flash.h" #include extern sampler sampler; extern CAN_transmiter CAN0_transmiter; extern FOC_Controlroller FOC; +extern FlashEEPROM flashEEP; ThermalController thermalController; // 热量控制器初始化 #define max(a, b, c) \ @@ -85,4 +87,17 @@ 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; +} + +void power_manage(void) { + float last_vbus = sampler.get_vbus_voltage(); + sampler.update_vbus_voltage(); + FOC.update_power(); + float vbus = sampler.get_vbus_voltage(); + if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) { + FOC.shut_down(); + // flashEEP.save_angle(FOC.rt_angle,false); + } else { + FOC.resume(); + } } \ No newline at end of file diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h index 3dfd298..a3b22cf 100644 --- a/Robot_V2/sys/health.h +++ b/Robot_V2/sys/health.h @@ -35,5 +35,6 @@ class ThermalController { }; void angsens_MG_check(void); +void power_manage(void); #endif diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index e574ab2..7e41501 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,4 +1,6 @@ #include "initial.h" +#include "gd32c10x_gpio.h" +#include "gd32c10x_rcu.h" #include @@ -31,3 +33,16 @@ int fputc(int ch, FILE *f) throw() { ; return ch; } + + +void unused_pin_init() { + rcu_periph_clock_enable(RCU_GPIOA); // 使能GPIOA时钟 + rcu_periph_clock_enable(RCU_GPIOB); // 使能GPIOB时钟 + rcu_periph_clock_enable(RCU_GPIOC); // 使能GPIOC时钟 + gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12); + gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15); + gpio_init(GPIOC, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15); + gpio_bit_reset(GPIOA, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12); + gpio_bit_reset(GPIOB, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15); + gpio_bit_reset(GPIOC, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15); +} \ No newline at end of file diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index f2c6e31..5a75963 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -19,6 +19,7 @@ int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 u8 USART0_init(void); u8 watchdog_init(u32 time); +void unused_pin_init(); // 任务标志定义 typedef enum { TASK_MOTOR_CONTROL = 0, From 02572df688547fc9f2833284f4ec0248f7f56f3e Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Wed, 17 Dec 2025 01:38:48 +0800 Subject: [PATCH 04/16] =?UTF-8?q?fix(flash):=20=E4=BC=98=E5=8C=96Flash=20E?= =?UTF-8?q?EPROM=E8=AF=BB=E5=86=99=E9=80=BB=E8=BE=91=E5=B9=B6=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D=E8=A7=92=E5=BA=A6=E4=BF=9D=E5=AD=98=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 移除调试打印语句，改进Flash读写稳定性。 使用类型别名统一数据访问，增强代码可读性。 修正CAN通信中角度保存逻辑，确保正确写入实时角度值。 完善错误处理机制，增加对齐检查和状态反馈。 调整EEPROM起始地址以避免潜在冲突。 --- Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c | 1 - Robot_V2/device/Can.cpp | 2 +- Robot_V2/sys/flash.cpp | 157 ++++++++---------- Robot_V2/sys/flash.h | 15 +- 4 files changed, 77 insertions(+), 98 deletions(-) diff --git a/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c b/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c index d9d1a58..7f29736 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c +++ b/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c @@ -330,7 +330,6 @@ fmc_state_enum fmc_doubleword_program(uint32_t address, uint64_t data) fmc_state_enum fmc_word_program(uint32_t address, uint32_t data) { fmc_state_enum fmc_state = fmc_ready_wait(FMC_TIMEOUT_COUNT); - printf("fmc_status: 0x%x\r\n", fmc_state); if(FMC_READY == fmc_state){ /* set the PG bit to start program */ diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 6b1e4cf..82859ea 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -696,7 +696,7 @@ void CAN_transmiter::communication_check() { FOC.target_velocity = target_velocity_union.f; } else if (receive_message.rx_efid == 0x00010301) { motor_reset(); - flashEEP.save_angle(666, false); + flashEEP.save_angle((u16)FOC.rt_angle, false); } else { printf("Unknown CAN ID: %X\n", receive_message.rx_efid); } diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index f26a71b..ea93778 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -9,6 +9,7 @@ FlashEEPROM flashEEP = FlashEEPROM(); extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; +constexpr u8 SIZE_U32 = sizeof(uint32_t); // FlashEEPROM类实现 FlashEEPROM::FlashEEPROM() { @@ -21,12 +22,8 @@ FlashEEPROM::FlashEEPROM() { * @retval None */ u8 FlashEEPROM::init() { - // 解锁Flash - fmc_unlock(); - // 读取保存的参数 u16 angle; - // 尝试从Flash读取参数，如果不存在则使用默认值 if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { angle = 0; // 默认值 @@ -34,9 +31,7 @@ u8 FlashEEPROM::init() { } else { std::printf("从Flash读取到角度数: %d\r\n", angle); } - KTH7823.angle_set((float)angle); - // save_angle(200, false); // 重置保存标志以允许再次保存 return 0; } @@ -46,31 +41,26 @@ u8 FlashEEPROM::init() { * @param data: 指向存储读取数据的指针 * @retval FlashEepromStatus: 操作状态 */ -FlashEepromStatus FlashEEPROM::read(uint16_t virtualAddr, - uint16_t *data) const { +FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const { // 检查参数有效性 - if (data == nullptr || virtualAddr == 0xFFFF) { - return FLASH_EEPROM_ERROR; + if (data == nullptr || virtualAddr > 0xFFFE) { + return FLASH_EEPROM_INVALID_ADDR; } // 从Flash末尾开始向前搜索，找到最新的数据 - for (uint32_t address = EEPROM_END_ADDRESS - sizeof(uint32_t); - address >= EEPROM_START_ADDRESS; address -= sizeof(uint32_t)) { - - uint32_t data_word = *((uint32_t *)address); - + for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; + address >= EEPROM_START_ADDRESS; address -= SIZE_U32) { + u32 data_word = *((u32 *)address); // 检查是否是有效数据（非擦除状态） if (data_word != 0xFFFFFFFF) { - uint16_t addr = (uint16_t)(data_word & 0xFFFF); - + u16 addr = (u16)(data_word & 0xFFFF); // 找到匹配的虚拟地址 if (addr == virtualAddr) { - *data = (uint16_t)((data_word >> 16) & 0xFFFF); + *data = (u16)((data_word >> 16) & 0xFFFF); return FLASH_EEPROM_OK; } } } - // 未找到指定虚拟地址的数据 return FLASH_EEPROM_NOT_FOUND; } @@ -81,47 +71,45 @@ FlashEepromStatus FlashEEPROM::read(uint16_t virtualAddr, * @param data: 要写入的数据 * @retval FlashEepromStatus: 操作状态 */ -FlashEepromStatus FlashEEPROM::write(uint16_t virtualAddr, uint16_t data) { +FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) { // 参数检查 - if (virtualAddr == 0xFFFF) { + if (virtualAddr > 0xFFFE) { return FLASH_EEPROM_INVALID_ADDR; } - // 查找下一个可写入的位置 - uint32_t next_address = EEPROM_START_ADDRESS; + u32 next_address = EEPROM_START_ADDRESS; // 遍历Flash页面寻找下一个可写入位置 - for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; - address += sizeof(uint32_t)) { + for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; + address += SIZE_U32) { - uint32_t data_word = *((uint32_t *)address); - - // 如果找到已使用的地址且与要写入的地址相同，检查数据是否需要更新 - if ((data_word & 0xFFFF) == virtualAddr) { - uint16_t old_data = (uint16_t)((data_word >> 16) & 0xFFFF); - if (old_data == data) { - // 数据相同，无需更新 - return FLASH_EEPROM_OK; - } - } + // 安全读取Flash数据 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word = *flash_ptr; // 找到空位置 if (data_word == 0xFFFFFFFF) { next_address = address; break; } // 更新下一个可能的写入位置（如果需要格式化） - next_address = address + sizeof(uint32_t); + next_address = address + SIZE_U32; } // 如果有足够空间，直接写入 if (next_address < EEPROM_END_ADDRESS) { - u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF); - fmc_state_enum fmc_status = - fmc_word_program(next_address, new_data_word); + u32 data_word = ((u32)data << 16) | (u32)virtualAddr; + + // 使用GD32库函数进行安全编程 + fmc_unlock(); + fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); + + // 确保使用对齐地址 + fmc_state_enum fmc_status = fmc_word_program(next_address, data_word); + + fmc_lock(); if (fmc_status != FMC_READY) { return FLASH_EEPROM_WRITE_ERROR; } - return FLASH_EEPROM_OK; } else { // 空间不足，需要格式化页面 @@ -131,8 +119,7 @@ FlashEepromStatus FlashEEPROM::write(uint16_t virtualAddr, uint16_t data) { } // 格式化后在页面开头写入数据 - uint32_t new_data_word = - ((uint32_t)data << 16) | (virtualAddr & 0xFFFF); + u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF); fmc_state_enum fmc_status = fmc_word_program(EEPROM_START_ADDRESS, new_data_word); @@ -153,35 +140,34 @@ FlashEepromStatus FlashEEPROM::format() { fmc_state_enum fmc_status; // 创建临时缓冲区存储唯一地址的最新数据 - uint32_t valid_entries[EEPROM_MAX_SIZE / sizeof(uint32_t)]; - uint16_t valid_count = 0; + u32 valid_entries[EEPROM_MAX_SIZE / SIZE_U32]; + u16 valid_count = 0; // 收集有效的数据项（去重，保留最新的） - for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; - address += sizeof(uint32_t)) { - - uint32_t data_word = *((uint32_t *)address); + for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; + address += SIZE_U32) { + u32 data_word = *((u32 *)address); // 检查是否是有效数据项 if (data_word != 0xFFFFFFFF) { - uint16_t addr = (uint16_t)(data_word & 0xFFFF); - uint16_t value = (uint16_t)((data_word >> 16) & 0xFFFF); + u16 addr = (u16)(data_word & 0xFFFF); + u16 value = (u16)((data_word >> 16) & 0xFFFF); // 查找是否已经有相同虚拟地址的数据项 uint8_t found = 0; - for (uint16_t i = 0; i < valid_count; i++) { - uint16_t existing_addr = (uint16_t)(valid_entries[i] & 0xFFFF); + for (u16 i = 0; i < valid_count; i++) { + u16 existing_addr = (u16)(valid_entries[i] & 0xFFFF); if (existing_addr == addr) { // 更新为最新的数据 - valid_entries[i] = ((uint32_t)value << 16) | addr; + valid_entries[i] = ((u32)value << 16) | addr; found = 1; break; } } // 如果是新的虚拟地址，添加到有效数据列表 - if (!found && valid_count < (EEPROM_MAX_SIZE / sizeof(uint32_t))) { - valid_entries[valid_count] = ((uint32_t)value << 16) | addr; + if (!found && valid_count < (EEPROM_MAX_SIZE / SIZE_U32)) { + valid_entries[valid_count] = ((u32)value << 16) | addr; valid_count++; } } @@ -194,9 +180,9 @@ FlashEepromStatus FlashEEPROM::format() { } // 重新写入有效的数据项 - for (uint16_t i = 0; i < valid_count; i++) { - fmc_status = fmc_word_program( - EEPROM_START_ADDRESS + i * sizeof(uint32_t), valid_entries[i]); + for (u16 i = 0; i < valid_count; i++) { + fmc_status = fmc_word_program(EEPROM_START_ADDRESS + i * SIZE_U32, + valid_entries[i]); if (fmc_status != FMC_READY) { return FLASH_EEPROM_WRITE_ERROR; } @@ -211,7 +197,7 @@ FlashEepromStatus FlashEEPROM::format() { * @param ki 新的KI值 * @param kd 新的KD值 */ -void FlashEEPROM::update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd) { +void FlashEEPROM::update_pid_parameters(u16 kp, u16 ki, u16 kd) { write(ADDR_MOTOR_KP, kp); write(ADDR_MOTOR_KI, ki); write(ADDR_MOTOR_KD, kd); @@ -221,7 +207,7 @@ void FlashEEPROM::update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd) { * @brief 更新系统配置并保存到Flash * @param config 新的系统配置值 */ -void FlashEEPROM::update_system_config(uint16_t config) { +void FlashEEPROM::update_system_config(u16 config) { write(ADDR_SYSTEM_CONFIG, config); } @@ -230,36 +216,27 @@ void FlashEEPROM::update_system_config(uint16_t config) { * @param angle 角度数据 * @return FlashEepromStatus: 操作状态 */ -FlashEepromStatus FlashEEPROM::save_angle(float angle, - bool saved) { // 解锁Flash - // 解锁Flash - fmc_unlock(); - - // 读取保存的参数 - u16 angle2; - - // 尝试从Flash读取参数，如果不存在则使用默认值 - if (read(ADDR_ANGLE_DATA, &angle2) != FLASH_EEPROM_OK) { - angle = 0; // 默认值 - std::printf("角度初始化为: %d\r\n", angle2); - } else { - std::printf("从Flash读取到角度数: %d\r\n", angle2); - } +FlashEepromStatus FlashEEPROM::save_angle(u16 angle, bool saved) { + u16 angle_read = 0; + FlashEepromStatus status = FLASH_EEPROM_ERROR; // 读取保存的参数 if (!angle_saved) { - printf("saved_angle: %u\r\n", (u16)angle); - FlashEepromStatus status = write(ADDR_ANGLE_DATA, (u16)angle); - if (status == FLASH_EEPROM_OK) { + // 写入参数 + status = write(ADDR_ANGLE_DATA, angle); + + if (read(ADDR_ANGLE_DATA, &angle_read) != FLASH_EEPROM_OK) + angle_read = 0; // 读不到角度值，存储失败 + + if (angle_read != angle) { + status = FLASH_EEPROM_WRITE_ERROR; + CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态 + } else { angle_saved = saved; // 标记已保存过 CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态 - } else { - CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态 } - CAN0_transmiter.testfloat_union.f = status; // CAN发送测试 - return status; } - return FLASH_EEPROM_OK; // 已保存过，直接返回 + return status; } /** @@ -268,17 +245,17 @@ FlashEepromStatus FlashEEPROM::save_angle(float angle, */ float FlashEEPROM::read_angle() { u16 angle = 0; - if (read(ADDR_ANGLE_DATA, &angle) == FLASH_EEPROM_OK) { - return (float)angle; + if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { + angle = 0; } - return 0.0f; // 默认返回0度 + return (float)angle; // 默认返回0度 } /** * @brief 增加错误计数并保存到Flash */ void FlashEEPROM::increment_error_count(void) { - uint16_t error_count = 0; + u16 error_count = 0; // 读取当前错误计数 FlashEepromStatus read_status = read(ADDR_ERROR_COUNT, &error_count); @@ -316,9 +293,9 @@ void flash_eeprom_example_main(void) { // 再次读取参数验证保存是否成功 std::printf("\r\n--- 验证保存的数据 ---\r\n"); - uint16_t kp_value, ki_value, kd_value; - uint16_t system_config; - uint16_t error_count; + u16 kp_value, ki_value, kd_value; + u16 system_config; + u16 error_count; // if (flashEEP.read(ADDR_MOTOR_KP, &kp_value) == FLASH_EEPROM_OK) { // std::printf("当前KP值: %d\r\n", kp_value); diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index c018c7d..7e6025c 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -3,6 +3,9 @@ #include "gd32c10x.h" #include +#include +// 修改后 - 强制4字节对齐 +#define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 // EEPROM操作状态 typedef enum { @@ -11,13 +14,14 @@ typedef enum { FLASH_EEPROM_NOT_FOUND, // 未找到指定数据 FLASH_EEPROM_INVALID_ADDR, // 无效地址 FLASH_EEPROM_WRITE_ERROR, // 写入错误 - FLASH_EEPROM_ERASE_ERROR // 擦除错误 + FLASH_EEPROM_ERASE_ERROR, // 擦除错误 + FLASH_EEPROM_ALIGNMENT_ERROR,// 对齐错误 } FlashEepromStatus; // Flash EEPROM类 class FlashEEPROM { - public: - // EEPROM emulation constants + public: + // EEPROM emulation constants static constexpr uint32_t EEPROM_START_ADDRESS = 0x0801F800; // Flash最后一页(2KB) static constexpr uint32_t EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 @@ -32,7 +36,7 @@ class FlashEEPROM { static constexpr uint16_t ADDR_ERROR_COUNT = 0x0030; // 错误计数地址 // 角度数据存储地址 - static constexpr u16 ADDR_ANGLE_DATA = 0x0021; + static constexpr u16 ADDR_ANGLE_DATA = 0x0013; bool angle_saved = false; // 构造函数 FlashEEPROM(); @@ -53,11 +57,10 @@ class FlashEEPROM { void update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd); void update_system_config(uint16_t config); void increment_error_count(void); - FlashEepromStatus save_angle(float angle,bool saved); + FlashEepromStatus save_angle(u16 angle, bool saved); float read_angle(); private: - }; void flash_eeprom_example_main(void); From d6cf143e87f4ea7524a7b1f994aa6f858b8e6448 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Wed, 17 Dec 2025 02:34:08 +0800 Subject: [PATCH 05/16] =?UTF-8?q?fix(flash):=20=E4=BF=AE=E6=94=B9=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E5=AD=98=E5=82=A8=E7=B1=BB=E5=9E=8B=E4=B8=BAfloat?= =?UTF-8?q?=E5=B9=B6=E6=9B=B4=E6=96=B0=E8=AF=BB=E5=86=99=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 将FlashEEPROM中角度数据的存储类型从u16改为float，并相应地更新了读写函数。 新增readFloat和writeFloat函数以支持浮点数存储，同时调整了虚拟地址类型为u8。 修改了save_angle和read_angle函数实现，确保角度值能正确保存与读取。 优化了格式化函数中的数据解析方式，提高数据存储可靠性。 --- Robot_V2/device/Can.cpp | 2 +- Robot_V2/sys/flash.cpp | 182 ++++++++++++++++++++++++++++++---------- Robot_V2/sys/flash.h | 54 ++++++------ 3 files changed, 169 insertions(+), 69 deletions(-) diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 82859ea..7fcf9a7 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -696,7 +696,7 @@ void CAN_transmiter::communication_check() { FOC.target_velocity = target_velocity_union.f; } else if (receive_message.rx_efid == 0x00010301) { motor_reset(); - flashEEP.save_angle((u16)FOC.rt_angle, false); + flashEEP.save_angle(FOC.rt_angle, false); } else { printf("Unknown CAN ID: %X\n", receive_message.rx_efid); } diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index ea93778..f5ab6c2 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -2,8 +2,10 @@ #include "ang_sensor.h" #include "core_cmFunc.h" #include "gd32c10x_fmc.h" +#include "math.h" #include #include +#include #include FlashEEPROM flashEEP = FlashEEPROM(); @@ -23,15 +25,15 @@ FlashEEPROM::FlashEEPROM() { */ u8 FlashEEPROM::init() { // 读取保存的参数 - u16 angle; + float angle; // 尝试从Flash读取参数，如果不存在则使用默认值 - if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { + if (readFloat(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { angle = 0; // 默认值 - std::printf("角度初始化为: %d\r\n", angle); + std::printf("角度初始化为: %f\r\n", angle); } else { - std::printf("从Flash读取到角度数: %d\r\n", angle); + std::printf("从Flash读取到角度数: %f\r\n", angle); } - KTH7823.angle_set((float)angle); + KTH7823.angle_set(angle); return 0; } @@ -41,9 +43,9 @@ u8 FlashEEPROM::init() { * @param data: 指向存储读取数据的指针 * @retval FlashEepromStatus: 操作状态 */ -FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const { - // 检查参数有效性 - if (data == nullptr || virtualAddr > 0xFFFE) { +FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { + // 参数检查 + if (data == nullptr or virtualAddr > 0xFE) { return FLASH_EEPROM_INVALID_ADDR; } @@ -53,7 +55,7 @@ FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const { u32 data_word = *((u32 *)address); // 检查是否是有效数据（非擦除状态） if (data_word != 0xFFFFFFFF) { - u16 addr = (u16)(data_word & 0xFFFF); + u8 addr = (u8)(data_word & 0xFF); // 找到匹配的虚拟地址 if (addr == virtualAddr) { *data = (u16)((data_word >> 16) & 0xFFFF); @@ -65,15 +67,45 @@ FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const { return FLASH_EEPROM_NOT_FOUND; } +/** + * @brief 从Flash EEPROM读取float数据 + * @param virtualAddr: 虚拟地址 + * @param data: 指向存储读取数据的指针 + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { + // 参数检查 + if (data == nullptr or virtualAddr > 0xFE) { + return FLASH_EEPROM_INVALID_ADDR; + } + // 从Flash末尾开始向前搜索，找到最新的数据 + for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; + address >= EEPROM_START_ADDRESS; address -= SIZE_U32) { + u32 data_word = *((u32 *)address); + // 检查是否是有效数据（非擦除状态） + if (data_word != 0xFFFFFFFF) { + u8 addr = (u8)(data_word & 0xFF); + // 找到匹配的虚拟地址 + if (addr == virtualAddr) { + // 使用完整32位数据解释为float + memcpy(data, &data_word, sizeof(float)); + return FLASH_EEPROM_OK; + } + } + } + // 未找到指定虚拟地址的数据 + return FLASH_EEPROM_NOT_FOUND; +} + /** * @brief 向Flash EEPROM写入数据 * @param virtualAddr: 虚拟地址 * @param data: 要写入的数据 * @retval FlashEepromStatus: 操作状态 */ -FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) { +FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { // 参数检查 - if (virtualAddr > 0xFFFE) { + if (virtualAddr > 0xFE) { return FLASH_EEPROM_INVALID_ADDR; } // 查找下一个可写入的位置 @@ -97,6 +129,7 @@ FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) { // 如果有足够空间，直接写入 if (next_address < EEPROM_END_ADDRESS) { + // 新格式: [addr:8bit][reserved:8bit][data:16bit] u32 data_word = ((u32)data << 16) | (u32)virtualAddr; // 使用GD32库函数进行安全编程 @@ -119,7 +152,8 @@ FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) { } // 格式化后在页面开头写入数据 - u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF); + // 新格式: [addr:8bit][reserved:8bit][data:16bit] + u32 new_data_word = ((u32)data << 16) | (u32)virtualAddr; fmc_state_enum fmc_status = fmc_word_program(EEPROM_START_ADDRESS, new_data_word); @@ -131,6 +165,84 @@ FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) { } } +/** + * @brief 向Flash EEPROM写入float数据 + * @param virtualAddr: 虚拟地址 + * @param data: 要写入的数据 + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { + // 参数检查 + if (virtualAddr > 0xFE) { + return FLASH_EEPROM_INVALID_ADDR; + } + // 查找下一个可写入的位置 + u32 next_address = EEPROM_START_ADDRESS; + + // 遍历Flash页面寻找下一个可写入位置 + for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; + address += SIZE_U32) { + + // 安全读取Flash数据 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word = *flash_ptr; + // 找到空位置 + if (data_word == 0xFFFFFFFF) { + next_address = address; + break; + } + // 更新下一个可能的写入位置（如果需要格式化） + next_address = address + SIZE_U32; + } + + // 如果有足够空间，直接写入 + if (next_address < EEPROM_END_ADDRESS) { + u32 float_bits; + + u32 data_word; + // 将完整的float数据复制到32位变量中 + memcpy(&data_word, &data, sizeof(float)); + // 插入虚拟地址到低8位 + data_word = (data_word & 0xFFFFFF00) | (u32)virtualAddr; + + // 使用GD32库函数进行安全编程 + fmc_unlock(); + fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); + + // 确保使用对齐地址 + fmc_state_enum fmc_status = fmc_word_program(next_address, data_word); + + fmc_lock(); + if (fmc_status != FMC_READY) { + return FLASH_EEPROM_WRITE_ERROR; + } + return FLASH_EEPROM_OK; + } else { + // 空间不足，需要格式化页面 + FlashEepromStatus status = format(); + if (status != FLASH_EEPROM_OK) { + return status; + } + + // 格式化后在页面开头写入数据 + u32 float_bits; + + u32 data_word; + // 将完整的float数据复制到32位变量中 + memcpy(&data_word, &data, sizeof(float)); + // 插入虚拟地址到低8位 + data_word = (data_word & 0xFFFFFF00) | (u32)virtualAddr; + + fmc_state_enum fmc_status = + fmc_word_program(EEPROM_START_ADDRESS, data_word); + + if (fmc_status != FMC_READY) { + return FLASH_EEPROM_WRITE_ERROR; + } + return FLASH_EEPROM_OK; + } +} + /** * @brief 格式化Flash EEPROM页面 * @param None @@ -150,24 +262,25 @@ FlashEepromStatus FlashEEPROM::format() { // 检查是否是有效数据项 if (data_word != 0xFFFFFFFF) { - u16 addr = (u16)(data_word & 0xFFFF); - u16 value = (u16)((data_word >> 16) & 0xFFFF); + u8 addr = (u8)(data_word & 0xFF); + u32 value = (data_word & 0xFFFFFF00); // 保留高24位 // 查找是否已经有相同虚拟地址的数据项 uint8_t found = 0; for (u16 i = 0; i < valid_count; i++) { - u16 existing_addr = (u16)(valid_entries[i] & 0xFFFF); + u8 existing_addr = (u8)(valid_entries[i] & 0xFF); if (existing_addr == addr) { // 更新为最新的数据 - valid_entries[i] = ((u32)value << 16) | addr; + valid_entries[i] = value | addr; found = 1; break; } } - // 如果是新的虚拟地址，添加到有效数据列表 + // 如果是新的虚拟地址/类型组合，添加到有效数据列表 if (!found && valid_count < (EEPROM_MAX_SIZE / SIZE_U32)) { - valid_entries[valid_count] = ((u32)value << 16) | addr; + valid_entries[valid_count] = + ((u32)value << 16) | ((u32)value << 8) | addr; valid_count++; } } @@ -191,17 +304,6 @@ FlashEepromStatus FlashEEPROM::format() { return FLASH_EEPROM_OK; } -/** - * @brief 更新参数并保存到Flash - * @param kp 新的KP值 - * @param ki 新的KI值 - * @param kd 新的KD值 - */ -void FlashEEPROM::update_pid_parameters(u16 kp, u16 ki, u16 kd) { - write(ADDR_MOTOR_KP, kp); - write(ADDR_MOTOR_KI, ki); - write(ADDR_MOTOR_KD, kd); -} /** * @brief 更新系统配置并保存到Flash @@ -216,25 +318,26 @@ void FlashEEPROM::update_system_config(u16 config) { * @param angle 角度数据 * @return FlashEepromStatus: 操作状态 */ -FlashEepromStatus FlashEEPROM::save_angle(u16 angle, bool saved) { - u16 angle_read = 0; +FlashEepromStatus FlashEEPROM::save_angle(float angle, bool saved) { + float angle_read = 0; FlashEepromStatus status = FLASH_EEPROM_ERROR; // 读取保存的参数 if (!angle_saved) { // 写入参数 - status = write(ADDR_ANGLE_DATA, angle); + status = writeFloat(ADDR_ANGLE_DATA, angle); - if (read(ADDR_ANGLE_DATA, &angle_read) != FLASH_EEPROM_OK) + if (readFloat(ADDR_ANGLE_DATA, &angle_read) != FLASH_EEPROM_OK) angle_read = 0; // 读不到角度值，存储失败 - if (angle_read != angle) { + if (fabsf(angle_read - angle) > 0.01 * angle) { status = FLASH_EEPROM_WRITE_ERROR; CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态 } else { angle_saved = saved; // 标记已保存过 CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态 } + printf("read:%f\r\n", angle_read); } return status; } @@ -244,11 +347,11 @@ FlashEepromStatus FlashEEPROM::save_angle(u16 angle, bool saved) { * @return float: 角度数据 */ float FlashEEPROM::read_angle() { - u16 angle = 0; - if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { + float angle = 0; + if (readFloat(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { angle = 0; } - return (float)angle; // 默认返回0度 + return angle; // 默认返回0度 } /** @@ -277,11 +380,6 @@ void flash_eeprom_example_main(void) { // 初始化Flash EEPROM flashEEP.init(); - - // 模拟参数更新 - std::printf("\r\n--- 更新PID参数 ---\r\n"); - flashEEP.update_pid_parameters(120, 80, 30); - // 模拟系统配置更新 std::printf("\r\n--- 更新系统配置 ---\r\n"); flashEEP.update_system_config(0x03); diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 7e6025c..4029b15 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -5,59 +5,61 @@ #include #include // 修改后 - 强制4字节对齐 -#define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 +#define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 // EEPROM操作状态 typedef enum { - FLASH_EEPROM_OK = 0, // 操作成功 - FLASH_EEPROM_ERROR, // 操作失败 - FLASH_EEPROM_NOT_FOUND, // 未找到指定数据 - FLASH_EEPROM_INVALID_ADDR, // 无效地址 - FLASH_EEPROM_WRITE_ERROR, // 写入错误 - FLASH_EEPROM_ERASE_ERROR, // 擦除错误 - FLASH_EEPROM_ALIGNMENT_ERROR,// 对齐错误 + FLASH_EEPROM_OK = 0, // 操作成功 + FLASH_EEPROM_ERROR, // 操作失败 + FLASH_EEPROM_NOT_FOUND, // 未找到指定数据 + FLASH_EEPROM_INVALID_ADDR, // 无效地址 + FLASH_EEPROM_WRITE_ERROR, // 写入错误 + FLASH_EEPROM_ERASE_ERROR, // 擦除错误 + FLASH_EEPROM_ALIGNMENT_ERROR, // 对齐错误 } FlashEepromStatus; // Flash EEPROM类 class FlashEEPROM { public: // EEPROM emulation constants - static constexpr uint32_t EEPROM_START_ADDRESS = + static constexpr u32 EEPROM_START_ADDRESS = 0x0801F800; // Flash最后一页(2KB) - static constexpr uint32_t EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 - static constexpr uint32_t EEPROM_PAGE_SIZE = 0x800; // 页大小2KB - static constexpr uint32_t EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 + static constexpr u32 EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 + static constexpr u32 EEPROM_PAGE_SIZE = 0x800; // 页大小2KB + static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 // 定义用户自定义的虚拟地址 - static constexpr uint16_t ADDR_MOTOR_KP = 0x0010; // 电机KP参数地址 - static constexpr uint16_t ADDR_MOTOR_KI = 0x0011; // 电机KI参数地址 - static constexpr uint16_t ADDR_MOTOR_KD = 0x0012; // 电机KD参数地址 - static constexpr uint16_t ADDR_SYSTEM_CONFIG = 0x0020; // 系统配置参数地址 - static constexpr uint16_t ADDR_ERROR_COUNT = 0x0030; // 错误计数地址 + static constexpr u8 ADDR_MOTOR_KP = 0x10; // 电机KP参数地址 + static constexpr u8 ADDR_MOTOR_KI = 0x11; // 电机KI参数地址 + static constexpr u8 ADDR_MOTOR_KD = 0x12; // 电机KD参数地址 + static constexpr u8 ADDR_SYSTEM_CONFIG = 0x20; // 系统配置参数地址 + static constexpr u8 ADDR_ERROR_COUNT = 0x30; // 错误计数地址 - // 角度数据存储地址 - static constexpr u16 ADDR_ANGLE_DATA = 0x0013; + static constexpr u8 ADDR_ANGLE_DATA = 0x01; // 角度数据存储地址 bool angle_saved = false; + // 构造函数 FlashEEPROM(); // 初始化Flash EEPROM模拟 u8 init(); - // 从Flash EEPROM读取数据 - FlashEepromStatus read(uint16_t virtualAddr, uint16_t *data) const; - + // 从Flash EEPROM中读取数据 + FlashEepromStatus read(u8 virtualAddr, u16 *data) const; // 向Flash EEPROM写入数据 - FlashEepromStatus write(uint16_t virtualAddr, uint16_t data); + FlashEepromStatus write(u8 virtualAddr, u16 data); + // Float数据读相关函数 + FlashEepromStatus readFloat(u8 virtualAddr, float *data) const; + // Float数据存储相关函数 + FlashEepromStatus writeFloat(u8 virtualAddr, float data); // 格式化Flash EEPROM页面 FlashEepromStatus format(); void flash_eeprom_example_init(void); - void update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd); - void update_system_config(uint16_t config); + void update_system_config(u16 config); void increment_error_count(void); - FlashEepromStatus save_angle(u16 angle, bool saved); + FlashEepromStatus save_angle(float angle, bool saved); float read_angle(); private: From 7c2860f302d1825bf518e4c27007bbaf6d622e95 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Thu, 18 Dec 2025 18:23:34 +0800 Subject: [PATCH 06/16] =?UTF-8?q?feat(can):=20=E5=A2=9E=E5=8A=A0EEPROM?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E7=A0=81=E5=92=8C=E7=B3=BB=E7=BB=9F=E9=85=8D?= =?UTF-8?q?=E7=BD=AE=E4=BF=9D=E5=AD=98=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 在CAN通信中增加EEPROM状态码字段，并通过CAN FD发送 - 实现Flash EEPROM对系统配置参数的CRC校验与保存 - 改进目标角度、速度等关键参数的读写逻辑，支持掉电保存 - 调整EEPROM地址映射，优化错误处理和状态反馈机制 - 修复CRC计算相关问题，增强数据可靠性 --- Robot_V2/Algorithm/CRC.cpp | 133 ++++++++++++++ Robot_V2/Algorithm/CRC.h | 85 +++++++++ Robot_V2/Robot_V2.cproject.yml | 2 + Robot_V2/device/Can.cpp | 10 +- Robot_V2/device/Can.h | 2 +- Robot_V2/main.cpp | 5 +- Robot_V2/sys/flash.cpp | 307 ++++++++++++++++++++------------- Robot_V2/sys/flash.h | 30 ++-- Robot_V2/sys/health.cpp | 3 +- Robot_V2/sys/systick.cpp | 190 +++++++++++--------- Robot_V2/sys/systick.h | 4 + 11 files changed, 546 insertions(+), 225 deletions(-) create mode 100644 Robot_V2/Algorithm/CRC.cpp create mode 100644 Robot_V2/Algorithm/CRC.h diff --git a/Robot_V2/Algorithm/CRC.cpp b/Robot_V2/Algorithm/CRC.cpp new file mode 100644 index 0000000..e965895 --- /dev/null +++ b/Robot_V2/Algorithm/CRC.cpp @@ -0,0 +1,133 @@ +#include "CRC.h" +#include +#include + +CRC_Calculator CRC_Calculator; +// 静态成员初始化 +bool CRC_Calculator::table_initialized = false; +u32 CRC_Calculator::crc_table[256]; + +/** + * @brief 初始化CRC表 + */ +void CRC_Calculator::init_crc_table() { + if (table_initialized) { + return; + } + + uint32_t polynomial = 0xEDB88320; // CRC-32 IEEE 802.3标准多项式 + + for (int i = 0; i < 256; i++) { + uint32_t crc = i; + for (int j = 0; j < 8; j++) { + if (crc & 1) { + crc = (crc >> 1) ^ polynomial; + } else { + crc >>= 1; + } + } + crc_table[i] = crc; + } + + table_initialized = true; +} + +/** + * @brief 更新CRC值 + * @param crc 当前CRC值 + * @param data 要处理的数据 + * @return 更新后的CRC值 + */ +uint32_t CRC_Calculator::update_crc(uint32_t crc, uint8_t data) { + // 确保表已初始化 + if (!table_initialized) { + init_crc_table(); + } + + uint8_t table_index = (crc ^ data) & 0xFF; + return (crc >> 8) ^ crc_table[table_index]; +} + +/** + * @brief 计算单个float值的CRC32 + * @param data float值 + * @return CRC32校验值 + */ +uint32_t CRC_Calculator::calculate(const float& data) { + // 强制初始化CRC表（如果尚未初始化） + if (!table_initialized) { + init_crc_table(); + } + + uint32_t crc = 0xFFFFFFFF; + + // 将float值按字节处理 + const uint8_t* bytes = reinterpret_cast(&data); + for (size_t i = 0; i < sizeof(float); i++) { + crc = update_crc(crc, bytes[i]); + } + + return crc ^ 0xFFFFFFFF; +} + +/** + * @brief 计算float数组的CRC32 + * @param data float数组指针 + * @param length 数组长度 + * @return CRC32校验值 + */ +uint32_t CRC_Calculator::calculate(const float* data, size_t length) { + // 强制初始化CRC表（如果尚未初始化） + if (!table_initialized) { + init_crc_table(); + } + + uint32_t crc = 0xFFFFFFFF; + + // 处理每个float值 + for (size_t i = 0; i < length; i++) { + const uint8_t* bytes = reinterpret_cast(&(data[i])); + for (size_t j = 0; j < sizeof(float); j++) { + crc = update_crc(crc, bytes[j]); + } + } + + return crc ^ 0xFFFFFFFF; +} + +/** + * @brief 验证单个float值的CRC32 + * @param data float值 + * @param expected_crc 期望的CRC值 + * @return true表示校验成功，false表示校验失败 + */ +bool CRC_Calculator::verify(const float& data, uint32_t expected_crc) { + return calculate(data) == expected_crc; +} + +/** + * @brief 验证float数组的CRC32 + * @param data float数组指针 + * @param length 数组长度 + * @param expected_crc 期望的CRC值 + * @return true表示校验成功，false表示校验失败 + */ +bool CRC_Calculator::verify(const float* data, size_t length, uint32_t expected_crc) { + return calculate(data, length) == expected_crc; +} + +/** + * @brief 获取CRC表初始化状态 + * @return true表示已初始化，false表示未初始化 + */ +bool CRC_Calculator::isInitialized() { + return table_initialized; +} + +/** + * @brief 强制重新初始化CRC表 + */ +void CRC_Calculator::reinitialize() { + table_initialized = false; + init_crc_table(); +} \ No newline at end of file diff --git a/Robot_V2/Algorithm/CRC.h b/Robot_V2/Algorithm/CRC.h new file mode 100644 index 0000000..8ad7965 --- /dev/null +++ b/Robot_V2/Algorithm/CRC.h @@ -0,0 +1,85 @@ +#ifndef __CRC_H +#define __CRC_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#include "gd32c10x.h" + +#ifdef __cplusplus +} +#endif + +#ifdef __cplusplus + +#include + +class CRC_Calculator { +private: + static bool table_initialized; + static uint32_t crc_table[256]; + + /** + * @brief 初始化CRC表 + */ + static void init_crc_table(); + + /** + * @brief 更新CRC值 + * @param crc 当前CRC值 + * @param data 要处理的数据 + * @return 更新后的CRC值 + */ + static uint32_t update_crc(uint32_t crc, uint8_t data); + +public: + /** + * @brief 计算单个float值的CRC32 + * @param data float值 + * @return CRC32校验值 + */ + static uint32_t calculate(const float& data); + + /** + * @brief 计算float数组的CRC32 + * @param data float数组指针 + * @param length 数组长度 + * @return CRC32校验值 + */ + static uint32_t calculate(const float* data, size_t length); + + /** + * @brief 验证单个float值的CRC32 + * @param data float值 + * @param expected_crc 期望的CRC值 + * @return true表示校验成功，false表示校验失败 + */ + static bool verify(const float& data, uint32_t expected_crc); + + /** + * @brief 验证float数组的CRC32 + * @param data float数组指针 + * @param length 数组长度 + * @param expected_crc 期望的CRC值 + * @return true表示校验成功，false表示校验失败 + */ + static bool verify(const float* data, size_t length, uint32_t expected_crc); + + /** + * @brief 获取CRC表初始化状态 + * @return true表示已初始化，false表示未初始化 + */ + static bool isInitialized(); + + /** + * @brief 强制重新初始化CRC表 + */ + static void reinitialize(); +}; + +#endif // __cplusplus + +#endif // __CRC_H \ No newline at end of file diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index e38f8de..bd0c077 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -35,6 +35,8 @@ project: - file: Algorithm/DengFOC.h - file: Algorithm/pid.cpp - file: pid.h + - file: Algorithm/CRC.cpp + - file: Algorithm/CRC.h components: - component: ARM::CMSIS:CORE - component: Device:Startup diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 7fcf9a7..6fe7994 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -437,16 +437,17 @@ u8 CAN_transmiter::init() { \brief CAN 数据更新 */ void CAN_transmiter::update(void) { + target_angle_union.f = FOC.target_angle; + target_velocity_union.f = FOC.target_velocity; limit_velocity_union.f = FOC.limit_velocity; limit_voltage = FOC.limit_voltage * 1000; 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 = FOC.rt_angle; - // rt_velocity_union.f = KTH7823.getVelocity(); rt_velocity_union.f = FOC.rt_velocity; MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET; - + EEPROMStatus = flashEEP.EEPROMStatus; Target_Control_Mode = FOC.control_mode; controller_Mode = FOC.control_mode; @@ -634,8 +635,7 @@ u8 CAN_transmiter::CANFD_all_Send() { transmit_data[10] = (transmit_data[10] & ~0x07) | ((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) | - ((ThermalStatus & 0x01) << 2) | ((EEPROMStatus & 0x01) << 3)); - + ((ThermalStatus & 0x01) << 2) | ((EEPROMStatus & 0x07) << 3)); 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); @@ -697,6 +697,8 @@ void CAN_transmiter::communication_check() { } else if (receive_message.rx_efid == 0x00010301) { motor_reset(); flashEEP.save_angle(FOC.rt_angle, false); + flashEEP.save_system_config(false); + } else { printf("Unknown CAN ID: %X\n", receive_message.rx_efid); } diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 9e17da1..856dc80 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -82,6 +82,7 @@ class CAN_transmiter { bool MotorStatus = 0; // 电机状态码 float_union_t target_angle_union = {0}; // 目标角度 u8 Target_Control_Mode = 0; // 目标控制模式 + u8 EEPROMStatus = 0; // EEPROM状态码 float_union_t target_velocity_union = {0}; // 目标速度 float_union_t u_current_union = {0}; // U相电流 float_union_t w_current_union = {0}; // W相电流 @@ -110,7 +111,6 @@ class CAN_transmiter { bool MagStatus = 0; // 磁状态码 bool ThermalStatus = 0; // 热能状态码 - bool EEPROMStatus = 0; // EEPROM状态码 void communication_check(); // 通信检查 }; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index a6baacf..fdadb19 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -45,8 +45,9 @@ int main(void) { std::printf("初始化失败\r\n"); } - watchdog_init(500); // 看门狗初始化 - angsens_MG_check(); // 角度传感器磁感应强度测试 + watchdog_init(500); // 看门狗初始化 + angsens_MG_check(); // 角度传感器磁感应强度测试 + SystemCoreClockUpdate(); // 更新系统时钟变量 while (1) { __disable_irq(); u32 currentTasks = diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index f5ab6c2..29bddd6 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -1,17 +1,24 @@ #include "flash.h" +#include "DengFOC.h" #include "ang_sensor.h" #include "core_cmFunc.h" #include "gd32c10x_fmc.h" #include "math.h" +#include #include #include +#include #include #include FlashEEPROM flashEEP = FlashEEPROM(); extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; +extern FOC_Controlroller FOC; // FOC控制 +extern CRC_Calculator CRC_Calculator; constexpr u8 SIZE_U32 = sizeof(uint32_t); +constexpr u8 DEFAULT_VALUE = 0; +constexpr u8 NUM_SAVED = 5; // FlashEEPROM类实现 FlashEEPROM::FlashEEPROM() { @@ -25,15 +32,70 @@ FlashEEPROM::FlashEEPROM() { */ u8 FlashEEPROM::init() { // 读取保存的参数 - float angle; - // 尝试从Flash读取参数，如果不存在则使用默认值 - if (readFloat(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { - angle = 0; // 默认值 - std::printf("角度初始化为: %f\r\n", angle); - } else { - std::printf("从Flash读取到角度数: %f\r\n", angle); + u32 CRC_NUM = 0x0000; + FlashEepromStatus status_read[NUM_SAVED + 1] = {FLASH_EEPROM_OK}; + float angle = 0.0f; + float data[NUM_SAVED] = {FOC.target_angle, FOC.target_velocity, + FOC.limit_voltage, (float)FOC.control_mode}; + FlashEepromStatus status = FLASH_EEPROM_OK; + if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) { + angle = DEFAULT_VALUE; // 默认值 } + + // 尝试从Flash读取参数，如果不存在则使用默认值 + status_read[1] = readFloat(ADDR_TARGET_ANGLE, &data[0]); + status_read[2] = readFloat(ADDR_TARGET_VEL, &data[1]); + status_read[3] = readFloat(ADDR_VOLTAGE_LIMIT, &data[2]); + status_read[4] = readFloat(ADDR_CONTROL_MODE, &data[3]); + + // 读取失败则使用默认值 + for (u8 i = 1; i < NUM_SAVED + 1; i++) { + if (status_read[i] == + FLASH_EEPROM_DATA_NOT_FOUND) { // 首次读取，不保存报错 + data[i - 1] = DEFAULT_VALUE; + } else if (status_read[i] != FLASH_EEPROM_OK) { // 其余错误，保存报错 + data[i - 1] = DEFAULT_VALUE; + status_read[0] = status_read[i]; + } + } + + if (status_read[0] == FLASH_EEPROM_OK) { + u32 next_address = EEPROM_END_ADDRESS; + // 遍历Flash页面寻找下一个可写入位置 + for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; + address > EEPROM_START_ADDRESS; address -= SIZE_U32) { + + // 安全读取Flash数据 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word = *flash_ptr; + // 找到空位置 + if (data_word == 0xFFFFFFFF) { + next_address = address + SIZE_U32; + break; + } + // 更新下一个可能的写入位置（如果需要格式化） + next_address = address - SIZE_U32; + } + CRC_NUM = CRC_Calculator::calculate(data, NUM_SAVED); + + if (CRC_NUM != *((u32 *)next_address)) { // CRC错误 + for (u8 i = 0; i < NUM_SAVED; i++) { + data[i] = DEFAULT_VALUE; + } + EEPROMStatus = FLASH_EEPROM_CRC_ERROR; + } else { // CRC正确 + EEPROMStatus = FLASH_EEPROM_OK; + } + } else { + // 读取失败，报错 + EEPROMStatus = status_read[0]; + } + KTH7823.angle_set(angle); + FOC.target_angle = data[0]; + FOC.target_velocity = data[1]; + FOC.limit_voltage = data[2]; + FOC.control_mode = (u8)data[3]; return 0; } @@ -45,7 +107,7 @@ u8 FlashEEPROM::init() { */ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { // 参数检查 - if (data == nullptr or virtualAddr > 0xFE) { + if (data == nullptr) { return FLASH_EEPROM_INVALID_ADDR; } @@ -63,8 +125,7 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { } } } - // 未找到指定虚拟地址的数据 - return FLASH_EEPROM_NOT_FOUND; + return FLASH_EEPROM_DATA_NOT_FOUND; } /** @@ -74,10 +135,11 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { * @retval FlashEepromStatus: 操作状态 */ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { - // 参数检查 - if (data == nullptr or virtualAddr > 0xFE) { + // 检查参数有效性 + if (data == nullptr) { return FLASH_EEPROM_INVALID_ADDR; } + // 从Flash末尾开始向前搜索，找到最新的数据 for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; address >= EEPROM_START_ADDRESS; address -= SIZE_U32) { @@ -87,14 +149,16 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { u8 addr = (u8)(data_word & 0xFF); // 找到匹配的虚拟地址 if (addr == virtualAddr) { - // 使用完整32位数据解释为float - memcpy(data, &data_word, sizeof(float)); + // 清除最低的8位地址信息，保留高24位数据 + u32 float_data = data_word & 0xFFFFFF00; + // 使用内存拷贝将处理后的数据解释为float + memcpy(data, &float_data, sizeof(float)); return FLASH_EEPROM_OK; } } } // 未找到指定虚拟地址的数据 - return FLASH_EEPROM_NOT_FOUND; + return FLASH_EEPROM_DATA_NOT_FOUND; } /** @@ -104,10 +168,6 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { * @retval FlashEepromStatus: 操作状态 */ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { - // 参数检查 - if (virtualAddr > 0xFE) { - return FLASH_EEPROM_INVALID_ADDR; - } // 查找下一个可写入的位置 u32 next_address = EEPROM_START_ADDRESS; @@ -157,12 +217,14 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { fmc_state_enum fmc_status = fmc_word_program(EEPROM_START_ADDRESS, new_data_word); - if (fmc_status != FMC_READY) { + if (fmc_status == FMC_READY) + return FLASH_EEPROM_OK; + if (fmc_status == FMC_PGAERR) + return FLASH_EEPROM_ALIGNMENT_ERROR; + else if (fmc_status != FMC_READY) return FLASH_EEPROM_WRITE_ERROR; - } - - return FLASH_EEPROM_OK; } + return FLASH_EEPROM_WRITE_ERROR; } /** @@ -172,10 +234,6 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { * @retval FlashEepromStatus: 操作状态 */ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { - // 参数检查 - if (virtualAddr > 0xFE) { - return FLASH_EEPROM_INVALID_ADDR; - } // 查找下一个可写入的位置 u32 next_address = EEPROM_START_ADDRESS; @@ -232,15 +290,18 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { memcpy(&data_word, &data, sizeof(float)); // 插入虚拟地址到低8位 data_word = (data_word & 0xFFFFFF00) | (u32)virtualAddr; - + fmc_unlock(); fmc_state_enum fmc_status = fmc_word_program(EEPROM_START_ADDRESS, data_word); - - if (fmc_status != FMC_READY) { + fmc_lock(); + if (fmc_status == FMC_READY) + return FLASH_EEPROM_OK; + if (fmc_status == FMC_PGAERR) + return FLASH_EEPROM_ALIGNMENT_ERROR; + else if (fmc_status != FMC_READY) return FLASH_EEPROM_WRITE_ERROR; - } - return FLASH_EEPROM_OK; } + return FLASH_EEPROM_WRITE_ERROR; } /** @@ -276,7 +337,6 @@ FlashEepromStatus FlashEEPROM::format() { break; } } - // 如果是新的虚拟地址/类型组合，添加到有效数据列表 if (!found && valid_count < (EEPROM_MAX_SIZE / SIZE_U32)) { valid_entries[valid_count] = @@ -291,7 +351,7 @@ FlashEepromStatus FlashEEPROM::format() { if (fmc_status != FMC_READY) { return FLASH_EEPROM_ERASE_ERROR; } - + fmc_unlock(); // 重新写入有效的数据项 for (u16 i = 0; i < valid_count; i++) { fmc_status = fmc_word_program(EEPROM_START_ADDRESS + i * SIZE_U32, @@ -300,17 +360,91 @@ FlashEepromStatus FlashEEPROM::format() { return FLASH_EEPROM_WRITE_ERROR; } } + fmc_lock(); return FLASH_EEPROM_OK; } - /** * @brief 更新系统配置并保存到Flash * @param config 新的系统配置值 */ -void FlashEEPROM::update_system_config(u16 config) { - write(ADDR_SYSTEM_CONFIG, config); +FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { + u32 CRC_NUM = 0x0000; + u32 next_address; + FlashEepromStatus status_save[NUM_SAVED] = {FLASH_EEPROM_OK}; + float data[NUM_SAVED] = {FOC.target_angle, FOC.target_velocity, + FOC.limit_voltage, (float)FOC.control_mode}; + + // 读取保存的参数 + if (!system_config_saved) { + // 写入参数 + status_save[0] = writeFloat(ADDR_TARGET_ANGLE, data[0]); + status_save[1] = writeFloat(ADDR_TARGET_VEL, data[1]); + status_save[2] = writeFloat(ADDR_VOLTAGE_LIMIT, data[2]); + status_save[3] = writeFloat(ADDR_CONTROL_MODE, data[3]); + for (FlashEepromStatus &p : status_save) { + if (p != FLASH_EEPROM_OK) { + EEPROMStatus = p; + return p; + } + } + + CRC_NUM = CRC_Calculator.calculate(data, NUM_SAVED); + // 遍历Flash页面寻找下一个可写入位置 + for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; + address > EEPROM_START_ADDRESS; address -= SIZE_U32) { + + // 安全读取Flash数据 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word = *flash_ptr; + // 找到空位置 + if (data_word == 0xFFFFFFFF) { + next_address = address; + break; + } + // 更新下一个可能的写入位置（如果需要格式化） + next_address = address - SIZE_U32; + } + // 如果有足够空间，直接写入 + if (next_address > EEPROM_START_ADDRESS) { + fmc_unlock(); + fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); + fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM); + // printf("write address:0x%X,status:0x%x \r\n", next_address, + // fmc_status); + // printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 + // *)next_address)); + fmc_lock(); + } else { + // 空间不足，需要格式化页面 + FlashEepromStatus status = format(); + if (status != FLASH_EEPROM_OK) { + return status; + } + // 格式化后在页面底部写入数据 + fmc_unlock(); + fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); + fmc_state_enum fmc_status = + fmc_word_program(EEPROM_END_ADDRESS - SIZE_U32, CRC_NUM); + fmc_lock(); + if (fmc_status == FMC_READY) + return FLASH_EEPROM_OK; + if (fmc_status == FMC_PGAERR) + return FLASH_EEPROM_ALIGNMENT_ERROR; + else if (fmc_status != FMC_READY) + return FLASH_EEPROM_WRITE_ERROR; + } + if (CRC_NUM == *((u32 *)next_address)) { + system_config_saved = saved; // 标记已保存过 + EEPROMStatus = FLASH_EEPROM_OK; // 设置EEPROM状态 + } else { + EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置EEPROM错误状态 + return FLASH_EEPROM_CRC_ERROR; + } + } + EEPROMStatus = FLASH_EEPROM_OK; + return FLASH_EEPROM_OK; } /** @@ -319,27 +453,30 @@ void FlashEEPROM::update_system_config(u16 config) { * @return FlashEepromStatus: 操作状态 */ FlashEepromStatus FlashEEPROM::save_angle(float angle, bool saved) { - float angle_read = 0; - FlashEepromStatus status = FLASH_EEPROM_ERROR; + float angle_read = DEFAULT_VALUE; + FlashEepromStatus status = FLASH_EEPROM_OK; // 读取保存的参数 if (!angle_saved) { // 写入参数 - status = writeFloat(ADDR_ANGLE_DATA, angle); + status = writeFloat(ADDR_ANGLE, angle); + if (status != FLASH_EEPROM_OK) { + EEPROMStatus = status; + return status; + } - if (readFloat(ADDR_ANGLE_DATA, &angle_read) != FLASH_EEPROM_OK) - angle_read = 0; // 读不到角度值，存储失败 + if (readFloat(ADDR_ANGLE, &angle_read) != FLASH_EEPROM_OK) + angle_read = DEFAULT_VALUE; // 读不到角度值，存储失败 - if (fabsf(angle_read - angle) > 0.01 * angle) { - status = FLASH_EEPROM_WRITE_ERROR; - CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态 + if (fabsf(angle_read - angle) > 0.01 * fabs(angle)) { + EEPROMStatus = FLASH_EEPROM_WRITE_ERROR; // 设置EEPROM错误状态 + return FLASH_EEPROM_WRITE_ERROR; } else { - angle_saved = saved; // 标记已保存过 - CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态 + angle_saved = saved; // 标记已保存过 + EEPROMStatus = FLASH_EEPROM_OK; // 设置EEPROM状态 } - printf("read:%f\r\n", angle_read); } - return status; + return FLASH_EEPROM_OK; } /** @@ -347,73 +484,9 @@ FlashEepromStatus FlashEEPROM::save_angle(float angle, bool saved) { * @return float: 角度数据 */ float FlashEEPROM::read_angle() { - float angle = 0; - if (readFloat(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) { - angle = 0; + float angle = DEFAULT_VALUE; + if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) { + angle = DEFAULT_VALUE; } - return angle; // 默认返回0度 + return angle; } - -/** - * @brief 增加错误计数并保存到Flash - */ -void FlashEEPROM::increment_error_count(void) { - u16 error_count = 0; - - // 读取当前错误计数 - FlashEepromStatus read_status = read(ADDR_ERROR_COUNT, &error_count); - if (read_status == FLASH_EEPROM_OK || - read_status == FLASH_EEPROM_NOT_FOUND) { - // 增加错误计数（如果是NOT_FOUND，则error_count已经是0） - error_count++; - - // 保存更新后的错误计数 - write(ADDR_ERROR_COUNT, error_count); - } -} - -/** - * @brief 示例主函数 - 展示Flash EEPROM的使用方法 - */ -void flash_eeprom_example_main(void) { - std::printf("\r\n=== Flash EEPROM 使用示例 ===\r\n"); - - // 初始化Flash EEPROM - flashEEP.init(); - // 模拟系统配置更新 - std::printf("\r\n--- 更新系统配置 ---\r\n"); - flashEEP.update_system_config(0x03); - - // 模拟错误发生 - std::printf("\r\n--- 模拟错误计数增加 ---\r\n"); - flashEEP.increment_error_count(); - flashEEP.increment_error_count(); - - // 再次读取参数验证保存是否成功 - std::printf("\r\n--- 验证保存的数据 ---\r\n"); - u16 kp_value, ki_value, kd_value; - u16 system_config; - u16 error_count; - - // if (flashEEP.read(ADDR_MOTOR_KP, &kp_value) == FLASH_EEPROM_OK) { - // std::printf("当前KP值: %d\r\n", kp_value); - // } - - // if (flashEEP.read(ADDR_MOTOR_KI, &ki_value) == FLASH_EEPROM_OK) { - // std::printf("当前KI值: %d\r\n", ki_value); - // } - - // if (flashEEP.read(ADDR_MOTOR_KD, &kd_value) == FLASH_EEPROM_OK) { - // std::printf("当前KD值: %d\r\n", kd_value); - // } - - // if (flashEEP.read(ADDR_SYSTEM_CONFIG, &system_config) == FLASH_EEPROM_OK) - // { std::printf("当前系统配置: 0x%02X\r\n", system_config); - // } - - // if (flashEEP.read(ADDR_ERROR_COUNT, &error_count) == FLASH_EEPROM_OK) { - // std::printf("当前错误计数: %d\r\n", error_count); - // } - - std::printf("\r\n=== 示例结束 ===\r\n"); -} \ No newline at end of file diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 4029b15..5891b6b 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -10,11 +10,12 @@ // EEPROM操作状态 typedef enum { FLASH_EEPROM_OK = 0, // 操作成功 - FLASH_EEPROM_ERROR, // 操作失败 - FLASH_EEPROM_NOT_FOUND, // 未找到指定数据 - FLASH_EEPROM_INVALID_ADDR, // 无效地址 FLASH_EEPROM_WRITE_ERROR, // 写入错误 + FLASH_EEPROM_READ_ERROR, // 读取错误 FLASH_EEPROM_ERASE_ERROR, // 擦除错误 + FLASH_EEPROM_INVALID_ADDR, // 无效地址 + FLASH_EEPROM_DATA_NOT_FOUND, // 找不到数据 + FLASH_EEPROM_CRC_ERROR, // CRC错误 FLASH_EEPROM_ALIGNMENT_ERROR, // 对齐错误 } FlashEepromStatus; @@ -23,21 +24,20 @@ class FlashEEPROM { public: // EEPROM emulation constants static constexpr u32 EEPROM_START_ADDRESS = - 0x0801F800; // Flash最后一页(2KB) + 0x0801F800; // Flash最后一页(2KB) static constexpr u32 EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 - static constexpr u32 EEPROM_PAGE_SIZE = 0x800; // 页大小2KB - static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 + static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 // 定义用户自定义的虚拟地址 - static constexpr u8 ADDR_MOTOR_KP = 0x10; // 电机KP参数地址 - static constexpr u8 ADDR_MOTOR_KI = 0x11; // 电机KI参数地址 - static constexpr u8 ADDR_MOTOR_KD = 0x12; // 电机KD参数地址 - static constexpr u8 ADDR_SYSTEM_CONFIG = 0x20; // 系统配置参数地址 - static constexpr u8 ADDR_ERROR_COUNT = 0x30; // 错误计数地址 + static constexpr u8 ADDR_CONTROL_MODE = 0x0; // 控制模式 + static constexpr u8 ADDR_ANGLE = 0x01; // 角度数据存储地址 + static constexpr u8 ADDR_TARGET_ANGLE = 0x02; // 角度命令数据存储地址 + static constexpr u8 ADDR_TARGET_VEL = 0x03; // 速度命令数据存储地址 + static constexpr u8 ADDR_VOLTAGE_LIMIT = 0x4; // 电压限制 - static constexpr u8 ADDR_ANGLE_DATA = 0x01; // 角度数据存储地址 bool angle_saved = false; - + bool system_config_saved = false; + u8 EEPROMStatus; // 构造函数 FlashEEPROM(); @@ -56,10 +56,8 @@ class FlashEEPROM { // 格式化Flash EEPROM页面 FlashEepromStatus format(); - void flash_eeprom_example_init(void); - void update_system_config(u16 config); - void increment_error_count(void); FlashEepromStatus save_angle(float angle, bool saved); + FlashEepromStatus save_system_config(bool saved); float read_angle(); private: diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index 10742ab..a0e4de6 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -96,7 +96,8 @@ void power_manage(void) { float vbus = sampler.get_vbus_voltage(); if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) { FOC.shut_down(); - // flashEEP.save_angle(FOC.rt_angle,false); + flashEEP.save_angle(FOC.rt_angle, true); + flashEEP.save_system_config(true); } else { FOC.resume(); } diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index cab013a..2f584fc 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -1,117 +1,139 @@ /*! - \file systick.c - \brief the systick configuration file - - \version 2025-06-20, V1.5.1, firmware for GD32C10x + \file systick.c + \brief the systick configuration file + + \version 2025-06-20, V1.5.1, firmware for GD32C10x */ /* - Copyright (c) 2024, GigaDevice Semiconductor Inc. + Copyright (c) 2024, GigaDevice Semiconductor Inc. - Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: + Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: - 1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - 3. Neither the name of the copyright holder nor the names of its contributors - may be used to endorse or promote products derived from this software without - specific prior written permission. + 1. Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its +contributors may be used to endorse or promote products derived from this +software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. -IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT -NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY -OF SUCH DAMAGE. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "gd32c10x.h" #include "systick.h" +#include "gd32c10x.h" +#include "gd32c10x_exti.h" +#include "gd32c10x_gpio.h" volatile static uint32_t delay; /*! - \brief configure systick - \param[in] none - \param[out] none - \retval none + \brief configure systick + \param[in] none + \param[out] none + \retval none */ -void systick_config(void) -{ - /* setup systick timer for 1000Hz interrupts */ - if (SysTick_Config(SystemCoreClock / 1000U)){ - /* capture error */ - while (1){ - } - } - /* configure the systick handler priority */ - NVIC_SetPriority(SysTick_IRQn, 0x00U); +void systick_config(void) { + /* setup systick timer for 1000Hz interrupts */ + if (SysTick_Config(SystemCoreClock / 1000U)) { + /* capture error */ + while (1) { + } + } + /* configure the systick handler priority */ + NVIC_SetPriority(SysTick_IRQn, 0x00U); } /*! - \brief delay a time in seconds - \param[in] count: count in seconds - \param[out] none - \retval none + \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); - } +void delay_1s(uint32_t count) { + uint32_t i; + for (i = 0; i < count; i++) { + delay_1ms(1000); + } } /*! - \brief delay a time in milliseconds - \param[in] count: count in milliseconds - \param[out] none - \retval none + \brief delay a time in milliseconds + \param[in] count: count in milliseconds + \param[out] none + \retval none */ -void delay_1ms(uint32_t count) -{ - delay = count; - while(0U != delay){ +void delay_1ms(uint32_t count) { + delay = count; + while (0U != delay) { fwdgt_counter_reload(); fwdgt_counter_reload(); - } + } } /*! - \brief delay decrement - \param[in] none - \param[out] none - \retval none + \brief delay decrement + \param[in] none + \param[out] none + \retval none */ -void delay_decrement(void) -{ - if (0U != delay){ - delay--; - } +void delay_decrement(void) { + if (0U != delay) { + delay--; + } } /*! - \brief system soft reset - \param[in] none - \param[out] none - \retval none + \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(); - } +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(); + } +} + +// 1. 进入深度睡眠前的准备 +void prepare_deep_sleep(void) { + // 将SIT1044设置为低功耗待机模式 + gpio_bit_set(GPIOB, GPIO_PIN_8); // STB = 高电平 + + // 配置EXTI中断（连接到SIT1044的RXD引脚） + exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_FALLING); // 下降沿触发 + + // 进入深度睡眠 + pmu_to_deepsleepmode(PMU_LDO_LOWPOWER, WFI_CMD); +} + +// 2. 唤醒中断处理 +void EXTI1_IRQHandler(void) { + if (exti_interrupt_flag_get(EXTI_1) != RESET) { + // 清除中断标志 + exti_interrupt_flag_clear(EXTI_1); + // 禁用EXTI线1的中断 + exti_interrupt_disable(EXTI_1); + + // 唤醒SIT1044到正常模式 + gpio_bit_reset(GPIOB, GPIO_PIN_8); // STB = 低电平 + } } \ No newline at end of file diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index 5294ccb..7604179 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -54,6 +54,10 @@ void idle_task_count(void); uint8_t get_cpu_usage(void); /* 系统软复位 */ void system_soft_reset(void); +/* 准备进入低功耗模式 */ +void prepare_deep_sleep(void); +/* 中断唤醒 */ +void EXTI1_IRQHandler(void); #ifdef __cplusplus } From 7354a71c5155b3437e9effee268ab5e447495513 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sat, 20 Dec 2025 19:53:52 +0800 Subject: [PATCH 07/16] =?UTF-8?q?feat(can):=20=E5=A2=9E=E5=8A=A0CAN?= =?UTF-8?q?=E7=9D=A1=E7=9C=A0=E4=B8=8E=E5=94=A4=E9=86=92=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E5=B9=B6=E4=BC=98=E5=8C=96=E9=80=9A=E4=BF=A1=E6=A3=80=E6=9F=A5?= =?UTF-8?q?=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 在CAN通信类中添加了CAN_Sleep()和CAN_Wakeup()方法，支持设备进入低功耗模式 - 优化CAN通信检查逻辑，通过解析接收数据的首字节和末字节实现多种控制命令 - 支持远程复位电机、保存角度、清零角度、进入睡眠等操作 - 解决CAN发送邮箱满时可能导致的通信阻塞问题，增加重初始化机制 - 调整mailbox不可用计数逻辑，防止持续打印错误信息 refactor(flash): 移除软件CRC计算模块，改用硬件CRC外设 - 删除自定义CRC.cpp和CRC.h文件及其相关类实现 - 使用GD32硬件CRC模块替代软件算法，提高效率和可靠性 - 更新FlashEEPROM类中的CRC校验方式，调用crc_block_data_calculate进行计算 - 修正Flash地址遍历范围以适配新CRC逻辑及页面管理策略 - 修改save_angle接口签名，不再手动传入角度值，改为内部读取FOC实时角度 feat(timer): 新增RTC闹钟中断处理函数声明 - 在Timer.h中声明rtc_set_wakeup_interval和RTC_ALARM_IRQHandler - 为后续实现基于RTC的定时唤醒功能做准备 chore(config): 同步csolution工具版本更新并调整组件配置 - 更新RTE_Components.h生成工具版本至2.12.0 - 启用BKP、CRC、RTC等标准外设组件宏定义 - 从项目配置文件中移除旧CRC源文件引用 - 添加SystemCoreClockUpdate调用确保系统时钟同步 refactor(main): 主循环任务调度重构并增强初始化流程 - 将主循环中的任务处理提取为独立do_task函数，提升代码可读性 - 提前执行电机初始化，并根据看门狗复位标志决定是否立即进入深度睡眠 - 调整EEPROM初始化顺序，保证依赖关系正确 - 添加sys/initial.h头文件包含以支持新增系统初始化逻辑 --- Robot_V2.csolution.yml | 3 +- Robot_V2/Algorithm/CRC.cpp | 133 ------------- Robot_V2/Algorithm/CRC.h | 85 -------- Robot_V2/{ => Algorithm}/pid.h | 0 .../RTE/_Release_GD32C103CB/RTE_Components.h | 8 +- Robot_V2/Robot_V2.cproject.yml | 7 +- Robot_V2/device/Can.cpp | 60 +++++- Robot_V2/device/Can.h | 66 ++++--- Robot_V2/device/Timer.cpp | 1 - Robot_V2/device/ang_sensor.cpp | 11 +- Robot_V2/main.cpp | 181 ++++++++++-------- Robot_V2/main.h | 2 + Robot_V2/sys/flash.cpp | 71 ++++--- Robot_V2/sys/flash.h | 11 +- Robot_V2/sys/health.cpp | 2 +- Robot_V2/sys/initial.cpp | 67 +++++-- Robot_V2/sys/initial.h | 2 + Robot_V2/sys/systick.cpp | 70 +++++-- Robot_V2/sys/systick.h | 4 +- 19 files changed, 366 insertions(+), 418 deletions(-) delete mode 100644 Robot_V2/Algorithm/CRC.cpp delete mode 100644 Robot_V2/Algorithm/CRC.h rename Robot_V2/{ => Algorithm}/pid.h (100%) diff --git a/Robot_V2.csolution.yml b/Robot_V2.csolution.yml index 5b0406c..54608ea 100644 --- a/Robot_V2.csolution.yml +++ b/Robot_V2.csolution.yml @@ -33,9 +33,8 @@ solution: - project-context: Robot_V2.Release debugger: name: ST-Link@pyOCD - port: 3333 - protocol: swd clock: 10000000 + protocol: swd # List of different build configurations. build-types: diff --git a/Robot_V2/Algorithm/CRC.cpp b/Robot_V2/Algorithm/CRC.cpp deleted file mode 100644 index e965895..0000000 --- a/Robot_V2/Algorithm/CRC.cpp +++ /dev/null @@ -1,133 +0,0 @@ -#include "CRC.h" -#include -#include - -CRC_Calculator CRC_Calculator; -// 静态成员初始化 -bool CRC_Calculator::table_initialized = false; -u32 CRC_Calculator::crc_table[256]; - -/** - * @brief 初始化CRC表 - */ -void CRC_Calculator::init_crc_table() { - if (table_initialized) { - return; - } - - uint32_t polynomial = 0xEDB88320; // CRC-32 IEEE 802.3标准多项式 - - for (int i = 0; i < 256; i++) { - uint32_t crc = i; - for (int j = 0; j < 8; j++) { - if (crc & 1) { - crc = (crc >> 1) ^ polynomial; - } else { - crc >>= 1; - } - } - crc_table[i] = crc; - } - - table_initialized = true; -} - -/** - * @brief 更新CRC值 - * @param crc 当前CRC值 - * @param data 要处理的数据 - * @return 更新后的CRC值 - */ -uint32_t CRC_Calculator::update_crc(uint32_t crc, uint8_t data) { - // 确保表已初始化 - if (!table_initialized) { - init_crc_table(); - } - - uint8_t table_index = (crc ^ data) & 0xFF; - return (crc >> 8) ^ crc_table[table_index]; -} - -/** - * @brief 计算单个float值的CRC32 - * @param data float值 - * @return CRC32校验值 - */ -uint32_t CRC_Calculator::calculate(const float& data) { - // 强制初始化CRC表（如果尚未初始化） - if (!table_initialized) { - init_crc_table(); - } - - uint32_t crc = 0xFFFFFFFF; - - // 将float值按字节处理 - const uint8_t* bytes = reinterpret_cast(&data); - for (size_t i = 0; i < sizeof(float); i++) { - crc = update_crc(crc, bytes[i]); - } - - return crc ^ 0xFFFFFFFF; -} - -/** - * @brief 计算float数组的CRC32 - * @param data float数组指针 - * @param length 数组长度 - * @return CRC32校验值 - */ -uint32_t CRC_Calculator::calculate(const float* data, size_t length) { - // 强制初始化CRC表（如果尚未初始化） - if (!table_initialized) { - init_crc_table(); - } - - uint32_t crc = 0xFFFFFFFF; - - // 处理每个float值 - for (size_t i = 0; i < length; i++) { - const uint8_t* bytes = reinterpret_cast(&(data[i])); - for (size_t j = 0; j < sizeof(float); j++) { - crc = update_crc(crc, bytes[j]); - } - } - - return crc ^ 0xFFFFFFFF; -} - -/** - * @brief 验证单个float值的CRC32 - * @param data float值 - * @param expected_crc 期望的CRC值 - * @return true表示校验成功，false表示校验失败 - */ -bool CRC_Calculator::verify(const float& data, uint32_t expected_crc) { - return calculate(data) == expected_crc; -} - -/** - * @brief 验证float数组的CRC32 - * @param data float数组指针 - * @param length 数组长度 - * @param expected_crc 期望的CRC值 - * @return true表示校验成功，false表示校验失败 - */ -bool CRC_Calculator::verify(const float* data, size_t length, uint32_t expected_crc) { - return calculate(data, length) == expected_crc; -} - -/** - * @brief 获取CRC表初始化状态 - * @return true表示已初始化，false表示未初始化 - */ -bool CRC_Calculator::isInitialized() { - return table_initialized; -} - -/** - * @brief 强制重新初始化CRC表 - */ -void CRC_Calculator::reinitialize() { - table_initialized = false; - init_crc_table(); -} \ No newline at end of file diff --git a/Robot_V2/Algorithm/CRC.h b/Robot_V2/Algorithm/CRC.h deleted file mode 100644 index 8ad7965..0000000 --- a/Robot_V2/Algorithm/CRC.h +++ /dev/null @@ -1,85 +0,0 @@ -#ifndef __CRC_H -#define __CRC_H - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#include "gd32c10x.h" - -#ifdef __cplusplus -} -#endif - -#ifdef __cplusplus - -#include - -class CRC_Calculator { -private: - static bool table_initialized; - static uint32_t crc_table[256]; - - /** - * @brief 初始化CRC表 - */ - static void init_crc_table(); - - /** - * @brief 更新CRC值 - * @param crc 当前CRC值 - * @param data 要处理的数据 - * @return 更新后的CRC值 - */ - static uint32_t update_crc(uint32_t crc, uint8_t data); - -public: - /** - * @brief 计算单个float值的CRC32 - * @param data float值 - * @return CRC32校验值 - */ - static uint32_t calculate(const float& data); - - /** - * @brief 计算float数组的CRC32 - * @param data float数组指针 - * @param length 数组长度 - * @return CRC32校验值 - */ - static uint32_t calculate(const float* data, size_t length); - - /** - * @brief 验证单个float值的CRC32 - * @param data float值 - * @param expected_crc 期望的CRC值 - * @return true表示校验成功，false表示校验失败 - */ - static bool verify(const float& data, uint32_t expected_crc); - - /** - * @brief 验证float数组的CRC32 - * @param data float数组指针 - * @param length 数组长度 - * @param expected_crc 期望的CRC值 - * @return true表示校验成功，false表示校验失败 - */ - static bool verify(const float* data, size_t length, uint32_t expected_crc); - - /** - * @brief 获取CRC表初始化状态 - * @return true表示已初始化，false表示未初始化 - */ - static bool isInitialized(); - - /** - * @brief 强制重新初始化CRC表 - */ - static void reinitialize(); -}; - -#endif // __cplusplus - -#endif // __CRC_H \ No newline at end of file diff --git a/Robot_V2/pid.h b/Robot_V2/Algorithm/pid.h similarity index 100% rename from Robot_V2/pid.h rename to Robot_V2/Algorithm/pid.h diff --git a/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h b/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h index f92e443..8ac4eac 100644 --- a/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h +++ b/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h @@ -1,6 +1,6 @@ /* * CSOLUTION generated file: DO NOT EDIT! - * Generated by: csolution version 2.11.0 + * Generated by: csolution version 2.12.0 * * Project: 'Robot_V2.Release+GD32C103CB' * Target: 'Release+GD32C103CB' @@ -17,8 +17,12 @@ /* GigaDevice::Device:GD32C10x_StdPeripherals:ADC@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_ADC +/* GigaDevice::Device:GD32C10x_StdPeripherals:BKP@1.0.0 */ +#define RTE_DEVICE_STDPERIPHERALS_BKP /* GigaDevice::Device:GD32C10x_StdPeripherals:CAN@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_CAN +/* GigaDevice::Device:GD32C10x_StdPeripherals:CRC@1.0.0 */ +#define RTE_DEVICE_STDPERIPHERALS_CRC /* GigaDevice::Device:GD32C10x_StdPeripherals:DBG@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_DBG /* GigaDevice::Device:GD32C10x_StdPeripherals:DMA@1.0.0 */ @@ -37,6 +41,8 @@ #define RTE_DEVICE_STDPERIPHERALS_PMU /* GigaDevice::Device:GD32C10x_StdPeripherals:RCU@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_RCU +/* GigaDevice::Device:GD32C10x_StdPeripherals:RTC@1.0.0 */ +#define RTE_DEVICE_STDPERIPHERALS_RTC /* GigaDevice::Device:GD32C10x_StdPeripherals:SPI_I2S@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_SPI /* GigaDevice::Device:GD32C10x_StdPeripherals:TIMER@1.0.0 */ diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index bd0c077..58e3019 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -34,9 +34,7 @@ project: - file: Algorithm/DengFOC.cpp - file: Algorithm/DengFOC.h - file: Algorithm/pid.cpp - - file: pid.h - - file: Algorithm/CRC.cpp - - file: Algorithm/CRC.h + - file: Algorithm/pid.h components: - component: ARM::CMSIS:CORE - component: Device:Startup @@ -57,6 +55,9 @@ project: - component: Device:EVAL:GD32C10x - component: Device:GD32C10x_StdPeripherals:FWDGT - component: Device:GD32C10x_StdPeripherals:WWDGT + - component: Device:GD32C10x_StdPeripherals:BKP + - component: Device:GD32C10x_StdPeripherals:CRC + - component: Device:GD32C10x_StdPeripherals:RTC output: type: - elf diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 6fe7994..9f94b77 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -3,11 +3,11 @@ #include "Filter.h" #include "adc.h" #include "ang_sensor.h" -#include "core_cmFunc.h" #include "flash.h" #include "gd32c10x.h" #include "gd32c10x_can.h" #include "gd32c10x_dma.h" +#include "gd32c10x_gpio.h" #include "motor.h" #include "systick.h" #include @@ -28,7 +28,8 @@ CAN_transmiter CAN0_transmiter; // CAN初始化 extern FOC_Controlroller FOC; extern sampler sampler; extern angle_sensor KTH7823; -extern EncoderVelocityKalman Vel_kalman; +extern EncoderAngleKalman Angle_kalman; // 角度滤波 +extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern FlashEEPROM flashEEP; // 添加DMA相关变量 @@ -566,8 +567,14 @@ u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) { // 检查是否成功获取mailbox if (CAN_NOMAILBOX == transmit_mailbox) { + static u32 count = 0; // 没有可用的mailbox，可能所有mailbox都在使用中 + count++; printf("No available mailbox for CANFD transmission\r\n"); + if (count > 0xff and !is_sleep) { + init(); + count = 0; + } return 0; } return 1; @@ -695,10 +702,27 @@ void CAN_transmiter::communication_check() { unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f; } else if (receive_message.rx_efid == 0x00010301) { - motor_reset(); - flashEEP.save_angle(FOC.rt_angle, false); - flashEEP.save_system_config(false); - + u8 first_byte = receive_message.rx_data[0]; + u8 eigth_byte = receive_message.rx_data[7]; + if (first_byte & 0x01) { + motor_reset(); + } + if (first_byte >> 1 & 0x01) { + flashEEP.save_angle(false); + } + if (first_byte >> 2 & 0x01) { + flashEEP.save_system_config(false); + } + if (first_byte >> 3 & 0x01) { + KTH7823.angle_set(0); + Angle_kalman.init(0); // 角度滤波 + Vel_kalman.init(0); // 角度滤波 + } + if (eigth_byte & 0x80 >> 7) { + prepare_deep_sleep(true); + // CAN_Sleep(); + } + } else { printf("Unknown CAN ID: %X\n", receive_message.rx_efid); } @@ -706,6 +730,30 @@ void CAN_transmiter::communication_check() { } } +/** + * @brief CAN睡眠 + * @param None + * @retval None + */ +u8 CAN_transmiter::CAN_Sleep() { + gpio_bit_set(GPIOB, GPIO_PIN_13); + is_sleep = true; + return 0; +} + +/** + * @brief CAN唤醒 + * @param None + * @retval None + */ +u8 CAN_transmiter::CAN_Wakeup() { + printf("can wake\r\n"); + is_sleep = false; + canfd_config(); + + return 0; +} + /** * @brief CAN0中断服务程序 * @retval 无 diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 856dc80..1cd3384 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -53,11 +53,11 @@ static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) { return float_union; } -u8 can_gpio_init(void); // CAN引脚初始化 -void can_config(void); // CAN初始化 -void canfd_config(void);// CANFD初始化 -void can_dma_init(void); // CAN DMA初始化 -void CAN0_RX0_IRQHandler(void);// CAN接收中断处理函数 +u8 can_gpio_init(void); // CAN引脚初始化 +void can_config(void); // CAN初始化 +void canfd_config(void); // CANFD初始化 +void can_dma_init(void); // CAN DMA初始化 +void CAN0_RX0_IRQHandler(void); // CAN接收中断处理函数 void DMA0_Channel4_IRQHandler(void); // DMA通道4中断处理函数(CAN接收) void DMA0_Channel5_IRQHandler(void); // DMA通道5中断处理函数(CAN发送) @@ -71,47 +71,51 @@ void can_loopback_test(void); class CAN_transmiter { private: - u8 controller_Mode = 0; // 电机模式 - float_union_t limit_velocity_union = {0}; // 限制速度 - u16 limit_voltage = 0; // 限制电压 - short mcu_temp = 0; // MCU温度 - short motor_temp = 0; // 电机温度 - u16 Power_Voltage = 0; // 电源电压 - float_union_t rt_angle_union = {0}; // 实时角度 - float_union_t rt_velocity_union = {0}; // 实时速度 - bool MotorStatus = 0; // 电机状态码 - float_union_t target_angle_union = {0}; // 目标角度 - u8 Target_Control_Mode = 0; // 目标控制模式 - u8 EEPROMStatus = 0; // EEPROM状态码 - float_union_t target_velocity_union = {0}; // 目标速度 - float_union_t u_current_union = {0}; // U相电流 - float_union_t w_current_union = {0}; // W相电流 - float_union_t v_current_union = {0}; // V相电流 + bool is_sleep = false; // 是否休眠 + + u8 controller_Mode = 0; // 电机模式 + float_union_t limit_velocity_union = {0}; // 限制速度 + u16 limit_voltage = 0; // 限制电压 + short mcu_temp = 0; // MCU温度 + short motor_temp = 0; // 电机温度 + u16 Power_Voltage = 0; // 电源电压 + float_union_t rt_angle_union = {0}; // 实时角度 + float_union_t rt_velocity_union = {0}; // 实时速度 + bool MotorStatus = 0; // 电机状态码 + float_union_t target_angle_union = {0}; // 目标角度 + u8 Target_Control_Mode = 0; // 目标控制模式 + u8 EEPROMStatus = 0; // EEPROM状态码 + float_union_t target_velocity_union = {0}; // 目标速度 + float_union_t u_current_union = {0}; // U相电流 + float_union_t w_current_union = {0}; // W相电流 + float_union_t v_current_union = {0}; // V相电流 can_trasnmit_message_struct transmit_message; // 发送消息结构体 u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态 u8 transmit_data[64]; // CANFD最大64字节 - void update(); // 更新数据 + void update(); // 更新数据 u8 CAN_Receive(); // CAN接收 u8 CANFD_Receive(); // CANFD接收 u8 CAN_Send(u32 ID, u8 length, u8 *data); // CAN发送 - u8 CANFD_Send(u32 ID, u8 length, u8 *data); // CANFD发送 - u8 CAN_all_Send(); // CAN发送 + u8 CANFD_Send(u32 ID, u8 length, u8 *data); // CANFD发送 + u8 CAN_all_Send(); // CAN发送 u8 CANFD_all_Send(); // CANFD发送 public: // 添加构造函数声明 u8 init(void); - can_receive_message_struct receive_message; // 接收消息结构体 - FlagStatus can0_receive_flag; // CAN0接收标志 - FlagStatus can0_send_flag; // CAN0发送标志 - float_union_t testfloat_union = {0}; // 测试用 + can_receive_message_struct receive_message; // 接收消息结构体 + FlagStatus can0_receive_flag; // CAN0接收标志 + FlagStatus can0_send_flag; // CAN0发送标志 + float_union_t testfloat_union = {0}; // 测试用 - bool MagStatus = 0; // 磁状态码 - bool ThermalStatus = 0; // 热能状态码 - void communication_check(); // 通信检查 + bool MagStatus = 0; // 磁状态码 + bool ThermalStatus = 0; // 热能状态码 + void communication_check(); // 通信检查 + u8 CAN_Sleep(); // CAN进入睡眠 + u8 CAN_Wakeup(); // CAN进入睡眠 }; #ifdef __cplusplus diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 5d62455..c59074a 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -97,7 +97,6 @@ void User_timer_init(void) { nvic_config(); } - /** * @brief TIMER2中断处理函数 * @return void diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 06d2b1d..6b8467e 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -216,7 +216,8 @@ u8 angle_sensor::init(void) { * @return 0成功/1失败 */ u8 angle_sensor::angle_set(float angle){ - m_cumulativeAngle = flashEEP.read_angle()*REDUCTION_RATIO; // 从Flash读取上次保存的角度 + m_cumulativeAngle = angle*REDUCTION_RATIO; // 从Flash读取上次保存的角度 + m_firstUpdate = true; return 0; } @@ -303,11 +304,3 @@ float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; } float angle_sensor::getcurrent_sensorangle(void) const { return current_angle_deg; } - -/** - * @brief 重置累积角度值 - */ -void angle_sensor::reset(void) { - m_cumulativeAngle = 0.0f; - m_firstUpdate = true; -} \ No newline at end of file diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index fdadb19..42b4855 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -11,43 +11,52 @@ #include "health.h" #include "initial.h" #include "motor.h" +#include "sys/initial.h" #include "systick.h" #include #include +extern CAN_transmiter CAN0_transmiter; // CAN初始化 +extern sampler sampler; // 采样器 +extern angle_sensor KTH7823; // 角度传感器 +extern EncoderVelocityKalman Vel_kalman; // 速度滤波 +extern EncoderAngleKalman Angle_kalman; // 角度滤波 +extern ThermalController thermalController; // 热量控制器 +extern FOC_Controlroller FOC; // FOC控制 +extern FlashEEPROM flashEEP; // EEPROM类 TaskScheduler scheduler = {0}; volatile u32 system_runtime_us = 0; volatile u32 cnt_test = 0; int main(void) { - systick_config(); // 系统时钟初始化 - unused_pin_init(); // 未使用引脚初始化 - USART0_init(); // 调试串口初始化 + systick_config(); // 系统时钟初始化 + SystemCoreClockUpdate(); // 更新系统时钟变量 + watchdog_init(50); // 看门狗初始化 + unused_pin_init(); // 未使用引脚初始化 + USART0_init(); // 调试串口初始化 + motor_init(); // 电机驱动初始化 + if (get_reset_cause() == + RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠 + printf("continue sleep\r\n"); + watchdog_init(5000); // 看门狗初始化 + delay_1ms(1); + prepare_deep_sleep(false); + } User_timer_init(); // 任务Timer初始化 - motor_init(); // 电机驱动初始化 - - extern CAN_transmiter CAN0_transmiter; // CAN初始化 - extern sampler sampler; // 采样器 - extern angle_sensor KTH7823; // 角度传感器 - extern EncoderVelocityKalman Vel_kalman; // 速度滤波 - extern EncoderAngleKalman Angle_kalman; // 角度滤波 - extern ThermalController thermalController; // 热量控制器 - extern FOC_Controlroller FOC; // FOC控制 - extern FlashEEPROM flashEEP; // EEPROM类 - u8 init_status = 0; - init_status += flashEEP.init(); // EEPROM初始化 - init_status += KTH7823.init(); // 角度传感器初始化 - init_status += sampler.init(); // 采样器初始化 - init_status += CAN0_transmiter.init(); // CAN初始化 + init_status += KTH7823.init(); // 角度传感器初始化 + init_status += sampler.init(); // 采样器初始化 + init_status += + flashEEP.init(); // EEPROM初始化，必须放在角度卡尔曼之前，角度传感器之后 + init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 - while (init_status) { // 初始化检测 + init_status += CAN0_transmiter.init(); // CAN初始化 + + while (init_status) { // 初始化检测 std::printf("初始化失败\r\n"); } - watchdog_init(500); // 看门狗初始化 - angsens_MG_check(); // 角度传感器磁感应强度测试 - SystemCoreClockUpdate(); // 更新系统时钟变量 + angsens_MG_check(); // 角度传感器磁感应强度测试 while (1) { __disable_irq(); u32 currentTasks = @@ -55,69 +64,71 @@ int main(void) { scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志 __enable_irq(); - // 按照优先级和重要性处理任务 - if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 - sampler.update_current(); - } - if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值 - KTH7823.update(); - FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()); - } - if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值 - FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity()); - } - if (currentTasks & - (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) - system_runtime_us++; - switch (FOC.control_mode) { - case 0: { - FOC.drv_hold(0.95); // 占空比置0 - break; - } - case 1: { - FOC.angle_control(FOC.target_angle); // 角度闭环控制 - // FOC.angle_control_with_velocity_limit(FOC.target_angle, - // FOC.limit_velocity); // 角度闭环控制，带速度限制 - break; - } - case 2: { - FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 - break; - } - case 3: { - // FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 - FOC.current_control(FOC.limit_velocity); // 电流闭环控制 - break; - } - case 4: { - FOC.velocity_open_loop(); // 速度开环控制 - break; - } - } - } - if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值 - sampler.update_mcu_temp(); - } - if (currentTasks & - (1 << TASK_DRV_TEMP_UPDATE)) { // 更新电机温度传感器值 - sampler.update_drv_temp(); - } - if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制 - thermalController.update(); - } - if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测 - angsens_MG_check(); - } - if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 - power_manage(); - } - if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 - // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 - cnt_test = 0; - CAN0_transmiter.can0_send_flag = SET; - CAN0_transmiter.communication_check(); - fwdgt_counter_reload(); // 看门狗复位 - } + do_task(currentTasks); } return 0; } + +void do_task(u32 currentTasks) { + // 按照优先级和重要性处理任务 + if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 + sampler.update_current(); + } + if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值 + KTH7823.update(); + FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()); + } + if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值 + FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity()); + } + if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) + system_runtime_us++; + switch (FOC.control_mode) { + case 0: { + FOC.drv_hold(0.95); // 占空比置0 + break; + } + case 1: { + FOC.angle_control(FOC.target_angle); // 角度闭环控制 + // FOC.angle_control_with_velocity_limit(FOC.target_angle, + // FOC.limit_velocity); // 角度闭环控制，带速度限制 + break; + } + case 2: { + FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 + break; + } + case 3: { + // FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 + FOC.current_control(FOC.limit_velocity); // 电流闭环控制 + break; + } + case 4: { + FOC.velocity_open_loop(); // 速度开环控制 + break; + } + } + } + if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值 + sampler.update_mcu_temp(); + } + if (currentTasks & (1 << TASK_DRV_TEMP_UPDATE)) { // 更新电机温度传感器值 + sampler.update_drv_temp(); + } + if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制 + thermalController.update(); + } + if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测 + angsens_MG_check(); + } + if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 + power_manage(); + } + if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 + // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 + cnt_test = 0; + CAN0_transmiter.can0_send_flag = SET; + CAN0_transmiter.communication_check(); + fwdgt_counter_reload(); // 看门狗复位 + } +} \ No newline at end of file diff --git a/Robot_V2/main.h b/Robot_V2/main.h index 86aa1aa..a8426d5 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 "stdint.h" +void do_task(u32 currentTasks); #endif /* MAIN_H */ diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 29bddd6..b554a58 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -1,11 +1,11 @@ #include "flash.h" #include "DengFOC.h" #include "ang_sensor.h" -#include "core_cmFunc.h" +#include "gd32c10x_crc.h" #include "gd32c10x_fmc.h" #include "math.h" -#include #include +#include #include #include #include @@ -15,7 +15,6 @@ FlashEEPROM flashEEP = FlashEEPROM(); extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; extern FOC_Controlroller FOC; // FOC控制 -extern CRC_Calculator CRC_Calculator; constexpr u8 SIZE_U32 = sizeof(uint32_t); constexpr u8 DEFAULT_VALUE = 0; constexpr u8 NUM_SAVED = 5; @@ -31,12 +30,15 @@ FlashEEPROM::FlashEEPROM() { * @retval None */ u8 FlashEEPROM::init() { + rcu_periph_clock_enable(RCU_CRC); + // 使用硬件CRC计算校验值 + crc_deinit(); // 初始化CRC模块 + // 读取保存的参数 u32 CRC_NUM = 0x0000; - FlashEepromStatus status_read[NUM_SAVED + 1] = {FLASH_EEPROM_OK}; + FlashEepromStatus status_read[NUM_SAVED] = {FLASH_EEPROM_OK}; float angle = 0.0f; - float data[NUM_SAVED] = {FOC.target_angle, FOC.target_velocity, - FOC.limit_voltage, (float)FOC.control_mode}; + float data[NUM_SAVED - 1] = {0.0f, 0.0f, 0.0f, 0.0f}; FlashEepromStatus status = FLASH_EEPROM_OK; if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) { angle = DEFAULT_VALUE; // 默认值 @@ -49,7 +51,7 @@ u8 FlashEEPROM::init() { status_read[4] = readFloat(ADDR_CONTROL_MODE, &data[3]); // 读取失败则使用默认值 - for (u8 i = 1; i < NUM_SAVED + 1; i++) { + for (u8 i = 1; i < NUM_SAVED; i++) { if (status_read[i] == FLASH_EEPROM_DATA_NOT_FOUND) { // 首次读取，不保存报错 data[i - 1] = DEFAULT_VALUE; @@ -62,8 +64,8 @@ u8 FlashEEPROM::init() { if (status_read[0] == FLASH_EEPROM_OK) { u32 next_address = EEPROM_END_ADDRESS; // 遍历Flash页面寻找下一个可写入位置 - for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; - address > EEPROM_START_ADDRESS; address -= SIZE_U32) { + for (u32 address = EEPROM_START_ADDRESS - SIZE_U32; + address > FLASH_START_ADDRESS; address -= SIZE_U32) { // 安全读取Flash数据 volatile u32 *flash_ptr = (volatile u32 *)address; @@ -76,8 +78,13 @@ u8 FlashEEPROM::init() { // 更新下一个可能的写入位置（如果需要格式化） next_address = address - SIZE_U32; } - CRC_NUM = CRC_Calculator::calculate(data, NUM_SAVED); + crc_data_register_reset(); // 确保CRC计算从初始状态开始 + CRC_NUM = crc_block_data_calculate((uint32_t *)data, NUM_SAVED); + // for (float &p : data) { + // printf("%f\r\n", p); + // } + // printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); if (CRC_NUM != *((u32 *)next_address)) { // CRC错误 for (u8 i = 0; i < NUM_SAVED; i++) { data[i] = DEFAULT_VALUE; @@ -91,7 +98,7 @@ u8 FlashEEPROM::init() { EEPROMStatus = status_read[0]; } - KTH7823.angle_set(angle); + KTH7823.angle_set(angle - KTH7823.update()); FOC.target_angle = data[0]; FOC.target_velocity = data[1]; FOC.limit_voltage = data[2]; @@ -149,10 +156,12 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { u8 addr = (u8)(data_word & 0xFF); // 找到匹配的虚拟地址 if (addr == virtualAddr) { + u32 data_word_c = *((u32 *)address); // 清除最低的8位地址信息，保留高24位数据 u32 float_data = data_word & 0xFFFFFF00; // 使用内存拷贝将处理后的数据解释为float memcpy(data, &float_data, sizeof(float)); + // printf("check:0x%x=%f\r\n", address, *data); return FLASH_EEPROM_OK; } } @@ -255,8 +264,6 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { // 如果有足够空间，直接写入 if (next_address < EEPROM_END_ADDRESS) { - u32 float_bits; - u32 data_word; // 将完整的float数据复制到32位变量中 memcpy(&data_word, &data, sizeof(float)); @@ -271,6 +278,11 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { fmc_state_enum fmc_status = fmc_word_program(next_address, data_word); fmc_lock(); + + // u32 data_word_c = *((u32 *)next_address); + // float float_data = *((float *)&data_word_c); + // printf("check:0x%x=%f\r\n", next_address, float_data); + if (fmc_status != FMC_READY) { return FLASH_EEPROM_WRITE_ERROR; } @@ -283,8 +295,6 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { } // 格式化后在页面开头写入数据 - u32 float_bits; - u32 data_word; // 将完整的float数据复制到32位变量中 memcpy(&data_word, &data, sizeof(float)); @@ -345,13 +355,12 @@ FlashEepromStatus FlashEEPROM::format() { } } } - + fmc_unlock(); // 擦除整个页面 fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); if (fmc_status != FMC_READY) { return FLASH_EEPROM_ERASE_ERROR; } - fmc_unlock(); // 重新写入有效的数据项 for (u16 i = 0; i < valid_count; i++) { fmc_status = fmc_word_program(EEPROM_START_ADDRESS + i * SIZE_U32, @@ -371,10 +380,10 @@ FlashEepromStatus FlashEEPROM::format() { */ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { u32 CRC_NUM = 0x0000; - u32 next_address; - FlashEepromStatus status_save[NUM_SAVED] = {FLASH_EEPROM_OK}; - float data[NUM_SAVED] = {FOC.target_angle, FOC.target_velocity, - FOC.limit_voltage, (float)FOC.control_mode}; + static u32 next_address; + FlashEepromStatus status_save[NUM_SAVED - 1] = {FLASH_EEPROM_OK}; + float data[NUM_SAVED - 1] = {FOC.target_angle, FOC.target_velocity, + FOC.limit_voltage, (float)FOC.control_mode}; // 读取保存的参数 if (!system_config_saved) { @@ -390,11 +399,12 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { } } - CRC_NUM = CRC_Calculator.calculate(data, NUM_SAVED); - // 遍历Flash页面寻找下一个可写入位置 - for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; - address > EEPROM_START_ADDRESS; address -= SIZE_U32) { + crc_data_register_reset(); // 确保CRC计算从初始状态开始 + CRC_NUM = crc_block_data_calculate((uint32_t *)data, NUM_SAVED); + // 遍历Flash页面寻找下一个可写入位置 + for (u32 address = EEPROM_START_ADDRESS - SIZE_U32; + address > FLASH_START_ADDRESS; address -= SIZE_U32) { // 安全读取Flash数据 volatile u32 *flash_ptr = (volatile u32 *)address; u32 data_word = *flash_ptr; @@ -407,16 +417,20 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { next_address = address - SIZE_U32; } // 如果有足够空间，直接写入 - if (next_address > EEPROM_START_ADDRESS) { + if (next_address > FLASH_START_ADDRESS) { fmc_unlock(); fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM); // printf("write address:0x%X,status:0x%x \r\n", next_address, // fmc_status); + fmc_lock(); + // for (float &p : data) { + // printf("%f\r\n", p); + // } // printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 // *)next_address)); - fmc_lock(); } else { + printf("no enough space\r\n"); // 空间不足，需要格式化页面 FlashEepromStatus status = format(); if (status != FLASH_EEPROM_OK) { @@ -452,8 +466,9 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { * @param angle 角度数据 * @return FlashEepromStatus: 操作状态 */ -FlashEepromStatus FlashEEPROM::save_angle(float angle, bool saved) { +FlashEepromStatus FlashEEPROM::save_angle(bool saved) { float angle_read = DEFAULT_VALUE; + float angle = FOC.rt_angle; FlashEepromStatus status = FLASH_EEPROM_OK; // 读取保存的参数 diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 5891b6b..d501619 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -1,8 +1,6 @@ #ifndef __FLASH_H #define __FLASH_H -#include "gd32c10x.h" -#include #include // 修改后 - 强制4字节对齐 #define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 @@ -24,9 +22,10 @@ class FlashEEPROM { public: // EEPROM emulation constants static constexpr u32 EEPROM_START_ADDRESS = - 0x0801F800; // Flash最后一页(2KB) - static constexpr u32 EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 - static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 + 0x0801F800; // Flash最后一页(2KB) + static constexpr u32 EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 + static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 + static constexpr u32 FLASH_START_ADDRESS = 0x08010000; // Flash起始地址 // 定义用户自定义的虚拟地址 static constexpr u8 ADDR_CONTROL_MODE = 0x0; // 控制模式 @@ -56,7 +55,7 @@ class FlashEEPROM { // 格式化Flash EEPROM页面 FlashEepromStatus format(); - FlashEepromStatus save_angle(float angle, bool saved); + FlashEepromStatus save_angle(bool saved); FlashEepromStatus save_system_config(bool saved); float read_angle(); diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index a0e4de6..c78469d 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -96,7 +96,7 @@ void power_manage(void) { float vbus = sampler.get_vbus_voltage(); if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) { FOC.shut_down(); - flashEEP.save_angle(FOC.rt_angle, true); + flashEEP.save_angle(true); flashEEP.save_system_config(true); } else { FOC.resume(); diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index 7e41501..90bdb52 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,9 +1,10 @@ #include "initial.h" +#include "gd32c10x.h" #include "gd32c10x_gpio.h" #include "gd32c10x_rcu.h" +#include #include - - +#include /* retarget the C library printf function to the USART */ u8 USART0_init() { @@ -34,15 +35,59 @@ int fputc(int ch, FILE *f) throw() { return ch; } - -void unused_pin_init() { +void unused_pin_init() { rcu_periph_clock_enable(RCU_GPIOA); // 使能GPIOA时钟 rcu_periph_clock_enable(RCU_GPIOB); // 使能GPIOB时钟 rcu_periph_clock_enable(RCU_GPIOC); // 使能GPIOC时钟 - gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12); - gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15); - gpio_init(GPIOC, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15); - gpio_bit_reset(GPIOA, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12); - gpio_bit_reset(GPIOB, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15); - gpio_bit_reset(GPIOC, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15); -} \ No newline at end of file + gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, + GPIO_PIN_8 | GPIO_PIN_11 | GPIO_PIN_12); + gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, + GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_15); + gpio_init(GPIOC, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, + GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15); + gpio_bit_reset(GPIOA, GPIO_PIN_8 | GPIO_PIN_11 | GPIO_PIN_12); + gpio_bit_reset(GPIOB, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_15); + gpio_bit_reset(GPIOC, GPIO_PIN_13 | 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)) { + // printf("Free Watchdog Timer Reset\r\n"); + cause = RCU_FLAG_FWDGTRST; + } + if (RESET != rcu_flag_get(RCU_FLAG_EPRST)) { + // printf("External Pin Reset\r\n"); + cause = RCU_FLAG_EPRST; + } + + if (RESET != rcu_flag_get(RCU_FLAG_PORRST)) { + // printf("Power Reset\r\n"); + cause = RCU_FLAG_PORRST; + } + + if (RESET != rcu_flag_get(RCU_FLAG_WWDGTRST)) { + // printf("Window Watchdog Timer Reset\r\n"); + cause = RCU_FLAG_WWDGTRST; + } + + if (RESET != rcu_flag_get(RCU_FLAG_LPRST)) { + // printf("Low Power Reset\r\n"); + cause = RCU_FLAG_LPRST; + } + + if (RESET != rcu_flag_get(RCU_FLAG_SWRST)) { + // printf("Software Reset\r\n"); + cause = RCU_FLAG_SWRST; + } + // 清除所有复位标志 + rcu_all_reset_flag_clear(); + // printf("cause:%d\r\n", cause); + return cause; +} diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 5a75963..4330a0e 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -4,6 +4,7 @@ #ifdef __cplusplus extern "C" { #endif + #include "gd32c10x_eval.h" #include "stdint.h" #include @@ -19,6 +20,7 @@ int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 u8 USART0_init(void); u8 watchdog_init(u32 time); +u16 get_reset_cause(void); void unused_pin_init(); // 任务标志定义 typedef enum { diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index 2f584fc..88c8fc0 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -33,11 +33,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "systick.h" +#include "Can.h" +#include "flash.h" #include "gd32c10x.h" #include "gd32c10x_exti.h" -#include "gd32c10x_gpio.h" +#include "motor.h" +#include volatile static uint32_t delay; +extern CAN_transmiter CAN0_transmiter; +extern FlashEEPROM flashEEP; /*! \brief configure systick @@ -113,27 +118,64 @@ void system_soft_reset(void) { } } -// 1. 进入深度睡眠前的准备 -void prepare_deep_sleep(void) { +/*! + \brief prepare system to enter deep sleep mode + \param[in] none + \param[out] none + \retval none +*/ +void prepare_deep_sleep(bool save_data) { + if (save_data) { + flashEEP.save_angle(true); + flashEEP.save_system_config(true); + } // 将SIT1044设置为低功耗待机模式 - gpio_bit_set(GPIOB, GPIO_PIN_8); // STB = 高电平 + // CAN0_transmiter.CAN_Sleep(); + motor_sleep(); - // 配置EXTI中断（连接到SIT1044的RXD引脚） - exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_FALLING); // 下降沿触发 + // 关闭不必要的外设时钟 + rcu_periph_clock_disable(RCU_TIMER0); // 关闭定时器时钟 + rcu_periph_clock_disable(RCU_TIMER1); + rcu_periph_clock_disable(RCU_SPI0); // 关闭SPI时钟 + rcu_periph_clock_disable(RCU_I2C0); // 关闭I2C时钟 + rcu_periph_clock_disable(RCU_ADC0); // 关闭ADC时钟 + // 注意不要关闭GPIOB和AF的时钟，因为需要它们来处理唤醒中断 + + // 设置深睡眠模式下的电压以进一步降低功耗 + rcu_deepsleep_voltage_set(RCU_DEEPSLEEP_V_0_8); // 设置最低电压 + + // 配置PB8为输入模式，用于外部中断 + rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_AF); + gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_8); + + // 配置EXTI中断（连接到PB8引脚） + gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOB, + GPIO_PIN_SOURCE_8); // 配置GPIOB的PIN8作为EXTI8的源 + exti_init(EXTI_8, EXTI_INTERRUPT, EXTI_TRIG_RISING); // 边沿触发 + exti_interrupt_enable(EXTI_8); + nvic_irq_enable(EXTI5_9_IRQn, 0, 0); // + + // 确保中断标志已被清除 + exti_interrupt_flag_clear(EXTI_8); // 进入深度睡眠 pmu_to_deepsleepmode(PMU_LDO_LOWPOWER, WFI_CMD); } -// 2. 唤醒中断处理 -void EXTI1_IRQHandler(void) { - if (exti_interrupt_flag_get(EXTI_1) != RESET) { +/** + * @brief EXTI5_9中断处理函数 + * @param 无 + * @retval 无 + */ +void EXTI5_9_IRQHandler(void) { + if (exti_interrupt_flag_get(EXTI_8) != RESET) { // 清除中断标志 - exti_interrupt_flag_clear(EXTI_1); - // 禁用EXTI线1的中断 - exti_interrupt_disable(EXTI_1); + exti_interrupt_flag_clear(EXTI_8); + // 禁用EXTI线8的中断，防止重复触发 + exti_interrupt_disable(EXTI_8); - // 唤醒SIT1044到正常模式 - gpio_bit_reset(GPIOB, GPIO_PIN_8); // STB = 低电平 + printf("MCU wake up\r\n"); + system_soft_reset(); } } \ No newline at end of file diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index 7604179..5cf51e5 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -55,9 +55,9 @@ uint8_t get_cpu_usage(void); /* 系统软复位 */ void system_soft_reset(void); /* 准备进入低功耗模式 */ -void prepare_deep_sleep(void); +void prepare_deep_sleep(bool save_data); /* 中断唤醒 */ -void EXTI1_IRQHandler(void); +void EXTI5_9_IRQHandler(void); #ifdef __cplusplus } From 8732174855043e6745cc44eb799c48b46d38b213 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Mon, 22 Dec 2025 02:20:25 +0800 Subject: [PATCH 08/16] =?UTF-8?q?refactor(flash):=20=E9=87=8D=E6=9E=84EEPR?= =?UTF-8?q?OM=E6=95=B0=E6=8D=AE=E5=AD=98=E5=82=A8=E9=80=BB=E8=BE=91?= =?UTF-8?q?=E4=BB=A5=E6=8F=90=E5=8D=87=E5=8F=AF=E9=9D=A0=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 修改FlashEEPROM类的数据读写流程，优化地址掩码和数据对齐方式 - 引入新的宏定义和常量用于数据校验及地址解析 - 调整系统配置保存函数，支持更精确的CRC校验机制 - 移除旧的角度保存接口，统一通过系统配置保存函数处理 - 在CAN通信检查中替换save_angle调用为format操作 - 增加调试开关EEPDEBUG控制打印信息输出 - 更新健康管理和系统休眠时的EEPROM保存策略 --- Robot_V2/device/Can.cpp | 2 +- Robot_V2/sys/flash.cpp | 530 +++++++++++++++++++++------------------ Robot_V2/sys/flash.h | 18 +- Robot_V2/sys/health.cpp | 1 - Robot_V2/sys/systick.cpp | 1 - 5 files changed, 297 insertions(+), 255 deletions(-) diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 9f94b77..76059c3 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -708,7 +708,7 @@ void CAN_transmiter::communication_check() { motor_reset(); } if (first_byte >> 1 & 0x01) { - flashEEP.save_angle(false); + flashEEP.format(); } if (first_byte >> 2 & 0x01) { flashEEP.save_system_config(false); diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index b554a58..0a6ebfb 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -16,8 +16,10 @@ extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; extern FOC_Controlroller FOC; // FOC控制 constexpr u8 SIZE_U32 = sizeof(uint32_t); +constexpr u8 SIZE_FLOAT = sizeof(float); constexpr u8 DEFAULT_VALUE = 0; -constexpr u8 NUM_SAVED = 5; +constexpr u8 NUM_SAVED = 6; +#define EEPDEBUG 0 // FlashEEPROM类实现 FlashEEPROM::FlashEEPROM() { @@ -31,27 +33,41 @@ FlashEEPROM::FlashEEPROM() { */ u8 FlashEEPROM::init() { rcu_periph_clock_enable(RCU_CRC); - // 使用硬件CRC计算校验值 crc_deinit(); // 初始化CRC模块 + FlashEepromStatus status = FLASH_EEPROM_OK; + // 读取系统配置 + read_saved_system_config(); + + if (EEPROMStatus == FLASH_EEPROM_OK) { + return FLASH_EEPROM_OK; + } else { + return FLASH_EEPROM_READ_ERROR; + } + return 1; // 正常不会返回这个，未预料的异常情况返回 +} + +/** + * @brief 读取EEPROM数据 + * @param virtualAddr: 虚拟地址 + * @param data: 存储读取数据的指针 + * @retval EEPROMStatus: 是否成功 + */ +u8 FlashEEPROM::read_saved_system_config(void) { // 读取保存的参数 u32 CRC_NUM = 0x0000; - FlashEepromStatus status_read[NUM_SAVED] = {FLASH_EEPROM_OK}; - float angle = 0.0f; - float data[NUM_SAVED - 1] = {0.0f, 0.0f, 0.0f, 0.0f}; - FlashEepromStatus status = FLASH_EEPROM_OK; - if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) { - angle = DEFAULT_VALUE; // 默认值 - } - + FlashEepromStatus status_read[NUM_SAVED + 1] = {FLASH_EEPROM_OK}; + float data[NUM_SAVED] = {0.0f}; // 尝试从Flash读取参数，如果不存在则使用默认值 - status_read[1] = readFloat(ADDR_TARGET_ANGLE, &data[0]); - status_read[2] = readFloat(ADDR_TARGET_VEL, &data[1]); - status_read[3] = readFloat(ADDR_VOLTAGE_LIMIT, &data[2]); - status_read[4] = readFloat(ADDR_CONTROL_MODE, &data[3]); + status_read[1] = readFloat(ADDR_CONTROL_MODE, &data[0]); + status_read[2] = readFloat(ADDR_VOLTAGE_LIMIT, &data[1]); + status_read[3] = readFloat(ADDR_VEL_LIMIT, &data[2]); + status_read[4] = readFloat(ADDR_ANGLE, &data[3]); + status_read[5] = readFloat(ADDR_TARGET_ANGLE, &data[4]); + status_read[6] = readFloat(ADDR_TARGET_VEL, &data[5]); // 读取失败则使用默认值 - for (u8 i = 1; i < NUM_SAVED; i++) { + for (u8 i = 1; i < NUM_SAVED + 1; i++) { if (status_read[i] == FLASH_EEPROM_DATA_NOT_FOUND) { // 首次读取，不保存报错 data[i - 1] = DEFAULT_VALUE; @@ -60,31 +76,34 @@ u8 FlashEEPROM::init() { status_read[0] = status_read[i]; } } - + // 读取到全部数据 if (status_read[0] == FLASH_EEPROM_OK) { u32 next_address = EEPROM_END_ADDRESS; // 遍历Flash页面寻找下一个可写入位置 - for (u32 address = EEPROM_START_ADDRESS - SIZE_U32; - address > FLASH_START_ADDRESS; address -= SIZE_U32) { - + for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32; + address > EEPROM_START_ADDRESS; address -= SIZE_U32) { // 安全读取Flash数据 volatile u32 *flash_ptr = (volatile u32 *)address; u32 data_word = *flash_ptr; // 找到空位置 if (data_word == 0xFFFFFFFF) { - next_address = address + SIZE_U32; + next_address = address + SIZE_U32; // 上一个位置为有效存储的数据 break; } // 更新下一个可能的写入位置（如果需要格式化） next_address = address - SIZE_U32; } + if (next_address == EEPROM_DATA_START_ADDRESS) { + // 从来没有存储过，直接返回 + return FLASH_EEPROM_OK; + } crc_data_register_reset(); // 确保CRC计算从初始状态开始 CRC_NUM = crc_block_data_calculate((uint32_t *)data, NUM_SAVED); - // for (float &p : data) { - // printf("%f\r\n", p); - // } - // printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); + +#if EEPDEBUG + printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); +#endif if (CRC_NUM != *((u32 *)next_address)) { // CRC错误 for (u8 i = 0; i < NUM_SAVED; i++) { data[i] = DEFAULT_VALUE; @@ -98,12 +117,15 @@ u8 FlashEEPROM::init() { EEPROMStatus = status_read[0]; } - KTH7823.angle_set(angle - KTH7823.update()); - FOC.target_angle = data[0]; - FOC.target_velocity = data[1]; - FOC.limit_voltage = data[2]; - FOC.control_mode = (u8)data[3]; - return 0; + KTH7823.angle_set(0); + + FOC.control_mode = (u8)data[0]; + FOC.limit_voltage = data[1]; + FOC.limit_velocity = data[2]; + KTH7823.angle_set(data[3]); + FOC.target_angle = data[4]; + FOC.target_velocity = data[5]; + return EEPROMStatus; } /** @@ -150,18 +172,22 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { // 从Flash末尾开始向前搜索，找到最新的数据 for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; address >= EEPROM_START_ADDRESS; address -= SIZE_U32) { - u32 data_word = *((u32 *)address); + // 使用volatile确保每次都从内存读取，防止编译器优化 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word = *flash_ptr; + // 检查是否是有效数据（非擦除状态） if (data_word != 0xFFFFFFFF) { - u8 addr = (u8)(data_word & 0xFF); + u8 addr = (u8)(data_word & DATA_ADDR_MASK); // 找到匹配的虚拟地址 if (addr == virtualAddr) { - u32 data_word_c = *((u32 *)address); // 清除最低的8位地址信息，保留高24位数据 - u32 float_data = data_word & 0xFFFFFF00; + u32 float_data = data_word & DATA_FLOAT_MASK; // 使用内存拷贝将处理后的数据解释为float - memcpy(data, &float_data, sizeof(float)); - // printf("check:0x%x=%f\r\n", address, *data); + memcpy(data, &float_data, SIZE_FLOAT); +#if EEPDEBUG + printf("read:0x%x=%f\r\n", address, *data); +#endif return FLASH_EEPROM_OK; } } @@ -179,11 +205,34 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { // 查找下一个可写入的位置 u32 next_address = EEPROM_START_ADDRESS; + u32 data_read; + // 插入虚拟地址到低8位 格式: [data:16bit][reserved:8bit][addr:8bit] + data = (data & DATA_U16_MASK) | (u32)virtualAddr; + + for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; + address >= EEPROM_DATA_START_ADDRESS; address -= SIZE_U32) { + // 安全读取Flash数据 + volatile u32 *flash_ptr = (volatile u32 *)address; + data_read = *flash_ptr; + + if ((data_read & DATA_ADDR_MASK) == (u32)virtualAddr) { // 虚拟地址相同 + if ((data_read & DATA_U16_MASK) == + (data & DATA_U16_MASK)) { // 数据相同 +#if EEPDEBUG + data_read = data_read & DATA_U16_MASK; + u16 float_data_read = *((u16 *)&data_read); + printf("Found data in:0x%x=%u\r\n", address, data_read); +#endif + return FLASH_EEPROM_OK; + } else { + break; + } + } + } // 遍历Flash页面寻找下一个可写入位置 - for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; + for (u32 address = EEPROM_DATA_START_ADDRESS; address < EEPROM_END_ADDRESS; address += SIZE_U32) { - // 安全读取Flash数据 volatile u32 *flash_ptr = (volatile u32 *)address; u32 data_word = *flash_ptr; @@ -198,42 +247,31 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { // 如果有足够空间，直接写入 if (next_address < EEPROM_END_ADDRESS) { - // 新格式: [addr:8bit][reserved:8bit][data:16bit] u32 data_word = ((u32)data << 16) | (u32)virtualAddr; // 使用GD32库函数进行安全编程 fmc_unlock(); fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); - - // 确保使用对齐地址 - fmc_state_enum fmc_status = fmc_word_program(next_address, data_word); - + fmc_state_enum fmc_status = + fmc_word_program(next_address, data_word); // 写入数据 fmc_lock(); + if (fmc_status != FMC_READY) { return FLASH_EEPROM_WRITE_ERROR; } return FLASH_EEPROM_OK; - } else { - // 空间不足，需要格式化页面 + } else { // 空间不足，需要格式化页面 +#if EEPDEBUG + printf("no enough space\r\n"); +#endif FlashEepromStatus status = format(); - if (status != FLASH_EEPROM_OK) { + if (status == FLASH_EEPROM_OK) { + status = save_system_config(false); return status; + } else { + return FLASH_EEPROM_ERASE_ERROR; } - - // 格式化后在页面开头写入数据 - // 新格式: [addr:8bit][reserved:8bit][data:16bit] - u32 new_data_word = ((u32)data << 16) | (u32)virtualAddr; - fmc_state_enum fmc_status = - fmc_word_program(EEPROM_START_ADDRESS, new_data_word); - - if (fmc_status == FMC_READY) - return FLASH_EEPROM_OK; - if (fmc_status == FMC_PGAERR) - return FLASH_EEPROM_ALIGNMENT_ERROR; - else if (fmc_status != FMC_READY) - return FLASH_EEPROM_WRITE_ERROR; } - return FLASH_EEPROM_WRITE_ERROR; } /** @@ -245,16 +283,44 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { // 查找下一个可写入的位置 u32 next_address = EEPROM_START_ADDRESS; + u32 data_word; - // 遍历Flash页面寻找下一个可写入位置 - for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; - address += SIZE_U32) { + // 将完整的float数据复制到32位变量中 + memcpy(&data_word, &data, SIZE_FLOAT); + // 插入虚拟地址到低8位 格式: [data:24bit][addr:8bit] + data_word = (data_word & DATA_FLOAT_MASK) | (u32)virtualAddr; + // 从Flash末尾开始向前搜索，找到最新的数据 + for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; + address >= EEPROM_DATA_START_ADDRESS; address -= SIZE_U32) { // 安全读取Flash数据 volatile u32 *flash_ptr = (volatile u32 *)address; - u32 data_word = *flash_ptr; + u32 data_word_read = *flash_ptr; + + if ((data_word_read & DATA_ADDR_MASK) == + (u32)virtualAddr) { // 虚拟地址相同 + if ((data_word_read & DATA_FLOAT_MASK) == + (data_word & DATA_FLOAT_MASK)) { // 数据相同 +#if EEPDEBUG + data_word_read = data_word_read & DATA_FLOAT_MASK; + float float_data_read = *((float *)&data_word_read); + printf("Found data in:0x%x=%f\r\n", address, float_data_read); +#endif + return FLASH_EEPROM_OK; + } else { + break; // 找到相同地址但数据不同，需要更新 + } + } + } + + // 遍历Flash页面寻找下一个可写入位置 + for (u32 address = EEPROM_DATA_START_ADDRESS; address < EEPROM_END_ADDRESS; + address += SIZE_U32) { + // 安全读取Flash数据 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word_read = *flash_ptr; // 找到空位置 - if (data_word == 0xFFFFFFFF) { + if (data_word_read == 0xFFFFFFFF) { next_address = address; break; } @@ -264,12 +330,6 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { // 如果有足够空间，直接写入 if (next_address < EEPROM_END_ADDRESS) { - u32 data_word; - // 将完整的float数据复制到32位变量中 - memcpy(&data_word, &data, sizeof(float)); - // 插入虚拟地址到低8位 - data_word = (data_word & 0xFFFFFF00) | (u32)virtualAddr; - // 使用GD32库函数进行安全编程 fmc_unlock(); fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); @@ -278,40 +338,29 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { fmc_state_enum fmc_status = fmc_word_program(next_address, data_word); fmc_lock(); - - // u32 data_word_c = *((u32 *)next_address); - // float float_data = *((float *)&data_word_c); - // printf("check:0x%x=%f\r\n", next_address, float_data); - +#if EEPDEBUG + u32 data_word_c = *((u32 *)next_address); + data_word_c = data_word_c & DATA_FLOAT_MASK; + float float_data = *((float *)&data_word_c); + printf("save:0x%x=%f\r\n", next_address, float_data); +#endif if (fmc_status != FMC_READY) { return FLASH_EEPROM_WRITE_ERROR; } return FLASH_EEPROM_OK; } else { // 空间不足，需要格式化页面 +#if EEPDEBUG + printf("no enough space\r\n"); +#endif FlashEepromStatus status = format(); - if (status != FLASH_EEPROM_OK) { + if (status == FLASH_EEPROM_OK) { + status = save_system_config(false); return status; + } else { + return FLASH_EEPROM_ERASE_ERROR; } - - // 格式化后在页面开头写入数据 - u32 data_word; - // 将完整的float数据复制到32位变量中 - memcpy(&data_word, &data, sizeof(float)); - // 插入虚拟地址到低8位 - data_word = (data_word & 0xFFFFFF00) | (u32)virtualAddr; - fmc_unlock(); - fmc_state_enum fmc_status = - fmc_word_program(EEPROM_START_ADDRESS, data_word); - fmc_lock(); - if (fmc_status == FMC_READY) - return FLASH_EEPROM_OK; - if (fmc_status == FMC_PGAERR) - return FLASH_EEPROM_ALIGNMENT_ERROR; - else if (fmc_status != FMC_READY) - return FLASH_EEPROM_WRITE_ERROR; } - return FLASH_EEPROM_WRITE_ERROR; } /** @@ -321,56 +370,22 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { */ FlashEepromStatus FlashEEPROM::format() { fmc_state_enum fmc_status; +#if EEPDEBUG + printf("擦除页面\r\n"); +#endif - // 创建临时缓冲区存储唯一地址的最新数据 - u32 valid_entries[EEPROM_MAX_SIZE / SIZE_U32]; - u16 valid_count = 0; - - // 收集有效的数据项（去重，保留最新的） - for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS; - address += SIZE_U32) { - u32 data_word = *((u32 *)address); - - // 检查是否是有效数据项 - if (data_word != 0xFFFFFFFF) { - u8 addr = (u8)(data_word & 0xFF); - u32 value = (data_word & 0xFFFFFF00); // 保留高24位 - - // 查找是否已经有相同虚拟地址的数据项 - uint8_t found = 0; - for (u16 i = 0; i < valid_count; i++) { - u8 existing_addr = (u8)(valid_entries[i] & 0xFF); - if (existing_addr == addr) { - // 更新为最新的数据 - valid_entries[i] = value | addr; - found = 1; - break; - } - } - // 如果是新的虚拟地址/类型组合，添加到有效数据列表 - if (!found && valid_count < (EEPROM_MAX_SIZE / SIZE_U32)) { - valid_entries[valid_count] = - ((u32)value << 16) | ((u32)value << 8) | addr; - valid_count++; - } - } - } fmc_unlock(); - // 擦除整个页面 - fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); + fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); // 擦除第一页 if (fmc_status != FMC_READY) { + fmc_lock(); return FLASH_EEPROM_ERASE_ERROR; } - // 重新写入有效的数据项 - for (u16 i = 0; i < valid_count; i++) { - fmc_status = fmc_word_program(EEPROM_START_ADDRESS + i * SIZE_U32, - valid_entries[i]); - if (fmc_status != FMC_READY) { - return FLASH_EEPROM_WRITE_ERROR; - } + fmc_status = fmc_page_erase(EEPROM_DATA_PAGE2_ADDRESS); // 擦除第二页 + if (fmc_status != FMC_READY) { + fmc_lock(); + return FLASH_EEPROM_ERASE_ERROR; } fmc_lock(); - return FLASH_EEPROM_OK; } @@ -380,120 +395,74 @@ FlashEepromStatus FlashEEPROM::format() { */ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { u32 CRC_NUM = 0x0000; - static u32 next_address; - FlashEepromStatus status_save[NUM_SAVED - 1] = {FLASH_EEPROM_OK}; - float data[NUM_SAVED - 1] = {FOC.target_angle, FOC.target_velocity, - FOC.limit_voltage, (float)FOC.control_mode}; + float data[NUM_SAVED] = {(float)FOC.control_mode, FOC.limit_voltage, + FOC.limit_velocity, FOC.rt_angle, + FOC.target_angle, FOC.target_velocity}; - // 读取保存的参数 - if (!system_config_saved) { - // 写入参数 - status_save[0] = writeFloat(ADDR_TARGET_ANGLE, data[0]); - status_save[1] = writeFloat(ADDR_TARGET_VEL, data[1]); - status_save[2] = writeFloat(ADDR_VOLTAGE_LIMIT, data[2]); - status_save[3] = writeFloat(ADDR_CONTROL_MODE, data[3]); - for (FlashEepromStatus &p : status_save) { - if (p != FLASH_EEPROM_OK) { - EEPROMStatus = p; - return p; - } - } - - crc_data_register_reset(); // 确保CRC计算从初始状态开始 - CRC_NUM = crc_block_data_calculate((uint32_t *)data, NUM_SAVED); - - // 遍历Flash页面寻找下一个可写入位置 - for (u32 address = EEPROM_START_ADDRESS - SIZE_U32; - address > FLASH_START_ADDRESS; address -= SIZE_U32) { - // 安全读取Flash数据 - volatile u32 *flash_ptr = (volatile u32 *)address; - u32 data_word = *flash_ptr; - // 找到空位置 - if (data_word == 0xFFFFFFFF) { - next_address = address; - break; - } - // 更新下一个可能的写入位置（如果需要格式化） - next_address = address - SIZE_U32; - } - // 如果有足够空间，直接写入 - if (next_address > FLASH_START_ADDRESS) { - fmc_unlock(); - fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); - fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM); - // printf("write address:0x%X,status:0x%x \r\n", next_address, - // fmc_status); - fmc_lock(); - // for (float &p : data) { - // printf("%f\r\n", p); - // } - // printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 - // *)next_address)); - } else { - printf("no enough space\r\n"); - // 空间不足，需要格式化页面 - FlashEepromStatus status = format(); - if (status != FLASH_EEPROM_OK) { - return status; - } - // 格式化后在页面底部写入数据 - fmc_unlock(); - fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); - fmc_state_enum fmc_status = - fmc_word_program(EEPROM_END_ADDRESS - SIZE_U32, CRC_NUM); - fmc_lock(); - if (fmc_status == FMC_READY) - return FLASH_EEPROM_OK; - if (fmc_status == FMC_PGAERR) - return FLASH_EEPROM_ALIGNMENT_ERROR; - else if (fmc_status != FMC_READY) - return FLASH_EEPROM_WRITE_ERROR; - } - if (CRC_NUM == *((u32 *)next_address)) { - system_config_saved = saved; // 标记已保存过 - EEPROMStatus = FLASH_EEPROM_OK; // 设置EEPROM状态 - } else { - EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置EEPROM错误状态 - return FLASH_EEPROM_CRC_ERROR; - } + // 如果系统配置标记已保存，则无需再次保存 + if (system_config_saved) { + EEPROMStatus = FLASH_EEPROM_OK; + return FLASH_EEPROM_OK; } + + // 写入参数并检查错误 + FlashEepromStatus status; + + status = writeFloat(ADDR_CONTROL_MODE, data[0]); + if (status != FLASH_EEPROM_OK) { + EEPROMStatus = status; + return status; + } + + status = writeFloat(ADDR_VOLTAGE_LIMIT, data[1]); + if (status != FLASH_EEPROM_OK) { + EEPROMStatus = status; + return status; + } + + status = writeFloat(ADDR_VEL_LIMIT, data[2]); + if (status != FLASH_EEPROM_OK) { + EEPROMStatus = status; + return status; + } + + status = writeFloat(ADDR_ANGLE, data[3]); + if (status != FLASH_EEPROM_OK) { + EEPROMStatus = status; + return status; + } + + status = writeFloat(ADDR_TARGET_ANGLE, data[4]); + if (status != FLASH_EEPROM_OK) { + EEPROMStatus = status; + return status; + } + + status = writeFloat(ADDR_TARGET_VEL, data[5]); + if (status != FLASH_EEPROM_OK) { + EEPROMStatus = status; + return status; + } + + // 创建用于CRC计算的模拟数据数组，模拟实际存储到Flash中的数据格式 + u32 crc_data_words[NUM_SAVED]; + + // 按照writeFloat函数的方式处理数据 + for (u8 i = 0; i < NUM_SAVED; i++) { + // 将完整的float数据复制到32位变量中 + memcpy(&crc_data_words[i], &data[i], SIZE_FLOAT); + // 去除最后2位精度 + crc_data_words[i] = crc_data_words[i] & DATA_FLOAT_MASK; + } + + crc_data_register_reset(); // 确保CRC计算从初始状态开始 + CRC_NUM = crc_block_data_calculate(crc_data_words, NUM_SAVED); + writeCRC(CRC_NUM, saved); + EEPROMStatus = FLASH_EEPROM_OK; return FLASH_EEPROM_OK; } -/** - * @brief 保存角度数据到Flash - * @param angle 角度数据 - * @return FlashEepromStatus: 操作状态 - */ -FlashEepromStatus FlashEEPROM::save_angle(bool saved) { - float angle_read = DEFAULT_VALUE; - float angle = FOC.rt_angle; - FlashEepromStatus status = FLASH_EEPROM_OK; - - // 读取保存的参数 - if (!angle_saved) { - // 写入参数 - status = writeFloat(ADDR_ANGLE, angle); - if (status != FLASH_EEPROM_OK) { - EEPROMStatus = status; - return status; - } - - if (readFloat(ADDR_ANGLE, &angle_read) != FLASH_EEPROM_OK) - angle_read = DEFAULT_VALUE; // 读不到角度值，存储失败 - - if (fabsf(angle_read - angle) > 0.01 * fabs(angle)) { - EEPROMStatus = FLASH_EEPROM_WRITE_ERROR; // 设置EEPROM错误状态 - return FLASH_EEPROM_WRITE_ERROR; - } else { - angle_saved = saved; // 标记已保存过 - EEPROMStatus = FLASH_EEPROM_OK; // 设置EEPROM状态 - } - } - return FLASH_EEPROM_OK; -} - /** * @brief 读取角度数据从Flash * @return float: 角度数据 @@ -505,3 +474,72 @@ float FlashEEPROM::read_angle() { } return angle; } + +/** + * @brief 保存CRC数据到Flash + * @param saved: 是否一次性保存 + * @return FlashEepromStatus: 状态 + */ +FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { + static u32 next_address; + // 遍历Flash页面寻找下一个可写入crc的位置 + for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32; + address > EEPROM_START_ADDRESS; address -= SIZE_U32) { + // 安全读取Flash数据 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word = *flash_ptr; + // 找到空位置 + if (data_word == 0xFFFFFFFF) { + next_address = address; + break; + } + // 更新下一个可能的写入位置（如果需要格式化） + next_address = address - SIZE_U32; + } + + // 如果当前CRC与现在的crc值不同，则写入 + if (*((u32 *)next_address) != CRC_NUM) { + // 如果有足够空间，直接写入crc值 + if (next_address > EEPROM_START_ADDRESS) { + fmc_unlock(); + fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); + fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM); + // printf("write address:0x%X,status:0x%x \r\n", next_address, + // fmc_status); + fmc_lock(); + +#if EEPDEBUG + printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); +#endif + + // 检查写入是否成功 + if (fmc_status != FMC_READY) { + EEPROMStatus = fmc_status; + return FLASH_EEPROM_WRITE_ERROR; + } + } else { + // 空间不足，需要格式化页面 +#if EEPDEBUG + printf("no enough space\r\n"); +#endif + FlashEepromStatus status = format(); + if (status == FLASH_EEPROM_OK) { + status = save_system_config(false); + return status; + } else { + EEPROMStatus = FLASH_EEPROM_ERASE_ERROR; + return FLASH_EEPROM_ERASE_ERROR; + } + } + } + + // 重新读取CRC校验 + if (CRC_NUM == *((u32 *)next_address)) { + system_config_saved = saved; // 标记已保存过 + EEPROMStatus = FLASH_EEPROM_OK; // 设置EEPROM状态 + return FLASH_EEPROM_OK; + } else { + EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置EEPROM错误状态 + return FLASH_EEPROM_CRC_ERROR; + } +} \ No newline at end of file diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index d501619..dc36494 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -21,11 +21,15 @@ typedef enum { class FlashEEPROM { public: // EEPROM emulation constants - static constexpr u32 EEPROM_START_ADDRESS = - 0x0801F800; // Flash最后一页(2KB) - static constexpr u32 EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 - static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 - static constexpr u32 FLASH_START_ADDRESS = 0x08010000; // Flash起始地址 + static constexpr u32 EEPROM_START_ADDRESS = 0x0801F800; // 第一页(1KB) + static constexpr u32 EEPROM_DATA_PAGE2_ADDRESS = 0x801FC00; // 第二页(1KB) + static constexpr u32 EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址 + static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量 + static constexpr u32 EEPROM_DATA_START_ADDRESS = + 0x0801F900; // DATA段开始地址 + static constexpr u32 DATA_FLOAT_MASK = 0xFFFFFF00; // float存储地址掩码 + static constexpr u32 DATA_U16_MASK = 0xFFFF0000; // u16存储地址掩码 + static constexpr u32 DATA_ADDR_MASK = 0x000000FF; // 虚拟地址掩码 // 定义用户自定义的虚拟地址 static constexpr u8 ADDR_CONTROL_MODE = 0x0; // 控制模式 @@ -33,6 +37,7 @@ class FlashEEPROM { static constexpr u8 ADDR_TARGET_ANGLE = 0x02; // 角度命令数据存储地址 static constexpr u8 ADDR_TARGET_VEL = 0x03; // 速度命令数据存储地址 static constexpr u8 ADDR_VOLTAGE_LIMIT = 0x4; // 电压限制 + static constexpr u8 ADDR_VEL_LIMIT = 0x05; // 角度数据存储地址 bool angle_saved = false; bool system_config_saved = false; @@ -55,11 +60,12 @@ class FlashEEPROM { // 格式化Flash EEPROM页面 FlashEepromStatus format(); - FlashEepromStatus save_angle(bool saved); FlashEepromStatus save_system_config(bool saved); + u8 read_saved_system_config(void); float read_angle(); private: + FlashEepromStatus writeCRC(u32 CRC_NUM, bool saved); }; void flash_eeprom_example_main(void); diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index c78469d..fbd5935 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -96,7 +96,6 @@ void power_manage(void) { float vbus = sampler.get_vbus_voltage(); if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) { FOC.shut_down(); - flashEEP.save_angle(true); flashEEP.save_system_config(true); } else { FOC.resume(); diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index 88c8fc0..c809b06 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -126,7 +126,6 @@ void system_soft_reset(void) { */ void prepare_deep_sleep(bool save_data) { if (save_data) { - flashEEP.save_angle(true); flashEEP.save_system_config(true); } // 将SIT1044设置为低功耗待机模式 From abd64da36044363e9ca7bccf978f95527b52877f Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 23 Dec 2025 11:55:09 +0800 Subject: [PATCH 09/16] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0=E5=B8=A6?= =?UTF-8?q?=E9=80=9F=E5=BA=A6=E9=99=90=E5=88=B6=E7=9A=84=E8=A7=92=E5=BA=A6?= =?UTF-8?q?=E9=97=AD=E7=8E=AF=E6=8E=A7=E5=88=B6=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增angle_control_with_velocity_limit函数，实现带速度限制的角度控制， 包含角度PID和速度PID双环控制，防止电机转速过快导致的不稳定。 - 新增带速度限制的角度控制函数 - 配置新的PID控制器用于速度内环和角度外环 --- Robot_V2/Algorithm/DengFOC.cpp | 30 ++++++++++++++++++++++++++++++ Robot_V2/Algorithm/DengFOC.h | 10 +++++++--- Robot_V2/device/Can.cpp | 2 +- Robot_V2/main.cpp | 9 ++++----- Robot_V2/sys/flash.cpp | 7 ++----- 5 files changed, 44 insertions(+), 14 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 4d6ece9..b824d00 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -209,6 +209,36 @@ float FOC_Controlroller::angle_control(float target_angle) { return shaft_angle; } +/** + * @brief 带速度限制的角度闭环控制 + * + * @param target_angle: 目标角度 + * @param max_velocity: 最大速度限制 (单位: rad/s) + * @return float: 当前角度 + */ +float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) { + // 计算角度误差 + float angle_error = target_angle - rt_angle; + + // 使用角度PID控制器计算目标速度 + float target_velocity = ang_inVelAngloop(angle_error); + + // 对目标速度进行限幅 + target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity); + + // 使用速度PID控制器计算最终输出电压 + float Uq = vel_inVelAngloop(target_velocity - rt_velocity); + // CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity; + + // 获取电机轴角度 + shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; + + // 设置相电压 + setPhaseVoltage(Uq, 0.0f); + + return shaft_angle; +} + /** * @brief 角度闭环控制 * diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index cc967e6..3a616aa 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -25,6 +25,7 @@ class FOC_Controlroller { float Ua = 0, Ub = 0, Uc = 0; float velocity_open_loop(); float angle_control(float target_angle); + float angle_control_with_velocity_limit(float target_angle); float velocity_control(float target_velocity); float current_control(float target_current); float angle_control_NPID(float target_angle); @@ -42,13 +43,16 @@ class FOC_Controlroller { float zero_electric_angle = 0; float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - - PIDController vel_loop = PIDController{0.028f, 0.2f, 1e-4, 1000000, - voltage_power_supply}; // 12V + PIDController vel_loop = + PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000, voltage_power_supply}; // 12V PIDController current_loop = PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V + PIDController vel_inVelAngloop = + PIDController{0.35f, 5.0f,1e-3, 1000000, voltage_power_supply}; + PIDController ang_inVelAngloop = + PIDController{30.0f, 0.0f, 1e-3, 1000000, 999}; void setPhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 76059c3..39df10c 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -716,7 +716,7 @@ void CAN_transmiter::communication_check() { if (first_byte >> 3 & 0x01) { KTH7823.angle_set(0); Angle_kalman.init(0); // 角度滤波 - Vel_kalman.init(0); // 角度滤波 + Vel_kalman.init(0); // 角度滤波 } if (eigth_byte & 0x80 >> 7) { prepare_deep_sleep(true); diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 42b4855..435ba1b 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -25,8 +25,8 @@ extern FOC_Controlroller FOC; // FOC控制 extern FlashEEPROM flashEEP; // EEPROM类 TaskScheduler scheduler = {0}; -volatile u32 system_runtime_us = 0; -volatile u32 cnt_test = 0; +volatile u32 system_runtime_us = 0; // 电机驱动计算轮次 +volatile u32 cnt_test = 0; // 负载检测 int main(void) { systick_config(); // 系统时钟初始化 SystemCoreClockUpdate(); // 更新系统时钟变量 @@ -89,9 +89,8 @@ void do_task(u32 currentTasks) { break; } case 1: { - FOC.angle_control(FOC.target_angle); // 角度闭环控制 - // FOC.angle_control_with_velocity_limit(FOC.target_angle, - // FOC.limit_velocity); // 角度闭环控制，带速度限制 + // FOC.angle_control(FOC.target_angle); // 角度闭环控制 + FOC.angle_control_with_velocity_limit(FOC.target_angle); // 角度闭环控制，带速度限制 break; } case 2: { diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 0a6ebfb..a3f9ac6 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -146,10 +146,10 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { u32 data_word = *((u32 *)address); // 检查是否是有效数据（非擦除状态） if (data_word != 0xFFFFFFFF) { - u8 addr = (u8)(data_word & 0xFF); + u8 addr = (u8)(data_word & DATA_ADDR_MASK); // 找到匹配的虚拟地址 if (addr == virtualAddr) { - *data = (u16)((data_word >> 16) & 0xFFFF); + *data = (u16)(data_word & DATA_U16_MASK); return FLASH_EEPROM_OK; } } @@ -504,10 +504,7 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { fmc_unlock(); fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM); - // printf("write address:0x%X,status:0x%x \r\n", next_address, - // fmc_status); fmc_lock(); - #if EEPDEBUG printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); #endif From d0198fdff74e883e652b72136552c62211f3d963 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sun, 4 Jan 2026 22:26:22 +0800 Subject: [PATCH 10/16] =?UTF-8?q?=E6=9B=B4=E6=96=B0OTA=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=EF=BC=8Cconfig=E7=8B=AC=E7=AB=8B=E6=88=90=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E7=BB=9F=E4=B8=80=E5=AE=9A=E4=B9=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit refactor(build): 更新项目配置和链接器设置 - 修改GD32C103CB的ROM基地址和大小配置 - 更新系统时钟向量表偏移量设置 - 在cproject.yml中添加BIN输出格式支持 - 增加-O3优化级别和代码移除选项 - 更新csolution版本依赖 feat(device): 优化ADC采样和配置管理 - 将DengFOC依赖替换为FOC模块 - 从config.h引入配置常量 - 重命名FILTER_SIZE为ADC_FILTER_SIZE以提高清晰度 - 优化温度和电流采样滤波实现 refactor(algorithm): 简化FOC控制函数接口 - 为angle_control、velocity_control等函数移除不必要的参数 - 统一函数指针类型定义 - 优化电流计算函数的代码格式 - 调整PID控制器初始化参数 --- Robot_V2/Algorithm/{DengFOC.cpp => FOC.cpp} | 84 ++--- Robot_V2/Algorithm/{DengFOC.h => FOC.h} | 27 +- Robot_V2/Algorithm/Filter.cpp | 3 +- .../GD32C103CB/ac6_linker_script.sct.src | 3 +- .../Device/GD32C103CB/regions_GD32C103CB.h | 4 +- .../RTE/Device/GD32C103CB/system_gd32c10x.c | 3 +- .../RTE/_Debug_GD32C103CB/RTE_Components.h | 8 +- .../RTE/_Release_GD32C103CB/RTE_Components.h | 2 +- Robot_V2/Robot_V2.cproject.yml | 19 +- Robot_V2/device/ADC.cpp | 47 ++- Robot_V2/device/Can.cpp | 332 ++++++++++-------- Robot_V2/device/Can.h | 5 +- Robot_V2/device/Timer.cpp | 30 +- Robot_V2/device/Timer.h | 4 - Robot_V2/device/adc.h | 16 +- Robot_V2/device/ang_sensor.cpp | 39 +- Robot_V2/device/ang_sensor.h | 2 +- Robot_V2/main.cpp | 98 +++--- Robot_V2/main.h | 1 + Robot_V2/sys/config.h | 88 +++++ Robot_V2/sys/flash.cpp | 277 ++++++++++++--- Robot_V2/sys/flash.h | 5 + Robot_V2/sys/health.cpp | 6 +- Robot_V2/sys/initial.cpp | 21 +- Robot_V2/sys/initial.h | 24 +- Robot_V2/sys/systick.cpp | 7 +- 26 files changed, 716 insertions(+), 439 deletions(-) rename Robot_V2/Algorithm/{DengFOC.cpp => FOC.cpp} (76%) rename Robot_V2/Algorithm/{DengFOC.h => FOC.h} (98%) create mode 100644 Robot_V2/sys/config.h diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/FOC.cpp similarity index 76% rename from Robot_V2/Algorithm/DengFOC.cpp rename to Robot_V2/Algorithm/FOC.cpp index b824d00..82184f1 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -1,26 +1,15 @@ -#include "DengFOC.h" +#include "FOC.h" #include "Can.h" #include "Timer.h" #include "adc.h" #include "ang_sensor.h" -#include "initial.h" +#include "config.h" +#include "motor.h" #include "pid.h" #include #include #include -#include "motor.h" -constexpr double PI = 3.1415926535897932384626433832795f; -constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f; -constexpr float _1_SQRT3 = 0.57735026919f; -constexpr float _2_SQRT3 = 1.15470053838f; -constexpr float _3PI_2 = 4.71238898038f; -constexpr u8 PP = 11; -constexpr float DIR = 1.0; -constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值 -constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值 -constexpr float zero_electric_angle = 0.0f; -constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI)); FOC_Controlroller FOC; // FOC控制初始化 extern sampler sampler; @@ -62,7 +51,7 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b, sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK]; float I_q = I_beta * cos_el - I_alpha * sin_el; - return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA + return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA } /** @@ -123,13 +112,15 @@ 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; +float FOC_Controlroller::drv_hold() { + float hold_pwm_duty = HOLD_PWM_DUTY; + if (HOLD_PWM_DUTY > 0.95f) { + hold_pwm_duty = 0.95f; + } else if (HOLD_PWM_DUTY < 0.0f) { + hold_pwm_duty = 0.0f; } - Set_3Pwm(duty, duty, duty); + Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty); + return hold_pwm_duty; } /** * @brief 电机恢复 @@ -173,7 +164,8 @@ float FOC_Controlroller::velocity_open_loop() { setPhaseVoltage(Uq, 0.0f); // CAN0_transmiter.testfloat_union.f = - cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); + cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); return limit_voltage; } @@ -198,7 +190,7 @@ float FOC_Controlroller::velocity_open_loop() { * @param target_angle: 目标角度 * @return float: 当前角度 */ -float FOC_Controlroller::angle_control(float target_angle) { +float FOC_Controlroller::angle_control() { // PID 控制计算 float Uq = angle_loop(target_angle - rt_angle); @@ -216,45 +208,27 @@ float FOC_Controlroller::angle_control(float target_angle) { * @param max_velocity: 最大速度限制 (单位: rad/s) * @return float: 当前角度 */ -float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) { +float FOC_Controlroller::angle_control_with_velocity_limit() { // 计算角度误差 float angle_error = target_angle - rt_angle; - + // 使用角度PID控制器计算目标速度 float target_velocity = ang_inVelAngloop(angle_error); - + // 对目标速度进行限幅 - target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity); - + target_velocity = + _constrain(target_velocity, -limit_velocity, limit_velocity); + // 使用速度PID控制器计算最终输出电压 float Uq = vel_inVelAngloop(target_velocity - rt_velocity); // CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity; - - // 获取电机轴角度 - shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; - - // 设置相电压 - setPhaseVoltage(Uq, 0.0f); - - return shaft_angle; -} - -/** - * @brief 角度闭环控制 - * - * @param target_angle: 目标角度 - * @return float: 当前角度 - */ -float FOC_Controlroller::angle_control_NPID(float target_angle) { - static float Kp = 20.0f; - // 控制力矩计算 - float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply, - voltage_power_supply); // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; + // 设置相电压 setPhaseVoltage(Uq, 0.0f); + return shaft_angle; } @@ -264,22 +238,24 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) { * @param target_velocity: 目标速度 * @return float: 当前速度 */ -float FOC_Controlroller::velocity_control(float target_velocity) { +float FOC_Controlroller::velocity_control() { // PID 控制计算 - float Uq = vel_loop((target_velocity - rt_velocity)*25); + float Uq = vel_loop((target_velocity - rt_velocity) * 25); // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; // 设置相电压 setPhaseVoltage(Uq, 0.0f); // CAN0_transmiter.testfloat_union.f = - cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); + cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); return rt_velocity; } -float FOC_Controlroller::current_control(float target_current) { +float FOC_Controlroller::current_control() { // PID 控制计算 - float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); + float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); float Uq = current_loop(target_current - Iq); CAN0_transmiter.testfloat_union.f = Iq; // 获取电机轴角度 diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/FOC.h similarity index 98% rename from Robot_V2/Algorithm/DengFOC.h rename to Robot_V2/Algorithm/FOC.h index 3a616aa..a4a8d38 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -1,6 +1,7 @@ // 函数声明 -#ifndef _DENGFOC_H -#define _DENGFOC_H +#ifndef _FOC_H +#define _FOC_H +#include "config.h" #include "pid.h" #include @@ -14,6 +15,7 @@ class FOC_Controlroller { float limit_velocity = 0.0f; float target_angle = 0.0f; float target_velocity = 0.0f; + float target_current = 0.0f; float rt_angle = 0.0f; float rt_velocity = 0.0f; bool over_current = false; @@ -23,19 +25,18 @@ class FOC_Controlroller { 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 velocity_open_loop(); - float angle_control(float target_angle); - float angle_control_with_velocity_limit(float target_angle); - float velocity_control(float target_velocity); - float current_control(float target_current); - float angle_control_NPID(float target_angle); + float velocity_open_loop(void); + float angle_control(void); + float angle_control_with_velocity_limit(void); + float velocity_control(void); + float current_control(void); void update_power(void); void DFOC_alignSensor(int _PP); void shut_down(void); void resume(void); void drv_stop(void); - void drv_hold(float duty = 0.5f); + float drv_hold(void); protected: float shaft_angle = 0; @@ -50,7 +51,7 @@ class FOC_Controlroller { PIDController current_loop = PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V PIDController vel_inVelAngloop = - PIDController{0.35f, 5.0f,1e-3, 1000000, voltage_power_supply}; + PIDController{0.35f, 5.0f, 1e-3, 1000000, voltage_power_supply}; PIDController ang_inVelAngloop = PIDController{30.0f, 0.0f, 1e-3, 1000000, 999}; @@ -58,12 +59,12 @@ class FOC_Controlroller { float cal_Iq_Id(float current_a, float current_b, float current_c); }; +// 定义函数指针类型 +typedef float (FOC_Controlroller::*FOCFunctionPtr)(void); + float _normalizeAngle(float angle); inline float _electricalAngle(); -constexpr u16 LUT_SIZE = 1024; -constexpr u16 LUT_MASK = (LUT_SIZE - 1); - __attribute__((aligned(4))) const float sin_lut[LUT_SIZE] = { 0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f, 0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f, diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index 01021aa..7a7821e 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -4,14 +4,13 @@ #include #include #include - +#include "config.h" extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; extern u32 system_runtime_us; EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f); EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f); -constexpr u8 SIZE_FLOAT = sizeof(float); /*! \brief 中值滤波器函数 diff --git a/Robot_V2/RTE/Device/GD32C103CB/ac6_linker_script.sct.src b/Robot_V2/RTE/Device/GD32C103CB/ac6_linker_script.sct.src index 7820e1f..1c46801 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/ac6_linker_script.sct.src +++ b/Robot_V2/RTE/Device/GD32C103CB/ac6_linker_script.sct.src @@ -32,7 +32,8 @@ LR_ROM0 __ROM0_BASE __ROM0_SIZE { ER_ROM0 __ROM0_BASE __ROM0_SIZE { - *.o (RESET, +First) + *.o (RESET, +First) /* 向量表和复位处理程序 */ + *(.text.init) /* 确保系统初始化代码在最前面 */ *(InRoot$$Sections) *(+RO +XO) } diff --git a/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h b/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h index 89512d0..13fc0b9 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h +++ b/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h @@ -14,10 +14,10 @@ // Base address <0x0-0xFFFFFFFF:8> // Defines base address of memory region. Default: 0x08000000 // Contains Startup and Vector Table -#define __ROM0_BASE 0x08000000 +#define __ROM0_BASE 0x08011000 // Region size [bytes] <0x0-0xFFFFFFFF:8> // Defines size of memory region. Default: 0x00020000 -#define __ROM0_SIZE 0x00020000 +#define __ROM0_SIZE 0x0000C000 // // __ROM1 (unused) diff --git a/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c b/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c index 225ed31..66e2e72 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c +++ b/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c @@ -42,7 +42,8 @@ #define __SYS_OSC_CLK (__IRC8M) /* main oscillator frequency */ /* Vector Table base offset */ -#define VECT_TAB_OFFSET 0x00 /* This value must be a multiple of 0x200. */ +#define APP_BASE ((uint32_t)0x08011000U) /*!< main FLASH base address */ +#define VECT_TAB_OFFSET (uint32_t)APP_BASE-FLASH_BASE /* This value must be a multiple of 0x200. */ /* select a system clock by uncommenting the following line */ /* use IRC8M */ diff --git a/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h b/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h index 5734403..5b9eec1 100644 --- a/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h +++ b/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h @@ -1,6 +1,6 @@ /* * CSOLUTION generated file: DO NOT EDIT! - * Generated by: csolution version 2.11.0 + * Generated by: csolution version 2.12.0+p41-gfeadfc5e * * Project: 'Robot_V2.Debug+GD32C103CB' * Target: 'Debug+GD32C103CB' @@ -17,8 +17,12 @@ /* GigaDevice::Device:GD32C10x_StdPeripherals:ADC@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_ADC +/* GigaDevice::Device:GD32C10x_StdPeripherals:BKP@1.0.0 */ +#define RTE_DEVICE_STDPERIPHERALS_BKP /* GigaDevice::Device:GD32C10x_StdPeripherals:CAN@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_CAN +/* GigaDevice::Device:GD32C10x_StdPeripherals:CRC@1.0.0 */ +#define RTE_DEVICE_STDPERIPHERALS_CRC /* GigaDevice::Device:GD32C10x_StdPeripherals:DBG@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_DBG /* GigaDevice::Device:GD32C10x_StdPeripherals:DMA@1.0.0 */ @@ -37,6 +41,8 @@ #define RTE_DEVICE_STDPERIPHERALS_PMU /* GigaDevice::Device:GD32C10x_StdPeripherals:RCU@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_RCU +/* GigaDevice::Device:GD32C10x_StdPeripherals:RTC@1.0.0 */ +#define RTE_DEVICE_STDPERIPHERALS_RTC /* GigaDevice::Device:GD32C10x_StdPeripherals:SPI_I2S@1.0.0 */ #define RTE_DEVICE_STDPERIPHERALS_SPI /* GigaDevice::Device:GD32C10x_StdPeripherals:TIMER@1.0.0 */ diff --git a/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h b/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h index 8ac4eac..7115d01 100644 --- a/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h +++ b/Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h @@ -1,6 +1,6 @@ /* * CSOLUTION generated file: DO NOT EDIT! - * Generated by: csolution version 2.12.0 + * Generated by: csolution version 2.12.0+p41-gfeadfc5e * * Project: 'Robot_V2.Release+GD32C103CB' * Target: 'Release+GD32C103CB' diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index 58e3019..f042e28 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -14,6 +14,7 @@ project: - file: sys/health.cpp - file: sys/flash.h - file: sys/flash.cpp + - file: sys/config.h - group: device files: - file: device/Can.cpp @@ -31,18 +32,20 @@ project: files: - file: Algorithm/Filter.cpp - file: Algorithm/Filter.h - - file: Algorithm/DengFOC.cpp - - file: Algorithm/DengFOC.h + - file: Algorithm/FOC.cpp + - file: Algorithm/FOC.h - file: Algorithm/pid.cpp - file: Algorithm/pid.h components: - component: ARM::CMSIS:CORE - component: Device:Startup + - component: Device:EVAL:GD32C10x + - component: Device:GD32C10x_libopt + - component: Device:GD32C10x_StdPeripherals:FMC - component: Device:GD32C10x_StdPeripherals:ADC - component: Device:GD32C10x_StdPeripherals:CAN - component: Device:GD32C10x_StdPeripherals:DMA - component: Device:GD32C10x_StdPeripherals:EXTI - - component: Device:GD32C10x_StdPeripherals:FMC - component: Device:GD32C10x_StdPeripherals:GPIO - component: Device:GD32C10x_StdPeripherals:MISC - component: Device:GD32C10x_StdPeripherals:PMU @@ -50,9 +53,7 @@ project: - component: Device:GD32C10x_StdPeripherals:SPI_I2S - component: Device:GD32C10x_StdPeripherals:TIMER - component: Device:GD32C10x_StdPeripherals:USART - - component: Device:GD32C10x_libopt - component: Device:GD32C10x_StdPeripherals:DBG - - component: Device:EVAL:GD32C10x - component: Device:GD32C10x_StdPeripherals:FWDGT - component: Device:GD32C10x_StdPeripherals:WWDGT - component: Device:GD32C10x_StdPeripherals:BKP @@ -62,6 +63,7 @@ project: type: - elf - hex + - bin - map packs: - pack: ARM::CMSIS @@ -73,11 +75,14 @@ project: - -fshort-wchar - -D__MICROLIB - -Wno-register + - -O3 # 添加优化级别 C: - - -D__packed=packed + - -D__packed=__attribute__((packed)) ASM: - - -D__packed=packed + - -D__packed=__attribute__((packed)) - -D__MICROLIB Link: - --library_type=microlib - --entry=Reset_Handler + - --remove # 移除未使用的代码段 + - --diag_suppress=L6207,L6210,L6218,L6220,L6222,L6224,L6226,L6228,L6230,L6232,L6234,L6236,L6238,L6240,L6242,L6244,L6246,L6248,L6308,L6329,L6310,L6312,L6314,L6316,L6318,L6320,L6322,L6324,L6326,L6328,L6330,L6332,L6334,L6336,L6338,L6340,L6342,L6344,L6346,L6348,L6350,L6352,L6354,L6311,L6313,L6315,L6317,L6319,L6321,L6323,L6325,L6327,L6329,L6331,L6333,L6335,L6337,L6339,L6341,L6343,L6345,L6347,L6349,L6351,L6353 diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index afa3e72..fe19cad 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -1,29 +1,22 @@ #include "adc.h" #include "Can.h" -#include "DengFOC.h" +#include "FOC.h" #include "Filter.h" +#include "config.h" #include "math.h" +#include "systick.h" #include #include #include -#include "systick.h" -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 = -1.60f; constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA -constexpr float MAX_TEMP = 150.0f; -constexpr float MIN_TEMP = -40.0f; -constexpr float MAX_CURRENT = 40.0f; -constexpr float MIN_CURRENT = -40.0f; -constexpr float MAX_VCC = 65.0f; -constexpr float MIN_VCC = -0.1f; extern CAN_transmiter CAN0_transmiter; extern FOC_Controlroller FOC; -sampler sampler; // 采样器初始化 +sampler sampler; // 采样器初始化 u16 adc_value[6] = {0}; // 显式初始化数组元素为0 /*! \brief configure the DMA peripheral @@ -116,7 +109,7 @@ u8 sampler::init() { adc_config(); // ADC初始化 delay_1ms(1); // 初始采样 - for (int i = 0; i < FILTER_SIZE + 1; i++) { + for (int i = 0; i < ADC_FILTER_SIZE + 1; i++) { mcu_temp_samples[i] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; @@ -142,7 +135,7 @@ u8 sampler::init() { * 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f */ float sampler::get_mcu_temp() { - tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1); + tempora_sample = medium_filter(mcu_temp_samples, ADC_FILTER_SIZE + 1); // 检查温度读数是否在有效范围内 if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) { return ERROR_ADC_VAL; @@ -158,7 +151,7 @@ float sampler::get_mcu_temp() { * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。 */ float sampler::get_drv_temp() { - tempora_sample = medium_filter(drv_temp_samples, FILTER_SIZE + 1); + tempora_sample = medium_filter(drv_temp_samples, ADC_FILTER_SIZE + 1); // 检查温度读数是否在有效范围内 if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) { return ERROR_ADC_VAL; @@ -177,17 +170,17 @@ float sampler::get_current(char type) { switch (type) { case 'U': { // return currU_samples[current_sample_index]; - tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1); + tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1); break; } case 'V': { // return currV_samples[current_sample_index]; - tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1); + tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1); break; } case 'W': { // return currW_samples[current_sample_index]; - tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1); + tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1); break; } default: { @@ -209,10 +202,10 @@ float sampler::get_current(char type) { * 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。 */ float sampler::get_vbus_voltage() { - tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1); + tempora_sample = medium_filter(vbus_samples, ADC_FILTER_SIZE + 1); // 检查电压读数是否在有效范围内 - if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC or - vbus_samples[FILTER_SIZE / 2] < MIN_VCC) { + if (vbus_samples[ADC_FILTER_SIZE / 2] > MAX_VCC or + vbus_samples[ADC_FILTER_SIZE / 2] < MIN_VCC) { return ERROR_ADC_VAL; } return tempora_sample; @@ -226,8 +219,8 @@ float sampler::get_vbus_voltage() { */ void sampler::update_mcu_temp() { memmove(mcu_temp_samples, mcu_temp_samples + 1, - FILTER_SIZE * sizeof(float)); - mcu_temp_samples[FILTER_SIZE] = + ADC_FILTER_SIZE * sizeof(float)); + mcu_temp_samples[ADC_FILTER_SIZE] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; } @@ -236,9 +229,9 @@ void sampler::update_mcu_temp() { */ void sampler::update_drv_temp() { memmove(drv_temp_samples, drv_temp_samples + 1, - FILTER_SIZE * sizeof(float)); + ADC_FILTER_SIZE * sizeof(float)); float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700; - drv_temp_samples[FILTER_SIZE] = + drv_temp_samples[ADC_FILTER_SIZE] = 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f; } @@ -258,7 +251,7 @@ void sampler::update_current() { // 更新索引 current_sample_index++; - if (current_sample_index >= FILTER_SIZE - 1) { + if (current_sample_index >= ADC_FILTER_SIZE - 1) { current_sample_index = 0; } // 卡尔曼滤波更新 @@ -269,6 +262,6 @@ void sampler::update_current() { * @brief 更新供电电压 */ void sampler::update_vbus_voltage() { - memmove(vbus_samples, vbus_samples + 1, FILTER_SIZE * sizeof(float)); - vbus_samples[FILTER_SIZE] = adc_value[5] * ADC_SCALE * VCC_FACTOR; + memmove(vbus_samples, vbus_samples + 1, ADC_FILTER_SIZE * sizeof(float)); + vbus_samples[ADC_FILTER_SIZE] = adc_value[5] * ADC_SCALE * VCC_FACTOR; } \ No newline at end of file diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 39df10c..4b1925d 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -1,8 +1,9 @@ #include "Can.h" -#include "DengFOC.h" +#include "FOC.h" #include "Filter.h" #include "adc.h" #include "ang_sensor.h" +#include "config.h" #include "flash.h" #include "gd32c10x.h" #include "gd32c10x_can.h" @@ -19,10 +20,6 @@ #include constexpr u16 CANX = CAN0; -constexpr u16 DEV_CAN0_ID = 0x0000; -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; CAN_transmiter CAN0_transmiter; // CAN初始化 extern FOC_Controlroller FOC; @@ -157,8 +154,11 @@ void can_config(void) { /* configure CAN0 NVIC */ nvic_irq_enable(CAN0_RX0_IRQn, 1, 0); + nvic_irq_enable(CAN0_TX_IRQn, 1, 1); // 添加发送中断 /* enable can receive FIFO0 not empty interrupt */ can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); + /* enable can transmit mailbox empty interrupt */ + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); // 使能DMA中断 nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1); @@ -173,6 +173,7 @@ void can_config(void) { */ void canfd_config(void) { can_gpio_init(); + can_dma_init(); // 初始化DMA can_parameter_struct can_parameter; can_fdframe_struct can_fd_parameter; can_fd_tdc_struct can_fd_tdc_parameter; @@ -235,6 +236,9 @@ void canfd_config(void) { nvic_irq_enable(CAN0_RX0_IRQn, 1, 0); /* enable can receive FIFO0 not empty interrupt */ can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); + // 使能DMA中断 + nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1); + dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF); } /*! @@ -458,128 +462,6 @@ void CAN_transmiter::update(void) { v_current_union.f = sampler.get_current('V') * 1000; } -/*! - \brief CAN 数据发送 - \param id CAN ID - \param length 数据长度 - \param data 数据 -*/ -u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) { - // 检查数据长度是否符合CAN规范 - if (length > 8) { - printf("CAN data length exceeds maximum of 8 bytes\r\n"); - return 0; - } - - // 检查数据指针是否有效 - if (data == nullptr) { - printf("Invalid data pointer\r\n"); - return 0; - } - - /* 配置发送消息参数 */ - transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 - transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式 - transmit_message.tx_sfid = id; // 设置帧ID - transmit_message.tx_dlen = length; // 设置数据长度 - - // 复制数据到发送缓冲区 - for (int i = 0; i < length && i < 8; i++) { - transmit_message.tx_data[i] = *(data + i); - } - - // 将数据复制到DMA缓冲区 - memcpy(can_tx_buffer, data, length); - - // 配置DMA发送参数 - dma_tx_parameter.number = length; - dma_init(DMA0, DMA_CH5, &dma_tx_parameter); - - // 启动DMA传输 - dma_channel_enable(DMA0, DMA_CH5); - - // 自动选择空闲mailbox发送消息，不需要人为指定 - transmit_mailbox = can_message_transmit(CAN0, &transmit_message); - - // 检查是否成功获取mailbox - if (CAN_NOMAILBOX == transmit_mailbox) { - // 没有可用的mailbox，可能所有mailbox都在使用中 - printf("No available mailbox for CAN transmission\r\n"); - return 0; - } - return 1; -} - -/*! - \brief CANFD 数据发送 - \param id CAN消息ID - \param length CAN消息长度 - \param data CAN消息数据 -*/ -u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) { - // 检查CANFD模式是否启用 - if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) { - printf("CANFD mode not enabled\r\n"); - return 0; - } - - // 检查数据指针是否有效 - if (data == nullptr) { - printf("Invalid data pointer\r\n"); - return 0; - } - - // 检查数据长度是否符合CANFD规范 - if (length > 64) { - printf("CANFD data length exceeds maximum of 64 bytes\r\n"); - return 0; - } - - /* 配置发送消息参数 */ - transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 - transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式 - // transmit_message.tx_sfid = id; // 设置帧ID - transmit_message.tx_efid = id; // 扩展帧ID - transmit_message.tx_dlen = length; // 设置数据长度 - - // 设置CANFD特定参数 - transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式 - transmit_message.fd_brs = ENABLE; // 使能比特率切换 - transmit_message.fd_esi = DISABLE; // 错误状态指示 - - // 复制数据到发送缓冲区 - for (int i = 0; i < length && i < 64; i++) { - transmit_message.tx_data[i] = *(data + i); - } - - // 将数据复制到DMA缓冲区 - memcpy(can_tx_buffer, data, length); - - // 配置DMA发送参数 - dma_tx_parameter.number = length; - dma_init(DMA0, DMA_CH5, &dma_tx_parameter); - - // 启动DMA传输 - dma_channel_enable(DMA0, DMA_CH5); - - // 自动选择空闲mailbox发送消息，不需要人为指定 - transmit_mailbox = can_message_transmit(CAN0, &transmit_message); - - // 检查是否成功获取mailbox - if (CAN_NOMAILBOX == transmit_mailbox) { - static u32 count = 0; - // 没有可用的mailbox，可能所有mailbox都在使用中 - count++; - printf("No available mailbox for CANFD transmission\r\n"); - if (count > 0xff and !is_sleep) { - init(); - count = 0; - } - return 0; - } - return 1; -} - /** * @brief CAN发送函数 * @retval 0: 发送失败 @@ -594,31 +476,39 @@ u8 CAN_transmiter::CAN_all_Send() { constexpr u32 Status_FEED = 0x118; constexpr u32 Limit_Vel_Vol_FEED = 0x119; - // 打包RT_Vel_Ang_FEED + // 打包RT_Vel_Ang_FEED - 只清零实际使用的8字节 + memset(transmit_data, 0, 8); pack_float_to_bytes(transmit_data, 0, rt_angle_union); pack_float_to_bytes(transmit_data, 4, rt_velocity_union); - success_flag += CAN_Send(RT_Vel_Ang_FEED, 8, transmit_data); - // 打包SET_Vel_Ang_FEED + success_flag += CAN_AsyncSend(RT_Vel_Ang_FEED, 8, transmit_data); + + // 打包SET_Vel_Ang_FEED - 只清零实际使用的8字节 + memset(transmit_data, 0, 8); pack_float_to_bytes(transmit_data, 0, target_angle_union); pack_float_to_bytes(transmit_data, 4, target_velocity_union); - success_flag += CAN_Send(SET_Vel_Ang_FEED, 8, transmit_data); - // 打包Current_FEED + success_flag += CAN_AsyncSend(SET_Vel_Ang_FEED, 8, transmit_data); + + // 打包Current_FEED - 只清零实际使用的6字节 + memset(transmit_data, 0, 6); pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f)); pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f)); pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f)); - success_flag += CAN_Send(Current_FEED, 6, transmit_data); + success_flag += CAN_AsyncSend(Current_FEED, 6, transmit_data); - // 打包Status_FEED + // 打包Status_FEED - 只清零实际使用的8字节 + memset(transmit_data, 0, 8); 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); pack_u16_to_bytes(transmit_data, 6, Power_Voltage); - success_flag += CAN_Send(Status_FEED, 8, transmit_data); - // 打包Limit_Vel_Vol_FEED + success_flag += CAN_AsyncSend(Status_FEED, 8, transmit_data); + + // 打包Limit_Vel_Vol_FEED - 只清零实际使用的6字节 + memset(transmit_data, 0, 6); pack_float_to_bytes(transmit_data, 0, limit_velocity_union); pack_u16_to_bytes(transmit_data, 4, limit_voltage); - success_flag += CAN_Send(Limit_Vel_Vol_FEED, 6, transmit_data); + success_flag += CAN_AsyncSend(Limit_Vel_Vol_FEED, 6, transmit_data); return success_flag; } @@ -631,9 +521,8 @@ u8 CAN_transmiter::CAN_all_Send() { u8 CAN_transmiter::CANFD_all_Send() { u32 success_flag = 0; - constexpr u32 STATUS_ID = 0x00010100; - constexpr u32 CONFIG_ID = 0x00010101; - // 打包status数据 + // 打包status数据 - 只清零实际使用的32字节 + memset(transmit_data, 0, 32); pack_u16_to_bytes(transmit_data, 0, mcu_temp); pack_u16_to_bytes(transmit_data, 2, motor_temp); pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f)); @@ -647,15 +536,16 @@ u8 CAN_transmiter::CANFD_all_Send() { pack_float_to_bytes(transmit_data, 13, rt_angle_union); pack_float_to_bytes(transmit_data, 17, rt_velocity_union); pack_float_to_bytes(transmit_data, 21, testfloat_union); - success_flag += CANFD_Send(STATUS_ID, 32, transmit_data); + success_flag += CANFD_AsyncSend(CANFD_STATUS_ID, 32, transmit_data); - // 打包config数据 + // 打包config数据 - 只清零实际使用的16字节 + memset(transmit_data, 0, 16); transmit_data[0] = controller_Mode; pack_u16_to_bytes(transmit_data, 1, limit_voltage); pack_float_to_bytes(transmit_data, 3, limit_velocity_union); pack_float_to_bytes(transmit_data, 7, target_angle_union); pack_float_to_bytes(transmit_data, 11, target_velocity_union); - success_flag += CANFD_Send(CONFIG_ID, 16, transmit_data); + success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 16, transmit_data); return success_flag; } @@ -669,9 +559,9 @@ void CAN_transmiter::communication_check() { // CAN0 发送数据标志 if (SET == can0_send_flag) { // printf("\r\n can0 %X receive data:", receive_message.rx_efid); - CAN0_transmiter.update(); + update(); // CAN0_transmiter.CAN_all_Send(); - CAN0_transmiter.CANFD_all_Send(); + CANFD_all_Send(); can0_send_flag = RESET; } @@ -681,7 +571,7 @@ void CAN_transmiter::communication_check() { // for (i = 0; i < receive_message.rx_dlen; i++) { // printf(" %02x", receive_message.rx_data[i]); // } - if (receive_message.rx_efid == 0x00010200) { + if (receive_message.rx_efid == CANFD_CMD_ID) { FOC.control_mode = receive_message.rx_data[0]; if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 != FOC.limit_voltage) { @@ -701,7 +591,7 @@ void CAN_transmiter::communication_check() { target_velocity_union.f = unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f; - } else if (receive_message.rx_efid == 0x00010301) { + } else if (receive_message.rx_efid == CANFD_RESET_ID) { u8 first_byte = receive_message.rx_data[0]; u8 eigth_byte = receive_message.rx_data[7]; if (first_byte & 0x01) { @@ -722,7 +612,8 @@ void CAN_transmiter::communication_check() { prepare_deep_sleep(true); // CAN_Sleep(); } - + } else if (receive_message.rx_efid == CANFD_BOOT_ID) { + jumpToBOOT(); } else { printf("Unknown CAN ID: %X\n", receive_message.rx_efid); } @@ -800,7 +691,6 @@ void DMA0_Channel4_IRQHandler(void) { if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_FTF)) { // 清除DMA传输完成中断标志 dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G); - // 将DMA缓冲区的数据复制到接收消息结构体 for (int i = 0; i < CAN0_transmiter.receive_message.rx_dlen && i < 8; i++) { @@ -815,7 +705,149 @@ void DMA0_Channel4_IRQHandler(void) { */ void DMA0_Channel5_IRQHandler(void) { if (dma_interrupt_flag_get(DMA0, DMA_CH5, DMA_INT_FLAG_FTF)) { + // DMA传输完成，可以在此处执行后续处理 // 清除DMA传输完成中断标志 - dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_G); + dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_FTF); + // 可以在此添加后续处理逻辑，如发送下一个数据包等 + } +} + +/** + * @brief 异步CAN数据发送(使用DMA和中断机制) + * @param id CAN消息ID + * @param length CAN消息长度 + * @param data CAN消息数据 + */ +u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) { + // 检查数据长度是否符合CAN规范 + if (length > 8) { + printf("CAN data length exceeds maximum of 8 bytes\r\n"); + return 0; + } + + // 检查数据指针是否有效 + if (data == nullptr) { + printf("Invalid data pointer\r\n"); + return 0; + } + + /* 配置发送消息参数 */ + transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 + transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式 + transmit_message.tx_sfid = id; // 设置帧ID + transmit_message.tx_dlen = length; // 设置数据长度 + + // 复制数据到发送缓冲区 + for (int i = 0; i < length && i < 8; i++) { + transmit_message.tx_data[i] = *(data + i); + } + + // 自动选择空闲mailbox发送消息 + transmit_mailbox = can_message_transmit(CAN0, &transmit_message); + + // 检查是否成功获取mailbox + if (transmit_mailbox == CAN_NOMAILBOX) { + // 没有可用的mailbox，启用发送中断以便在邮箱可用时重试 + printf("No available mailbox for CAN transmission, enabling TX " + "interrupt\r\n"); + // 可以设置一个标志位，当邮箱可用时发送 + // 在实际应用中，可以维护一个待发送队列 + return 0; + } + + // 启用CAN发送中断以检测传输完成 + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); + return 1; +} + +/** + * @brief 异步CANFD数据发送(使用DMA和中断机制) + * @param id CAN消息ID + * @param length CAN消息长度 + * @param data CAN消息数据 + */ +u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { + u8 valid_length = length; + // 检查CANFD模式是否启用 + if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) { + printf("CANFD mode not enabled\r\n"); + return 0; + } + + // 检查数据指针是否有效 + if (data == nullptr) { + printf("Invalid data pointer\r\n"); + return 0; + } + + // 检查数据长度是否符合CANFD规范 + if (length > 8) { + if (length > 8 and length < 13) { + valid_length = 12; + } else if (length > 12 and length < 17) { + valid_length = 16; + } else if (length > 16 and length < 21) { + valid_length = 20; + } else if (length > 20 and length < 24) { + valid_length = 24; + } else if (length > 28 and length < 33) { + valid_length = 32; + } else if (length > 32 and length < 49) { + valid_length = 48; + } else if (length > 48 and length < 65) { + valid_length = 64; + } else { + printf("CANFD data length error\r\n"); + return 0; + } + } + + /* 配置发送消息参数 */ + transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 + transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式 + transmit_message.tx_efid = id; // 扩展帧ID + transmit_message.tx_dlen = valid_length; // 设置数据长度 + + // 设置CANFD特定参数 + transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式 + transmit_message.fd_brs = ENABLE; // 使能比特率切换 + transmit_message.fd_esi = DISABLE; // 错误状态指示 + // 复制数据到发送缓冲区 + for (int i = 0; i < valid_length; i++) { + if (i < length) { + transmit_message.tx_data[i] = *(data + i); + } else { + transmit_message.tx_data[i] = 0; + } + } + + // 自动选择空闲mailbox发送消息 + transmit_mailbox = can_message_transmit(CAN0, &transmit_message); + + // 检查是否成功获取mailbox + if (transmit_mailbox == CAN_NOMAILBOX) { + // 没有可用的mailbox，启用发送中断以便在邮箱可用时重试 + printf("No available mailbox for CANFD transmission, enabling TX " + "interrupt\r\n"); + return 0; + } + + // 启用CAN发送中断以检测传输完成 + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); + return 1; +} + +// 添加CAN传输中断处理函数 +void CAN0_TX_IRQHandler(void) { + // 检查是否有发送邮箱空闲 + if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET || + can_flag_get(CAN0, CAN_FLAG_TME1) != RESET || + can_flag_get(CAN0, CAN_FLAG_TME2) != RESET) { + // 邮箱已空闲，可以发送新消息 + // 可以在这里处理待发送队列中的消息 + // 清除发送中断标志 + can_flag_clear(CAN0, CAN_FLAG_TME0); + can_flag_clear(CAN0, CAN_FLAG_TME1); + can_flag_clear(CAN0, CAN_FLAG_TME2); } } diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 1cd3384..8374d24 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -60,6 +60,7 @@ void can_dma_init(void); // CAN DMA初始化 void CAN0_RX0_IRQHandler(void); // CAN接收中断处理函数 void DMA0_Channel4_IRQHandler(void); // DMA通道4中断处理函数(CAN接收) void DMA0_Channel5_IRQHandler(void); // DMA通道5中断处理函数(CAN发送) +void jumpToBOOT(); void can_loopback_init(void); void can_loopback_test(void); @@ -97,8 +98,8 @@ class CAN_transmiter { void update(); // 更新数据 u8 CAN_Receive(); // CAN接收 u8 CANFD_Receive(); // CANFD接收 - u8 CAN_Send(u32 ID, u8 length, u8 *data); // CAN发送 - u8 CANFD_Send(u32 ID, u8 length, u8 *data); // CANFD发送 + u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN异步发送(使用DMA) + u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA) u8 CAN_all_Send(); // CAN发送 u8 CANFD_all_Send(); // CANFD发送 diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index c59074a..1b41c42 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -1,6 +1,6 @@ #include "Timer.h" #include "Can.h" -#include "DengFOC.h" +#include "FOC.h" #include "Timer.h" #include "gd32c10x.h" #include "gd32c10x_gpio.h" @@ -12,16 +12,6 @@ #include #include -constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz -constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us - -constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz -constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz -constexpr int TIMER1_CH0_PULSE = 0; -constexpr int TIMER1_CH1_PULSE = 0; -constexpr int TIMER1_CH2_PULSE = 0; -constexpr int TIMER1_CH3_COMPARE_VALUE = - (TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断 volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1 extern u16 adc_value[7]; @@ -46,7 +36,7 @@ void nvic_config(void) { } /** - * @brief 配置定时器2 + * @brief 配置定时器2-任务管理 * @return void * *配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务 @@ -87,7 +77,7 @@ void TIMER2_config(void) { } /** - * @brief 配置TIMER2中断 + * @brief 配置TIMER2-任务管理中断 * @return void */ void User_timer_init(void) { @@ -98,7 +88,7 @@ void User_timer_init(void) { } /** - * @brief TIMER2中断处理函数 + * @brief TIMER2-任务管理中断处理函数 * @return void */ void TIMER2_IRQHandler(void) { @@ -174,7 +164,7 @@ void TIMER2_IRQHandler(void) { } /** - * @brief TIMER1 GPIO配置函数 + * @brief TIMER1-PWM GPIO配置函数 */ void timer1_gpio_config(void) { // 时钟使能 @@ -198,7 +188,7 @@ void timer1_gpio_config(void) { } /** - * @brief TIMER1 配置函数 + * @brief TIMER1-PWM 配置函数 */ void timer1_pwm_config(void) { timer_parameter_struct timer_initpara; @@ -242,11 +232,11 @@ void timer1_pwm_config(void) { TIMER_OC_MODE_PWM0); // CH3也设为PWM模式 timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, - TIMER1_CH0_PULSE); + TIMER1_DEFAULT_PULSE); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, - TIMER1_CH1_PULSE); + TIMER1_DEFAULT_PULSE); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2, - TIMER1_CH2_PULSE); + TIMER1_DEFAULT_PULSE); // 设置CH3比较值为PWM周期x%位置 timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_3, TIMER1_CH3_COMPARE_VALUE); @@ -262,7 +252,7 @@ void timer1_pwm_config(void) { timer_enable(TIMER1); } -// 添加TIMER1中断处理函数 +// 添加TIMER1-PWM中断处理函数 /** * @brief TIMER1中断处理函数 * @return void diff --git a/Robot_V2/device/Timer.h b/Robot_V2/device/Timer.h index ed220de..9029aeb 100644 --- a/Robot_V2/device/Timer.h +++ b/Robot_V2/device/Timer.h @@ -16,10 +16,6 @@ void timer1_pwm_config(void); void timer1_gpio_config(void); void TIMER1_IRQHandler(void); // 添加TIMER1中断处理函数声明 -/* 获取系统运行时间 */ -uint64_t get_system_runtime_us(void); - - #ifdef __cplusplus } #endif diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h index 3a65951..65e3834 100644 --- a/Robot_V2/device/adc.h +++ b/Robot_V2/device/adc.h @@ -6,16 +6,16 @@ extern "C" { #endif #include "stdint.h" -constexpr u8 FILTER_SIZE = 6; -constexpr u8 BLOCK_SIZE = FILTER_SIZE * sizeof(float); +#include "config.h" + class sampler { private: - float vbus_samples[FILTER_SIZE + 1]; - float currU_samples[FILTER_SIZE + 1]; - float currV_samples[FILTER_SIZE + 1]; - float currW_samples[FILTER_SIZE + 1]; - float mcu_temp_samples[FILTER_SIZE + 1]; - float drv_temp_samples[FILTER_SIZE + 1]; + float vbus_samples[ADC_FILTER_SIZE + 1]; + float currU_samples[ADC_FILTER_SIZE + 1]; + float currV_samples[ADC_FILTER_SIZE + 1]; + float currW_samples[ADC_FILTER_SIZE + 1]; + float mcu_temp_samples[ADC_FILTER_SIZE + 1]; + float drv_temp_samples[ADC_FILTER_SIZE + 1]; u8 current_sample_index; float tempora_sample; diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 6b8467e..520db62 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -1,31 +1,20 @@ #include "ang_sensor.h" +#include "Can.h" #include "Filter.h" -#include "core_cmFunc.h" -#include "gd32c10x_can.h" +#include "flash.h" #include "gd32c10x_dma.h" #include "gd32c10x_gpio.h" #include "gd32c10x_spi.h" -#include "initial.h" -#include "flash.h" #include #include -#include "Can.h" #include +#define ANG_SENS_DEBUG 1 #define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) #define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15) #define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14) #define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14) -constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f; -constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f; -constexpr u16 DEFAULT_COMMAND = 0x0000; -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 u8 SIZE_FLOAT = sizeof(float); - // DMA传输完成标志 volatile bool spi_dma_tx_complete = false; volatile bool spi_dma_rx_complete = false; @@ -34,7 +23,7 @@ angle_sensor KTH7823; // 角度传感器初始化 extern FlashEEPROM flashEEP; extern EncoderAngleKalman Angle_kalman; extern EncoderVelocityKalman Vel_kalman; - extern CAN_transmiter CAN0_transmiter; // CAN初始化 +extern CAN_transmiter CAN0_transmiter; // CAN初始化 /** * @brief SPI DMA initialization @@ -199,10 +188,9 @@ void KTH7823_spi_init(void) { spi_enable(SPI0); } - /** -* @ 角度传感器类 初始化 -* @return 0:初始化成功 1:初始化失败 + * @ 角度传感器类 初始化 + * @return 0:初始化成功 1:初始化失败 */ u8 angle_sensor::init(void) { KTH7823_gpio_init(); // GPIO初始化 @@ -211,17 +199,16 @@ u8 angle_sensor::init(void) { return 0; } /** -* @ 角度传感器类 设置当前角度 -* @param angle 输出级角度值 -* @return 0成功/1失败 + * @ 角度传感器类 设置当前角度 + * @param angle 输出级角度值 + * @return 0成功/1失败 */ -u8 angle_sensor::angle_set(float angle){ - m_cumulativeAngle = angle*REDUCTION_RATIO; // 从Flash读取上次保存的角度 +u8 angle_sensor::angle_set(float angle) { + m_cumulativeAngle = angle * REDUCTION_RATIO; // 从Flash读取上次保存的角度 m_firstUpdate = true; return 0; } - /** * @brief 读取传感器数据 * @return 传感器数据 @@ -235,19 +222,17 @@ inline float angle_sensor::read_sensorAngle(void) { if (result == 0) { // 单次浮点乘法优化：只计算一次度数，然后转换为弧度 - float angle_deg = (float)raw_data * ANGLE_FACTOR; + float angle_deg = (float)raw_data * ANGLESENS_FACTOR; current_angle_deg = angle_deg; current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f return angle_deg; } -#ifdef DEBUG // 仅在调试模式下输出错误，避免发布版本中的I/O开销 if (result != 0) { // 使用更高效的串口输出替代printf（如果可用） printf("Error reading angle from sensor.\n"); } -#endif return (float)FAULT_RECEIVE; } diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index b2501c0..b9cd594 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -5,12 +5,12 @@ extern "C" { #endif #include "gd32c10x_eval.h" #include "stdio.h" +#include "config.h" u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord); int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data, uint16_t *rx_data); -constexpr u16 VEL_FILTER_SIZE = 4; class angle_sensor { public: diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 435ba1b..8aa0b1b 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -1,16 +1,16 @@ #include "main.h" #include "Can.h" -#include "DengFOC.h" +#include "FOC.h" #include "Filter.h" #include "Timer.h" #include "adc.h" #include "ang_sensor.h" -#include "core_cm4.h" -#include "core_cmFunc.h" #include "flash.h" #include "health.h" #include "initial.h" #include "motor.h" +#include "sys/config.h" +#include "sys/flash.h" #include "sys/initial.h" #include "systick.h" #include @@ -28,35 +28,14 @@ TaskScheduler scheduler = {0}; volatile u32 system_runtime_us = 0; // 电机驱动计算轮次 volatile u32 cnt_test = 0; // 负载检测 int main(void) { - systick_config(); // 系统时钟初始化 - SystemCoreClockUpdate(); // 更新系统时钟变量 - watchdog_init(50); // 看门狗初始化 - unused_pin_init(); // 未使用引脚初始化 - USART0_init(); // 调试串口初始化 - motor_init(); // 电机驱动初始化 - if (get_reset_cause() == - RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠 - printf("continue sleep\r\n"); - watchdog_init(5000); // 看门狗初始化 - delay_1ms(1); - prepare_deep_sleep(false); - } - User_timer_init(); // 任务Timer初始化 - u8 init_status = 0; - init_status += KTH7823.init(); // 角度传感器初始化 - init_status += sampler.init(); // 采样器初始化 - init_status += - flashEEP.init(); // EEPROM初始化，必须放在角度卡尔曼之前，角度传感器之后 - - init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 - init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 - init_status += CAN0_transmiter.init(); // CAN初始化 + u8 init_status = hardware_init(); while (init_status) { // 初始化检测 std::printf("初始化失败\r\n"); } - + OTA_initial(); // OTA初始化,A/B分区处理 angsens_MG_check(); // 角度传感器磁感应强度测试 + while (1) { __disable_irq(); u32 currentTasks = @@ -69,6 +48,32 @@ int main(void) { return 0; } +u8 hardware_init(void) { + __enable_irq(); + systick_config(); // 系统时钟初始化 + watchdog_init(50); // 看门狗初始化 + unused_pin_init(); // 未使用引脚初始化 + USART0_init(); // 调试串口初始化 + motor_init(); // 电机驱动初始化 + if (get_reset_cause() == + RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠 + printf("continue sleep\r\n"); + watchdog_init(5000); // 看门狗初始化 + delay_1ms(3000); + prepare_deep_sleep(false); + } + User_timer_init(); // 任务Timer初始化 + u8 init_status = 0; + init_status += KTH7823.init(); // 角度传感器初始化 + init_status += sampler.init(); // 采样器初始化 + flashEEP.init(); // EEPROM初始化，必须放在角度卡尔曼之前，角度传感器之后 + + init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 + init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 + init_status += CAN0_transmiter.init(); // CAN初始化 + return init_status; +} + void do_task(u32 currentTasks) { // 按照优先级和重要性处理任务 if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 @@ -83,29 +88,22 @@ void do_task(u32 currentTasks) { } if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) system_runtime_us++; - switch (FOC.control_mode) { - case 0: { - FOC.drv_hold(0.95); // 占空比置0 - break; - } - case 1: { - // FOC.angle_control(FOC.target_angle); // 角度闭环控制 - FOC.angle_control_with_velocity_limit(FOC.target_angle); // 角度闭环控制，带速度限制 - break; - } - case 2: { - FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 - break; - } - case 3: { - // FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 - FOC.current_control(FOC.limit_velocity); // 电流闭环控制 - break; - } - case 4: { - FOC.velocity_open_loop(); // 速度开环控制 - break; - } + + // 函数指针数组，包含各种控制模式对应的函数 + static FOCFunctionPtr control_functions[] = { + &FOC_Controlroller::drv_hold, // 0: 占空比置 + &FOC_Controlroller:: + angle_control_with_velocity_limit, // 1: + // 角度闭环控制，带速度限制 + &FOC_Controlroller::velocity_control, // 2: 速度闭环控制 + &FOC_Controlroller::current_control, // 3: 电流闭环控制 + &FOC_Controlroller::velocity_open_loop // 4: 速度开环控制 + }; + + // 边界检查，防止数组越界 + if (FOC.control_mode >= 0 && FOC.control_mode < 5) { + // 调用对应的控制函数 + (FOC.*control_functions[FOC.control_mode])(); } } if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值 diff --git a/Robot_V2/main.h b/Robot_V2/main.h index a8426d5..f39ea43 100644 --- a/Robot_V2/main.h +++ b/Robot_V2/main.h @@ -37,5 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "stdint.h" void do_task(u32 currentTasks); +u8 hardware_init(void); #endif /* MAIN_H */ diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h new file mode 100644 index 0000000..c2efc6c --- /dev/null +++ b/Robot_V2/sys/config.h @@ -0,0 +1,88 @@ +#ifndef __CONFIG_H +#define __CONFIG_H +#include + +// 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 +// #define ARM_MATH_CM4 + +#define EEP_DEBUG 1 +// BOOT 相关 +constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT起始地址 +constexpr u32 OTA_CONFIG_NUM = 6; +constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801D000; +constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801D004; +constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801D008; +constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801D00C; +constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801D010; +constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801D014; +constexpr u32 OTA_NOT_AVAL = 0xFFFFFFFF; +constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 + +// 参数配置 +constexpr u8 PP = 11; +constexpr float DIR = 1.0; +constexpr u16 REDUCTION_RATIO = 25; +constexpr float CURRENT_OFFSET = -1.615f; // 当前电流传感器的零点偏移 +constexpr float HOLD_PWM_DUTY = 0.95f; + +constexpr u32 CANFD_BOOT_ID = 0x00000000; +constexpr u32 CANFD_STATUS_ID = 0x00010100; +constexpr u32 CANFD_CONFIG_ID = 0x00010101; +constexpr u32 CANFD_CMD_ID = 0x00010200; +constexpr u32 CANFD_RESET_ID = 0x00010301; + +// 常量定义 +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 ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * PI; +constexpr float DEG2RAD = 1.0f / 360.0f * 2 * PI; +constexpr float RAD2DEG = 360.0f / 2 * PI; +constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3) +constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值 +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相关 +constexpr u16 DEV_CAN0_ID = 0x0000; +constexpr u16 DEV_CAN0_MASK = 0x0000; +constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000; + +// 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; +constexpr float MAX_CURRENT = 40.0f; +constexpr float MIN_CURRENT = -40.0f; +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-任务调度 +constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz +constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us + +// TIMER1-PWM +constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz +constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz +constexpr int TIMER1_DEFAULT_PULSE = 0; +constexpr int TIMER1_CH3_COMPARE_VALUE = + (TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断 + +#endif \ No newline at end of file diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index a3f9ac6..37511c4 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -1,26 +1,28 @@ #include "flash.h" -#include "DengFOC.h" +#include "FOC.h" #include "ang_sensor.h" +#include "config.h" +#include "gd32c10x.h" #include "gd32c10x_crc.h" #include "gd32c10x_fmc.h" -#include "math.h" #include -#include -#include -#include #include -#include FlashEEPROM flashEEP = FlashEEPROM(); extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; extern FOC_Controlroller FOC; // FOC控制 -constexpr u8 SIZE_U32 = sizeof(uint32_t); -constexpr u8 SIZE_FLOAT = sizeof(float); -constexpr u8 DEFAULT_VALUE = 0; -constexpr u8 NUM_SAVED = 6; -#define EEPDEBUG 0 +inline fmc_state_enum write_ADDRESS(u32 address, u32 data) { + fmc_unlock(); + fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); + + // 确保使用对齐地址 + fmc_state_enum fmc_status = fmc_word_program(address, data); + + fmc_lock(); + return fmc_status; +} // FlashEEPROM类实现 FlashEEPROM::FlashEEPROM() { // 构造函数可以为空，或者做一些初始化工作 @@ -56,8 +58,8 @@ u8 FlashEEPROM::init() { u8 FlashEEPROM::read_saved_system_config(void) { // 读取保存的参数 u32 CRC_NUM = 0x0000; - FlashEepromStatus status_read[NUM_SAVED + 1] = {FLASH_EEPROM_OK}; - float data[NUM_SAVED] = {0.0f}; + FlashEepromStatus status_read[CONFIG_SAVED_NUM + 1] = {FLASH_EEPROM_OK}; + float data[CONFIG_SAVED_NUM] = {0.0f}; // 尝试从Flash读取参数，如果不存在则使用默认值 status_read[1] = readFloat(ADDR_CONTROL_MODE, &data[0]); status_read[2] = readFloat(ADDR_VOLTAGE_LIMIT, &data[1]); @@ -67,7 +69,7 @@ u8 FlashEEPROM::read_saved_system_config(void) { status_read[6] = readFloat(ADDR_TARGET_VEL, &data[5]); // 读取失败则使用默认值 - for (u8 i = 1; i < NUM_SAVED + 1; i++) { + for (u8 i = 1; i < CONFIG_SAVED_NUM + 1; i++) { if (status_read[i] == FLASH_EEPROM_DATA_NOT_FOUND) { // 首次读取，不保存报错 data[i - 1] = DEFAULT_VALUE; @@ -99,13 +101,13 @@ u8 FlashEEPROM::read_saved_system_config(void) { } crc_data_register_reset(); // 确保CRC计算从初始状态开始 - CRC_NUM = crc_block_data_calculate((uint32_t *)data, NUM_SAVED); + CRC_NUM = crc_block_data_calculate((uint32_t *)data, CONFIG_SAVED_NUM); -#if EEPDEBUG +#if EEP_DEBUG printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); #endif if (CRC_NUM != *((u32 *)next_address)) { // CRC错误 - for (u8 i = 0; i < NUM_SAVED; i++) { + for (u8 i = 0; i < CONFIG_SAVED_NUM; i++) { data[i] = DEFAULT_VALUE; } EEPROMStatus = FLASH_EEPROM_CRC_ERROR; @@ -185,7 +187,7 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { u32 float_data = data_word & DATA_FLOAT_MASK; // 使用内存拷贝将处理后的数据解释为float memcpy(data, &float_data, SIZE_FLOAT); -#if EEPDEBUG +#if EEP_DEBUG printf("read:0x%x=%f\r\n", address, *data); #endif return FLASH_EEPROM_OK; @@ -218,7 +220,7 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { if ((data_read & DATA_ADDR_MASK) == (u32)virtualAddr) { // 虚拟地址相同 if ((data_read & DATA_U16_MASK) == (data & DATA_U16_MASK)) { // 数据相同 -#if EEPDEBUG +#if EEP_DEBUG data_read = data_read & DATA_U16_MASK; u16 float_data_read = *((u16 *)&data_read); printf("Found data in:0x%x=%u\r\n", address, data_read); @@ -249,19 +251,14 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) { if (next_address < EEPROM_END_ADDRESS) { u32 data_word = ((u32)data << 16) | (u32)virtualAddr; - // 使用GD32库函数进行安全编程 - fmc_unlock(); - fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); - fmc_state_enum fmc_status = - fmc_word_program(next_address, data_word); // 写入数据 - fmc_lock(); + fmc_state_enum fmc_status = write_ADDRESS(next_address, data_word); if (fmc_status != FMC_READY) { return FLASH_EEPROM_WRITE_ERROR; } return FLASH_EEPROM_OK; } else { // 空间不足，需要格式化页面 -#if EEPDEBUG +#if EEP_DEBUG printf("no enough space\r\n"); #endif FlashEepromStatus status = format(); @@ -301,7 +298,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { (u32)virtualAddr) { // 虚拟地址相同 if ((data_word_read & DATA_FLOAT_MASK) == (data_word & DATA_FLOAT_MASK)) { // 数据相同 -#if EEPDEBUG +#if EEP_DEBUG data_word_read = data_word_read & DATA_FLOAT_MASK; float float_data_read = *((float *)&data_word_read); printf("Found data in:0x%x=%f\r\n", address, float_data_read); @@ -330,15 +327,8 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { // 如果有足够空间，直接写入 if (next_address < EEPROM_END_ADDRESS) { - // 使用GD32库函数进行安全编程 - fmc_unlock(); - fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); - - // 确保使用对齐地址 - fmc_state_enum fmc_status = fmc_word_program(next_address, data_word); - - fmc_lock(); -#if EEPDEBUG + fmc_state_enum fmc_status = write_ADDRESS(next_address, data_word); +#if EEP_DEBUG u32 data_word_c = *((u32 *)next_address); data_word_c = data_word_c & DATA_FLOAT_MASK; float float_data = *((float *)&data_word_c); @@ -350,7 +340,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { return FLASH_EEPROM_OK; } else { // 空间不足，需要格式化页面 -#if EEPDEBUG +#if EEP_DEBUG printf("no enough space\r\n"); #endif FlashEepromStatus status = format(); @@ -370,7 +360,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { */ FlashEepromStatus FlashEEPROM::format() { fmc_state_enum fmc_status; -#if EEPDEBUG +#if EEP_DEBUG printf("擦除页面\r\n"); #endif @@ -395,9 +385,10 @@ FlashEepromStatus FlashEEPROM::format() { */ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { u32 CRC_NUM = 0x0000; - float data[NUM_SAVED] = {(float)FOC.control_mode, FOC.limit_voltage, - FOC.limit_velocity, FOC.rt_angle, - FOC.target_angle, FOC.target_velocity}; + float data[CONFIG_SAVED_NUM] = { + (float)FOC.control_mode, FOC.limit_voltage, + FOC.limit_velocity, FOC.rt_angle, + FOC.target_angle, FOC.target_velocity}; // 如果系统配置标记已保存，则无需再次保存 if (system_config_saved) { @@ -445,10 +436,10 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { } // 创建用于CRC计算的模拟数据数组，模拟实际存储到Flash中的数据格式 - u32 crc_data_words[NUM_SAVED]; + u32 crc_data_words[CONFIG_SAVED_NUM]; // 按照writeFloat函数的方式处理数据 - for (u8 i = 0; i < NUM_SAVED; i++) { + for (u8 i = 0; i < CONFIG_SAVED_NUM; i++) { // 将完整的float数据复制到32位变量中 memcpy(&crc_data_words[i], &data[i], SIZE_FLOAT); // 去除最后2位精度 @@ -456,7 +447,7 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { } crc_data_register_reset(); // 确保CRC计算从初始状态开始 - CRC_NUM = crc_block_data_calculate(crc_data_words, NUM_SAVED); + CRC_NUM = crc_block_data_calculate(crc_data_words, CONFIG_SAVED_NUM); writeCRC(CRC_NUM, saved); EEPROMStatus = FLASH_EEPROM_OK; @@ -501,14 +492,10 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { if (*((u32 *)next_address) != CRC_NUM) { // 如果有足够空间，直接写入crc值 if (next_address > EEPROM_START_ADDRESS) { - fmc_unlock(); - fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); - fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM); - fmc_lock(); -#if EEPDEBUG + fmc_state_enum fmc_status = write_ADDRESS(next_address, CRC_NUM); +#if EEP_DEBUG printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); #endif - // 检查写入是否成功 if (fmc_status != FMC_READY) { EEPROMStatus = fmc_status; @@ -516,7 +503,7 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { } } else { // 空间不足，需要格式化页面 -#if EEPDEBUG +#if EEP_DEBUG printf("no enough space\r\n"); #endif FlashEepromStatus status = format(); @@ -539,4 +526,192 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置EEPROM错误状态 return FLASH_EEPROM_CRC_ERROR; } +} + +u8 OTA_SET(u32 addr, bool enable) { + fmc_state_enum fmc_status; + u32 temp_val[OTA_CONFIG_NUM]; + u32 data = enable ? OTA_NOT_AVAL : OTA_AVAL; + volatile u32 *flash_ptr = (volatile u32 *)addr; + temp_val[0] = *flash_ptr; + if (temp_val[0] == data) { // 如果相同 + return FLASH_EEPROM_OK; // 成功 + } + + fmc_unlock(); + // 如果不同，且要写0，直接写 + if (data == OTA_AVAL) { + fmc_status = write_ADDRESS(addr, data); + fmc_lock(); + if (fmc_status == FMC_READY) { + return FLASH_EEPROM_OK; // 成功可以直接退出 + } + } + + // 如果不同或写入出错，要写1，需要重置，然后再写 + flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; + temp_val[0] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + temp_val[1] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR; + temp_val[2] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_EN_ADDR; + temp_val[3] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + temp_val[4] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + temp_val[5] = *flash_ptr; +#if EEP_DEBUG + printf("OTA_SET\r\n"); + printf("0x%x_0x%x_0x%x_0x%x\r\n", temp_val[0], temp_val[1], temp_val[2], + temp_val[3]); +#endif + fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); // 擦除页 + if (fmc_status != FMC_READY) { + return FLASH_EEPROM_ERASE_ERROR; // 擦除错误退出 + } + for (int i = 0; i < OTA_CONFIG_NUM; i++) { + if (temp_val[i] == OTA_AVAL and + n_APP_LATEST_AVAL_ADDR + 4 * i != addr) { + fmc_status = write_ADDRESS(n_APP_LATEST_AVAL_ADDR + 4 * i, + OTA_AVAL); // 把需要写入0的写入0 + if (fmc_status != FMC_READY) { + fmc_lock(); + return FLASH_EEPROM_WRITE_ERROR; // 写入错误退出 + } + } + } + fmc_lock(); + return FLASH_EEPROM_OK; +} + +/** + * @brief 跳转应用处理模块 + * @param 无 + * @retval 无 + */ +void jumpToBOOT() { + OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活 + // 1. 关闭所有中断 + __disable_irq(); + for (int i = 0; i < 8; i++) { + NVIC->ICER[i] = 0xFFFFFFFF; + } + // 2. 重置外设（特别是CAN，防止冲突） + can_deinit(CAN0); + + // 3. 重置系统时钟到默认状态 + rcu_system_clock_source_config(RCU_CKSYSSRC_IRC8M); + + // 4. 关闭一些关键外设时钟 + rcu_periph_clock_disable(RCU_GPIOA); + rcu_periph_clock_disable(RCU_GPIOB); + rcu_periph_clock_disable(RCU_GPIOC); + rcu_periph_clock_disable(RCU_USART0); + rcu_periph_clock_disable(RCU_CAN0); + rcu_periph_clock_disable(RCU_TIMER0); + rcu_periph_clock_disable(RCU_DMA0); + + // 5. 设置中断向量表偏移到应用程序起始地址 + SCB->VTOR = BOOTLOADER_START_ADDR; + + // 6. 设置MSP并跳转 + uint32_t msp_value = *(__IO uint32_t *)BOOTLOADER_START_ADDR; + uint32_t reset_handler = *(__IO uint32_t *)(BOOTLOADER_START_ADDR + 4); + typedef void (*AppEntryFunc)(void); + AppEntryFunc app_entry = (AppEntryFunc)reset_handler; + __set_MSP(msp_value); + // 7. 跳转 + app_entry(); + + // 如果程序执行到这里，说明跳转失败 + printf("Jump to application failed!\r\n"); +} + +/** + * @brief 将0x08011000地址后0xC000大小区域内的数据搬运到0x08050000地址 + * @param None + * @retval fmc_state_enum: 操作状态 + */ +fmc_state_enum move_app_data() { + // 源地址: 0x08011000 + // 目标地址: 0x08050000 + // 数据大小: 0xC000 (49152字节) + + uint32_t src_addr = 0x08011000; + uint32_t dst_addr = 0x08050000; + uint32_t size = 0xC000; // 48KB + + fmc_state_enum fmc_status = FMC_READY; + + // 计算源数据的CRC + rcu_periph_clock_enable(RCU_CRC); + crc_deinit(); + crc_data_register_reset(); + + // 临时缓冲区用于计算CRC + uint32_t word_count = size / 4; + uint32_t src_crc, dst_crc; + + // 计算源数据CRC + src_crc = crc_block_data_calculate((uint32_t *)src_addr, word_count); + + // 关闭全局中断以防止Flash操作期间的中断干扰 + __disable_irq(); + + fmc_unlock(); + + // 首先擦除目标地址的Flash页面 + // 计算需要擦除的页面数量（GD32C103的Flash页大小为1KB） + uint32_t page_size = 1024; // 1KB per page + uint32_t num_pages = (size + page_size - 1) / page_size; // 向上取整 + + for (uint32_t i = 0; i < num_pages; i++) { + uint32_t page_addr = dst_addr + i * page_size; + + // 擦除页面 + fmc_status = fmc_page_erase(page_addr); + if (fmc_status != FMC_READY) { + fmc_lock(); + __enable_irq(); // 重新开启中断 + return fmc_status; + } + } + + // 复制数据 - 按字（32位）进行复制以提高效率 + for (uint32_t offset = 0; offset < size; offset += 4) { + uint32_t src_data = *(volatile uint32_t *)(src_addr + offset); + + // 检查源地址是否为有效的Flash数据（非0xFFFFFFFF表示有数据） + if (src_data != 0xFFFFFFFF) { + fmc_status = fmc_word_program(dst_addr + offset, src_data); + if (fmc_status != FMC_READY) { + fmc_lock(); + __enable_irq(); // 重新开启中断 + return fmc_status; + } + } + } + + fmc_lock(); + + // 重新开启全局中断 + __enable_irq(); + + // 计算目标数据的CRC + crc_data_register_reset(); + dst_crc = crc_block_data_calculate((uint32_t *)dst_addr, word_count); + + // 比较源数据和目标数据的CRC + if (src_crc != dst_crc) { + // 如果CRC不匹配，可以选择返回错误状态 + // 这里我们返回一个特殊值表示CRC校验失败 + return (fmc_state_enum)0xFF; // 自定义错误码表示CRC校验失败 + } + + // 设置备份应用可用标志 + OTA_SET(n_APP_BACKUP_AVAL_ADDR, OTA_AVAL); + OTA_SET(n_APP_BACKUP_CHECK_ADDR, OTA_AVAL); + + return fmc_status; } \ No newline at end of file diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index dc36494..826aa50 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -2,6 +2,8 @@ #define __FLASH_H #include +#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用 + // 修改后 - 强制4字节对齐 #define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 @@ -69,4 +71,7 @@ class FlashEEPROM { }; void flash_eeprom_example_main(void); +u8 OTA_SET(u32 addr, bool enable); +fmc_state_enum move_app_data(void); + #endif /* __FLASH_H */ \ No newline at end of file diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index fbd5935..2b7af17 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -1,9 +1,11 @@ #include "health.h" +#include "config.h" +#include #include "Can.h" -#include "DengFOC.h" +#include "FOC.h" #include "adc.h" #include "flash.h" -#include + extern sampler sampler; extern CAN_transmiter CAN0_transmiter; diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index 90bdb52..8c1e907 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,7 +1,10 @@ #include "initial.h" +#include "config.h" +#include "flash.h" #include "gd32c10x.h" #include "gd32c10x_gpio.h" #include "gd32c10x_rcu.h" +#include "gd32c10x_fmc.h" #include #include #include @@ -27,7 +30,6 @@ u8 watchdog_init(u32 time) { return status; } - int fputc(int ch, FILE *f) throw() { usart_data_transmit(EVAL_COM0, (uint8_t)ch); while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE)) @@ -91,3 +93,20 @@ u16 get_reset_cause(void) { // printf("cause:%d\r\n", cause); return cause; } + +void OTA_initial(void) { + // 确认当前APP可用 + OTA_SET(n_APP_LATEST_CHECK_ADDR, true); + + volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + u32 back_aval = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + u32 back_check = *flash_ptr; + // 如果BACK不可用 + if (back_aval == OTA_NOT_AVAL) { + move_app_data(); + } else if (back_aval == OTA_AVAL && + back_check == OTA_NOT_AVAL) { // BACK已刷写，但未检查 + move_app_data(); + } +} diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 4330a0e..64e9491 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -5,23 +5,11 @@ extern "C" { #endif +#include "config.h" #include "gd32c10x_eval.h" #include "stdint.h" #include - -constexpr u16 REDUCTION_RATIO = 25; -constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f; -constexpr float RAD2DEG = 360.0f / 6.28318530718f; - -void rcu_config(void); -static volatile uint32_t timer_delay_count = 0; // volatile 很重要！ -int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 - -u8 USART0_init(void); -u8 watchdog_init(u32 time); -u16 get_reset_cause(void); -void unused_pin_init(); // 任务标志定义 typedef enum { TASK_MOTOR_CONTROL = 0, @@ -37,6 +25,16 @@ typedef enum { TASK_COUNT } SystemTask; +void rcu_config(void); +static volatile uint32_t timer_delay_count = 0; // volatile 很重要！ +int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 + +u8 USART0_init(void); +u8 watchdog_init(u32 time); +u16 get_reset_cause(void); +void unused_pin_init(); +void OTA_initial(void); + // 任务管理结构 typedef struct { volatile u32 taskFlags; // 任务标志位(每个位代表一个任务) diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index c809b06..b7f6c4a 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -83,7 +83,6 @@ void delay_1ms(uint32_t count) { delay = count; while (0U != delay) { fwdgt_counter_reload(); - fwdgt_counter_reload(); } } @@ -177,4 +176,10 @@ void EXTI5_9_IRQHandler(void) { printf("MCU wake up\r\n"); system_soft_reset(); } +} + + +void OTA_initial(){ + + } \ No newline at end of file From fccb93dd414af34b91116d72c6b6677b91e8f10f Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Mon, 5 Jan 2026 00:55:00 +0800 Subject: [PATCH 11/16] =?UTF-8?q?feat(can):=20=E6=B7=BB=E5=8A=A0OTA?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=E5=AE=9E=E7=8E=B0AB=E5=88=86=E5=8C=BA=20-=20?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=96=B0=E7=9A=84CANFD=E6=B6=88=E6=81=AFID?= =?UTF-8?q?=E5=A4=84=E7=90=86=EF=BC=8C=E6=94=AF=E6=8C=81OTA=E8=AF=B7?= =?UTF-8?q?=E6=B1=82=E5=93=8D=E5=BA=94=20-=20=E5=AE=9E=E7=8E=B0OTA?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E6=A3=80=E6=9F=A5=E5=92=8CACK=E5=8F=91?= =?UTF-8?q?=E9=80=81=E5=8A=9F=E8=83=BD=20-=20=E4=BC=98=E5=8C=96CAN=5Ftrans?= =?UTF-8?q?miter=E7=B1=BB=E6=96=B9=E6=B3=95=E5=A3=B0=E6=98=8E=E6=A0=BC?= =?UTF-8?q?=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit refactor(flash): 重构OTA设置和应用数据移动功能 - 修改OTA_SET函数参数类型从bool到u32 - 优化move_app_data函数实现，添加CRC校验和错误处理 - 添加调试输出信息和看门狗喂狗机制 - 修复数据移动过程中的地址计算错误 refactor(config): 优化Flash分区配置和常量定义 - 重新组织Flash分区地址常量定义 - 添加OTA相关CANFD消息ID定义 - 移动CANFD相关常量到合适位置 docs(flash): 更新函数注释和文档说明 - 更新move_app_data函数的注释说明 - 添加代码注释以提高可读性 fix(initial): 修复OTA初始化和看门狗配置 - 将看门狗初始化移到main函数中合适位置 - 修正OTA初始化逻辑 - 添加CAN模块外部引用 --- Robot_V2/device/Can.cpp | 11 ++++ Robot_V2/device/Can.h | 24 ++++---- Robot_V2/main.cpp | 7 ++- Robot_V2/sys/config.h | 39 ++++++++----- Robot_V2/sys/flash.cpp | 119 ++++++++++++++++++++++++--------------- Robot_V2/sys/flash.h | 2 +- Robot_V2/sys/initial.cpp | 8 ++- 7 files changed, 135 insertions(+), 75 deletions(-) diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 4b1925d..7b350f2 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -614,6 +614,17 @@ void CAN_transmiter::communication_check() { } } else if (receive_message.rx_efid == CANFD_BOOT_ID) { jumpToBOOT(); + } else if (receive_message.rx_efid == 0x00000102) { + volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR; + u32 app_enable = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + 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(CANFD_OTA_ACK_ID, 2, transmit_data); + } } else { printf("Unknown CAN ID: %X\n", receive_message.rx_efid); } diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 8374d24..6d5282c 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -95,13 +95,9 @@ class CAN_transmiter { u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态 u8 transmit_data[64]; // CANFD最大64字节 - void update(); // 更新数据 - u8 CAN_Receive(); // CAN接收 - u8 CANFD_Receive(); // CANFD接收 - u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN异步发送(使用DMA) - u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA) - u8 CAN_all_Send(); // CAN发送 - u8 CANFD_all_Send(); // CANFD发送 + void update(); // 更新数据 + u8 CAN_Receive(); // CAN接收 + u8 CANFD_Receive(); // CANFD接收 public: // 添加构造函数声明 @@ -112,11 +108,15 @@ class CAN_transmiter { FlagStatus can0_send_flag; // CAN0发送标志 float_union_t testfloat_union = {0}; // 测试用 - bool MagStatus = 0; // 磁状态码 - bool ThermalStatus = 0; // 热能状态码 - void communication_check(); // 通信检查 - u8 CAN_Sleep(); // CAN进入睡眠 - u8 CAN_Wakeup(); // CAN进入睡眠 + bool MagStatus = 0; // 磁状态码 + bool ThermalStatus = 0; // 热能状态码 + void communication_check(); // 通信检查 + u8 CAN_Sleep(); // CAN进入睡眠 + u8 CAN_Wakeup(); // CAN进入睡眠 + u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN异步发送(使用DMA) + u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA) + u8 CAN_all_Send(); // CAN发送 + u8 CANFD_all_Send(); // CANFD发送 }; #ifdef __cplusplus diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 8aa0b1b..d844ece 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -33,9 +33,11 @@ int main(void) { while (init_status) { // 初始化检测 std::printf("初始化失败\r\n"); } - OTA_initial(); // OTA初始化,A/B分区处理 + OTA_initial(); // OTA初始化,A/B分区处理 + + watchdog_init(50); // 看门狗初始化 angsens_MG_check(); // 角度传感器磁感应强度测试 - + while (1) { __disable_irq(); u32 currentTasks = @@ -51,7 +53,6 @@ int main(void) { u8 hardware_init(void) { __enable_irq(); systick_config(); // 系统时钟初始化 - watchdog_init(50); // 看门狗初始化 unused_pin_init(); // 未使用引脚初始化 USART0_init(); // 调试串口初始化 motor_init(); // 电机驱动初始化 diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index c2efc6c..cc4d311 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -6,15 +6,24 @@ // #define ARM_MATH_CM4 #define EEP_DEBUG 1 -// BOOT 相关 +#define OTA_DEBUG 1 + +// Flash分区配置 +constexpr u32 BOOTLOADER_SIZE = 0x4000; // 16KB constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT起始地址 +constexpr u32 APPLATEST_START_ADDR = 0x08011000; // APP起始地址 +constexpr u32 APPBACK_START_ADDR = 0x08005000; // APPBACK起始地址 +constexpr u32 APP_MAX_SIZE = 0xC000; // 48KB +constexpr u32 FMC_PAGE_SIZE = 0x400; // 1KB + +// BOOT 相关 constexpr u32 OTA_CONFIG_NUM = 6; -constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801D000; -constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801D004; -constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801D008; -constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801D00C; -constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801D010; -constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801D014; +constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801D000; // 最新APP可用地址 +constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801D004; // 备份APP可用地址 +constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801D008; // 最新APP激活 +constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801D00C; // 备份APP激活 +constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801D010; // 最新APP校验 +constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801D014; // 备份APP校验 constexpr u32 OTA_NOT_AVAL = 0xFFFFFFFF; constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 @@ -25,12 +34,6 @@ constexpr u16 REDUCTION_RATIO = 25; constexpr float CURRENT_OFFSET = -1.615f; // 当前电流传感器的零点偏移 constexpr float HOLD_PWM_DUTY = 0.95f; -constexpr u32 CANFD_BOOT_ID = 0x00000000; -constexpr u32 CANFD_STATUS_ID = 0x00010100; -constexpr u32 CANFD_CONFIG_ID = 0x00010101; -constexpr u32 CANFD_CMD_ID = 0x00010200; -constexpr u32 CANFD_RESET_ID = 0x00010301; - // 常量定义 constexpr u8 SIZE_FLOAT = sizeof(float); constexpr u8 SIZE_U32 = sizeof(uint32_t); @@ -54,6 +57,16 @@ constexpr u16 DEV_CAN0_ID = 0x0000; constexpr u16 DEV_CAN0_MASK = 0x0000; constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000; +constexpr u32 CANFD_BOOT_ID = 0x00000000; +constexpr u32 CANFD_STATUS_ID = 0x00010100; +constexpr u32 CANFD_CONFIG_ID = 0x00010101; +constexpr u32 CANFD_CMD_ID = 0x00010200; +constexpr u32 CANFD_RESET_ID = 0x00010301; +constexpr u32 CANFD_OTA_REQ_ID = 0x00000100; +constexpr u32 CANFD_OTA_DATA_ID = 0x00000101; +constexpr u32 CANFD_OTA_ACK_ID = 0x00000102; +constexpr u32 CANFD_OTA_WAIT_ID = 0x00000103; + // ADC constexpr u8 ADC_FILTER_SIZE = 6; constexpr u8 BLOCK_SIZE = ADC_FILTER_SIZE * SIZE_FLOAT; diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 37511c4..5ac1fde 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -2,10 +2,10 @@ #include "FOC.h" #include "ang_sensor.h" #include "config.h" -#include "gd32c10x.h" +#include "gd32c10x.h" // GD32主头文件，确保外设函数声明可用 #include "gd32c10x_crc.h" -#include "gd32c10x_fmc.h" #include +#include #include FlashEEPROM flashEEP = FlashEEPROM(); @@ -528,22 +528,27 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { } } -u8 OTA_SET(u32 addr, bool enable) { +u8 OTA_SET(u32 addr, u32 data) { fmc_state_enum fmc_status; u32 temp_val[OTA_CONFIG_NUM]; - u32 data = enable ? OTA_NOT_AVAL : OTA_AVAL; volatile u32 *flash_ptr = (volatile u32 *)addr; temp_val[0] = *flash_ptr; - if (temp_val[0] == data) { // 如果相同 + if (temp_val[0] == data) { // 如果相同 +#if EEP_DEBUG + printf("OTA_SET%x same%X,skip\r\n", addr, data); +#endif return FLASH_EEPROM_OK; // 成功 } fmc_unlock(); // 如果不同，且要写0，直接写 if (data == OTA_AVAL) { - fmc_status = write_ADDRESS(addr, data); + fmc_status = write_ADDRESS(addr, OTA_AVAL); fmc_lock(); if (fmc_status == FMC_READY) { +#if EEP_DEBUG + printf("OTA_SET%x 0\r\n", addr); +#endif return FLASH_EEPROM_OK; // 成功可以直接退出 } } @@ -557,7 +562,7 @@ u8 OTA_SET(u32 addr, bool enable) { temp_val[2] = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_EN_ADDR; temp_val[3] = *flash_ptr; - flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; temp_val[4] = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; temp_val[5] = *flash_ptr; @@ -568,6 +573,9 @@ u8 OTA_SET(u32 addr, bool enable) { #endif fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); // 擦除页 if (fmc_status != FMC_READY) { +#if EEP_DEBUG + printf("OTA_SET erase error\r\n"); +#endif return FLASH_EEPROM_ERASE_ERROR; // 擦除错误退出 } for (int i = 0; i < OTA_CONFIG_NUM; i++) { @@ -577,11 +585,25 @@ u8 OTA_SET(u32 addr, bool enable) { OTA_AVAL); // 把需要写入0的写入0 if (fmc_status != FMC_READY) { fmc_lock(); +#if EEP_DEBUG + printf("OTA_SET write error\r\n"); +#endif return FLASH_EEPROM_WRITE_ERROR; // 写入错误退出 } } } + flash_ptr = (volatile u32 *)addr; + temp_val[0] = *flash_ptr; + if (temp_val[0] != data) { +#if EEP_DEBUG + printf("OTA_SET write error\r\n"); +#endif + return FLASH_EEPROM_WRITE_ERROR; + } fmc_lock(); +#if EEP_DEBUG + printf("OTA_SET%x ok\r\n", addr); +#endif return FLASH_EEPROM_OK; } @@ -629,67 +651,76 @@ void jumpToBOOT() { } /** - * @brief 将0x08011000地址后0xC000大小区域内的数据搬运到0x08050000地址 + * @brief 将0x08011000地址后0xC000大小区域内的数据搬运到0x08005000地址 * @param None * @retval fmc_state_enum: 操作状态 */ fmc_state_enum move_app_data() { - // 源地址: 0x08011000 - // 目标地址: 0x08050000 - // 数据大小: 0xC000 (49152字节) - - uint32_t src_addr = 0x08011000; - uint32_t dst_addr = 0x08050000; - uint32_t size = 0xC000; // 48KB + // 源地址: APPLATEST_START_ADDR (0x08011000) + // 目标地址: APPBACK_START_ADDR (0x08005000) + // 数据大小: APP_MAX_SIZE (0xC000 = 48KB) fmc_state_enum fmc_status = FMC_READY; - // 计算源数据的CRC - rcu_periph_clock_enable(RCU_CRC); - crc_deinit(); - crc_data_register_reset(); - - // 临时缓冲区用于计算CRC - uint32_t word_count = size / 4; - uint32_t src_crc, dst_crc; - - // 计算源数据CRC - src_crc = crc_block_data_calculate((uint32_t *)src_addr, word_count); - // 关闭全局中断以防止Flash操作期间的中断干扰 __disable_irq(); - fmc_unlock(); // 首先擦除目标地址的Flash页面 - // 计算需要擦除的页面数量（GD32C103的Flash页大小为1KB） - uint32_t page_size = 1024; // 1KB per page - uint32_t num_pages = (size + page_size - 1) / page_size; // 向上取整 + uint32_t num_pages = (APP_MAX_SIZE + FMC_PAGE_SIZE - 1) / FMC_PAGE_SIZE; // 向上取整 for (uint32_t i = 0; i < num_pages; i++) { - uint32_t page_addr = dst_addr + i * page_size; + uint32_t page_addr = APPBACK_START_ADDR + i * FMC_PAGE_SIZE; // 擦除页面 fmc_status = fmc_page_erase(page_addr); if (fmc_status != FMC_READY) { fmc_lock(); +#if OTA_DEBUG + printf("move_app_data erase error\r\n"); +#endif __enable_irq(); // 重新开启中断 return fmc_status; } } // 复制数据 - 按字（32位）进行复制以提高效率 - for (uint32_t offset = 0; offset < size; offset += 4) { - uint32_t src_data = *(volatile uint32_t *)(src_addr + offset); + uint32_t offset = 0; + uint32_t write_count = 0; // 记录实际写入的字数 + const uint32_t* src_ptr = (const uint32_t*)APPLATEST_START_ADDR; + uint32_t* dst_ptr = (uint32_t*)APPBACK_START_ADDR; + + // 预计算源数据的CRC，避免重复访问内存 + crc_deinit(); + crc_data_register_reset(); + uint32_t src_crc = crc_block_data_calculate((uint32_t*)APPLATEST_START_ADDR, APP_MAX_SIZE/4); + + while (offset < APP_MAX_SIZE) { + uint32_t src_data = src_ptr[offset/4]; // 检查源地址是否为有效的Flash数据（非0xFFFFFFFF表示有数据） if (src_data != 0xFFFFFFFF) { - fmc_status = fmc_word_program(dst_addr + offset, src_data); + // 清除之前的标志位 + fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGAERR | FMC_FLAG_PGERR); + + fmc_status = fmc_word_program(APPBACK_START_ADDR + offset, src_data); + + // 检查写入操作是否成功 if (fmc_status != FMC_READY) { +#if OTA_DEBUG + printf("Flash write failed at offset 0x%08X with status: %d\r\n", offset, fmc_status); +#endif fmc_lock(); __enable_irq(); // 重新开启中断 return fmc_status; } + write_count++; + } + offset += 4; + + // 每写入一定数量的数据后，喂狗一次 + if ((offset % 4096) == 0) { // 每4KB喂狗一次 + fwdgt_counter_reload(); } } @@ -697,21 +728,21 @@ fmc_state_enum move_app_data() { // 重新开启全局中断 __enable_irq(); + crc_data_register_reset(); // 确保CRC计算从初始状态开始 - // 计算目标数据的CRC - crc_data_register_reset(); - dst_crc = crc_block_data_calculate((uint32_t *)dst_addr, word_count); + // 计算目标数据的CRC值 + uint32_t dst_crc = crc_block_data_calculate((uint32_t*)APPBACK_START_ADDR, APP_MAX_SIZE/4); - // 比较源数据和目标数据的CRC + // 比较CRC值，确保数据完整性 if (src_crc != dst_crc) { - // 如果CRC不匹配，可以选择返回错误状态 - // 这里我们返回一个特殊值表示CRC校验失败 - return (fmc_state_enum)0xFF; // 自定义错误码表示CRC校验失败 +#if OTA_DEBUG + printf("CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n", src_crc, dst_crc); +#endif + return (fmc_state_enum)0xFF; // CRC校验失败 } - // 设置备份应用可用标志 OTA_SET(n_APP_BACKUP_AVAL_ADDR, OTA_AVAL); OTA_SET(n_APP_BACKUP_CHECK_ADDR, OTA_AVAL); return fmc_status; -} \ No newline at end of file +} diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 826aa50..0c67474 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -71,7 +71,7 @@ class FlashEEPROM { }; void flash_eeprom_example_main(void); -u8 OTA_SET(u32 addr, bool enable); +u8 OTA_SET(u32 addr, u32 data) ; fmc_state_enum move_app_data(void); #endif /* __FLASH_H */ \ No newline at end of file diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index 8c1e907..f8d7d86 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -2,13 +2,15 @@ #include "config.h" #include "flash.h" #include "gd32c10x.h" +#include "gd32c10x_fmc.h" #include "gd32c10x_gpio.h" #include "gd32c10x_rcu.h" -#include "gd32c10x_fmc.h" #include #include +#include "can.h" #include +extern CAN_transmiter CAN0_transmiter; // CAN初始化 /* retarget the C library printf function to the USART */ u8 USART0_init() { gd_eval_com_init(EVAL_COM0); @@ -96,12 +98,14 @@ u16 get_reset_cause(void) { void OTA_initial(void) { // 确认当前APP可用 - OTA_SET(n_APP_LATEST_CHECK_ADDR, true); + OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL); + OTA_SET(n_APP_LATEST_EN_ADDR, OTA_AVAL); volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; u32 back_aval = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; u32 back_check = *flash_ptr; + // 如果BACK不可用 if (back_aval == OTA_NOT_AVAL) { move_app_data(); From 72506ce87debbbda2bee3fffe8af18961083b9ba Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Wed, 7 Jan 2026 23:13:21 +0800 Subject: [PATCH 12/16] =?UTF-8?q?feat(FOC):=20=E9=9B=86=E6=88=90ARM?= =?UTF-8?q?=E6=95=B0=E5=AD=A6=E5=BA=93=E4=BC=98=E5=8C=96FOC=E7=AE=97?= =?UTF-8?q?=E6=B3=95=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 引入arm_math.h库以使用ARM优化的数学函数 - 将角度归一化从弧度制改为角度制，使用360.0f替代2*PI - 使用arm_sin_cos_f32替代查找表实现正弦余弦计算 - 移除注释中的ARM Clark和Park变换函数调用 - 修正机械角度计算中的时间常量为TIMER2_FREQUENCY倒数 - 移除传感器角度到弧度的转换，保持原始角度值 - 激活电流环控制中的Iq/Id计算并发送CAN数据 --- Robot_V2/Algorithm/FOC.cpp | 46 +++-- Robot_V2/Algorithm/FOC.h | 353 +-------------------------------- Robot_V2/Algorithm/Filter.cpp | 14 +- Robot_V2/Robot_V2.cproject.yml | 3 + Robot_V2/device/ADC.cpp | 1 + Robot_V2/device/Can.cpp | 2 + Robot_V2/device/Can.h | 7 +- Robot_V2/device/Timer.cpp | 104 +++++----- Robot_V2/device/ang_sensor.cpp | 2 +- Robot_V2/main.cpp | 18 +- Robot_V2/sys/config.h | 11 +- Robot_V2/sys/flash.h | 4 +- Robot_V2/sys/initial.cpp | 11 +- Robot_V2/sys/systick.cpp | 9 +- 14 files changed, 133 insertions(+), 452 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 82184f1..a093f79 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -6,11 +6,11 @@ #include "config.h" #include "motor.h" #include "pid.h" +#include #include #include #include - FOC_Controlroller FOC; // FOC控制初始化 extern sampler sampler; extern angle_sensor KTH7823; @@ -22,16 +22,16 @@ extern CAN_transmiter CAN0_transmiter; #define constrain(value, min, max, target_min, target_max) \ ((value - min) / (max - min) * (target_max - target_min)) -/** +/**@ * @brief 归一化角度 * * @param angle * @return float */ inline float _normalizeAngle(float angle) { - float a = fmod(angle, 2 * PI); + float a = fmod(angle, 360.0f); // 取余运算可以用于归一化，列出特殊值例子算便知 - return a >= 0 ? a : (a + 2 * PI); + return a >= 0 ? a : (a + 360.0f); } // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq) @@ -40,16 +40,19 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b, // 计算电气角度 float angle_el = _normalizeAngle(shaft_angle * PP); + float I_alpha, I_beta; // Clarke变换 (使用三相电流) - float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b - - (1.0f / 3.0f) * current_c; - float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c; + // arm_clarke_f32(current_a, current_b, &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; - cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK]; - sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK]; - float I_q = I_beta * cos_el - I_alpha * sin_el; + arm_sin_cos_f32(angle_el, &sin_el, &cos_el); + float I_q, I_d; + I_q = I_beta * cos_el - I_alpha * sin_el; + // arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el); return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA } @@ -73,8 +76,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { // 帕克逆变换 (直接计算，避免函数调用开销) float sin_el, cos_el; - sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK]; - cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK]; + 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; @@ -93,7 +95,6 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { duty_b = duty_b * voltage_exchange; duty_c = duty_c * voltage_exchange; Set_3Pwm(duty_a, duty_b, duty_c); - // std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c); } /** @@ -156,8 +157,8 @@ void FOC_Controlroller::update_power(void) { float FOC_Controlroller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 // 预计算常量 1e-4 = 100微秒控制周期 - shaft_angle = - _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f); + shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * + 1.0 / TIMER2_FREQUENCY); // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = voltage_power_supply / 2.0f; @@ -195,7 +196,7 @@ float FOC_Controlroller::angle_control() { float Uq = angle_loop(target_angle - rt_angle); // 获取电机轴角度 - shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; + shaft_angle = KTH7823.getcurrent_sensorangle(); // 设置相电压 setPhaseVoltage(Uq, 0.0f); return shaft_angle; @@ -224,7 +225,7 @@ float FOC_Controlroller::angle_control_with_velocity_limit() { // CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity; // 获取电机轴角度 - shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; + shaft_angle = KTH7823.getcurrent_sensorangle(); // 设置相电压 setPhaseVoltage(Uq, 0.0f); @@ -243,12 +244,13 @@ float FOC_Controlroller::velocity_control() { float Uq = vel_loop((target_velocity - rt_velocity) * 25); // 获取电机轴角度 - shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; + shaft_angle = KTH7823.getcurrent_sensorangle(); // 设置相电压 setPhaseVoltage(Uq, 0.0f); - // CAN0_transmiter.testfloat_union.f = - cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), - sampler.get_current('W')); + CAN0_transmiter.testfloat_union.f = + cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); + return rt_velocity; } @@ -259,7 +261,7 @@ float FOC_Controlroller::current_control() { float Uq = current_loop(target_current - Iq); CAN0_transmiter.testfloat_union.f = Iq; // 获取电机轴角度 - shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; + shaft_angle = KTH7823.getcurrent_sensorangle(); // 设置相电压 setPhaseVoltage(Uq, 0.0f); return 1; diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index a4a8d38..e882a55 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -45,15 +45,15 @@ class FOC_Controlroller { float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 PIDController vel_loop = - PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V + PIDController{0.01f, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000, voltage_power_supply}; // 12V PIDController current_loop = PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V PIDController vel_inVelAngloop = - PIDController{0.35f, 5.0f, 1e-3, 1000000, voltage_power_supply}; + PIDController{0.12f, 3.0f, 5e-4, 1000000, voltage_power_supply}; PIDController ang_inVelAngloop = - PIDController{30.0f, 0.0f, 1e-3, 1000000, 999}; + PIDController{100.0f, 0.0f, 5e-4, 1000000, 999}; void setPhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); @@ -62,351 +62,8 @@ class FOC_Controlroller { // 定义函数指针类型 typedef float (FOC_Controlroller::*FOCFunctionPtr)(void); -float _normalizeAngle(float angle); +inline float _normalizeAngle(float angle); inline float _electricalAngle(); -__attribute__((aligned(4))) const float sin_lut[LUT_SIZE] = { - 0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f, - 0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f, - 0.073565f, 0.079682f, 0.085797f, 0.091909f, 0.098017f, 0.104122f, - 0.110222f, 0.116319f, 0.122411f, 0.128498f, 0.134581f, 0.140658f, - 0.146730f, 0.152797f, 0.158858f, 0.164913f, 0.170962f, 0.177004f, - 0.183040f, 0.189069f, 0.195090f, 0.201105f, 0.207111f, 0.213110f, - 0.219101f, 0.225084f, 0.231058f, 0.237024f, 0.242980f, 0.248928f, - 0.254866f, 0.260794f, 0.266713f, 0.272621f, 0.278520f, 0.284408f, - 0.290285f, 0.296151f, 0.302006f, 0.307850f, 0.313682f, 0.319502f, - 0.325310f, 0.331106f, 0.336890f, 0.342661f, 0.348419f, 0.354164f, - 0.359895f, 0.365613f, 0.371317f, 0.377007f, 0.382683f, 0.388345f, - 0.393992f, 0.399624f, 0.405241f, 0.410843f, 0.416430f, 0.422000f, - 0.427555f, 0.433094f, 0.438616f, 0.444122f, 0.449611f, 0.455084f, - 0.460539f, 0.465976f, 0.471397f, 0.476799f, 0.482184f, 0.487550f, - 0.492898f, 0.498228f, 0.503538f, 0.508830f, 0.514103f, 0.519356f, - 0.524590f, 0.529804f, 0.534998f, 0.540171f, 0.545325f, 0.550458f, - 0.555570f, 0.560662f, 0.565732f, 0.570781f, 0.575808f, 0.580814f, - 0.585798f, 0.590760f, 0.595699f, 0.600616f, 0.605511f, 0.610383f, - 0.615232f, 0.620057f, 0.624859f, 0.629638f, 0.634393f, 0.639124f, - 0.643832f, 0.648514f, 0.653173f, 0.657807f, 0.662416f, 0.667000f, - 0.671559f, 0.676093f, 0.680601f, 0.685084f, 0.689541f, 0.693971f, - 0.698376f, 0.702755f, 0.707107f, 0.711432f, 0.715731f, 0.720003f, - 0.724247f, 0.728464f, 0.732654f, 0.736817f, 0.740951f, 0.745058f, - 0.749136f, 0.753187f, 0.757209f, 0.761202f, 0.765167f, 0.769103f, - 0.773010f, 0.776888f, 0.780737f, 0.784557f, 0.788346f, 0.792107f, - 0.795837f, 0.799537f, 0.803208f, 0.806848f, 0.810457f, 0.814036f, - 0.817585f, 0.821103f, 0.824589f, 0.828045f, 0.831470f, 0.834863f, - 0.838225f, 0.841555f, 0.844854f, 0.848120f, 0.851355f, 0.854558f, - 0.857729f, 0.860867f, 0.863973f, 0.867046f, 0.870087f, 0.873095f, - 0.876070f, 0.879012f, 0.881921f, 0.884797f, 0.887640f, 0.890449f, - 0.893224f, 0.895966f, 0.898674f, 0.901349f, 0.903989f, 0.906596f, - 0.909168f, 0.911706f, 0.914210f, 0.916679f, 0.919114f, 0.921514f, - 0.923880f, 0.926210f, 0.928506f, 0.930767f, 0.932993f, 0.935184f, - 0.937339f, 0.939459f, 0.941544f, 0.943593f, 0.945607f, 0.947586f, - 0.949528f, 0.951435f, 0.953306f, 0.955141f, 0.956940f, 0.958703f, - 0.960431f, 0.962121f, 0.963776f, 0.965394f, 0.966976f, 0.968522f, - 0.970031f, 0.971504f, 0.972940f, 0.974339f, 0.975702f, 0.977028f, - 0.978317f, 0.979570f, 0.980785f, 0.981964f, 0.983105f, 0.984210f, - 0.985278f, 0.986308f, 0.987301f, 0.988258f, 0.989177f, 0.990058f, - 0.990903f, 0.991710f, 0.992480f, 0.993212f, 0.993907f, 0.994565f, - 0.995185f, 0.995767f, 0.996313f, 0.996820f, 0.997290f, 0.997723f, - 0.998118f, 0.998476f, 0.998795f, 0.999078f, 0.999322f, 0.999529f, - 0.999699f, 0.999831f, 0.999925f, 0.999981f, 1.000000f, 0.999981f, - 0.999925f, 0.999831f, 0.999699f, 0.999529f, 0.999322f, 0.999078f, - 0.998795f, 0.998476f, 0.998118f, 0.997723f, 0.997290f, 0.996820f, - 0.996313f, 0.995767f, 0.995185f, 0.994565f, 0.993907f, 0.993212f, - 0.992480f, 0.991710f, 0.990903f, 0.990058f, 0.989177f, 0.988258f, - 0.987301f, 0.986308f, 0.985278f, 0.984210f, 0.983105f, 0.981964f, - 0.980785f, 0.979570f, 0.978317f, 0.977028f, 0.975702f, 0.974339f, - 0.972940f, 0.971504f, 0.970031f, 0.968522f, 0.966976f, 0.965394f, - 0.963776f, 0.962121f, 0.960431f, 0.958703f, 0.956940f, 0.955141f, - 0.953306f, 0.951435f, 0.949528f, 0.947586f, 0.945607f, 0.943593f, - 0.941544f, 0.939459f, 0.937339f, 0.935184f, 0.932993f, 0.930767f, - 0.928506f, 0.926210f, 0.923880f, 0.921514f, 0.919114f, 0.916679f, - 0.914210f, 0.911706f, 0.909168f, 0.906596f, 0.903989f, 0.901349f, - 0.898674f, 0.895966f, 0.893224f, 0.890449f, 0.887640f, 0.884797f, - 0.881921f, 0.879012f, 0.876070f, 0.873095f, 0.870087f, 0.867046f, - 0.863973f, 0.860867f, 0.857729f, 0.854558f, 0.851355f, 0.848120f, - 0.844854f, 0.841555f, 0.838225f, 0.834863f, 0.831470f, 0.828045f, - 0.824589f, 0.821103f, 0.817585f, 0.814036f, 0.810457f, 0.806848f, - 0.803208f, 0.799537f, 0.795837f, 0.792107f, 0.788346f, 0.784557f, - 0.780737f, 0.776888f, 0.773010f, 0.769103f, 0.765167f, 0.761202f, - 0.757209f, 0.753187f, 0.749136f, 0.745058f, 0.740951f, 0.736817f, - 0.732654f, 0.728464f, 0.724247f, 0.720003f, 0.715731f, 0.711432f, - 0.707107f, 0.702755f, 0.698376f, 0.693971f, 0.689541f, 0.685084f, - 0.680601f, 0.676093f, 0.671559f, 0.667000f, 0.662416f, 0.657807f, - 0.653173f, 0.648514f, 0.643832f, 0.639124f, 0.634393f, 0.629638f, - 0.624859f, 0.620057f, 0.615232f, 0.610383f, 0.605511f, 0.600616f, - 0.595699f, 0.590760f, 0.585798f, 0.580814f, 0.575808f, 0.570781f, - 0.565732f, 0.560662f, 0.555570f, 0.550458f, 0.545325f, 0.540171f, - 0.534998f, 0.529804f, 0.524590f, 0.519356f, 0.514103f, 0.508830f, - 0.503538f, 0.498228f, 0.492898f, 0.487550f, 0.482184f, 0.476799f, - 0.471397f, 0.465976f, 0.460539f, 0.455084f, 0.449611f, 0.444122f, - 0.438616f, 0.433094f, 0.427555f, 0.422000f, 0.416430f, 0.410843f, - 0.405241f, 0.399624f, 0.393992f, 0.388345f, 0.382683f, 0.377007f, - 0.371317f, 0.365613f, 0.359895f, 0.354164f, 0.348419f, 0.342661f, - 0.336890f, 0.331106f, 0.325310f, 0.319502f, 0.313682f, 0.307850f, - 0.302006f, 0.296151f, 0.290285f, 0.284408f, 0.278520f, 0.272621f, - 0.266713f, 0.260794f, 0.254866f, 0.248928f, 0.242980f, 0.237024f, - 0.231058f, 0.225084f, 0.219101f, 0.213110f, 0.207111f, 0.201105f, - 0.195090f, 0.189069f, 0.183040f, 0.177004f, 0.170962f, 0.164913f, - 0.158858f, 0.152797f, 0.146730f, 0.140658f, 0.134581f, 0.128498f, - 0.122411f, 0.116319f, 0.110222f, 0.104122f, 0.098017f, 0.091909f, - 0.085797f, 0.079682f, 0.073565f, 0.067444f, 0.061321f, 0.055195f, - 0.049068f, 0.042938f, 0.036807f, 0.030675f, 0.024541f, 0.018407f, - 0.012272f, 0.006136f, 0.000000f, -0.006136f, -0.012272f, -0.018407f, - -0.024541f, -0.030675f, -0.036807f, -0.042938f, -0.049068f, -0.055195f, - -0.061321f, -0.067444f, -0.073565f, -0.079682f, -0.085797f, -0.091909f, - -0.098017f, -0.104122f, -0.110222f, -0.116319f, -0.122411f, -0.128498f, - -0.134581f, -0.140658f, -0.146730f, -0.152797f, -0.158858f, -0.164913f, - -0.170962f, -0.177004f, -0.183040f, -0.189069f, -0.195090f, -0.201105f, - -0.207111f, -0.213110f, -0.219101f, -0.225084f, -0.231058f, -0.237024f, - -0.242980f, -0.248928f, -0.254866f, -0.260794f, -0.266713f, -0.272621f, - -0.278520f, -0.284408f, -0.290285f, -0.296151f, -0.302006f, -0.307850f, - -0.313682f, -0.319502f, -0.325310f, -0.331106f, -0.336890f, -0.342661f, - -0.348419f, -0.354164f, -0.359895f, -0.365613f, -0.371317f, -0.377007f, - -0.382683f, -0.388345f, -0.393992f, -0.399624f, -0.405241f, -0.410843f, - -0.416430f, -0.422000f, -0.427555f, -0.433094f, -0.438616f, -0.444122f, - -0.449611f, -0.455084f, -0.460539f, -0.465976f, -0.471397f, -0.476799f, - -0.482184f, -0.487550f, -0.492898f, -0.498228f, -0.503538f, -0.508830f, - -0.514103f, -0.519356f, -0.524590f, -0.529804f, -0.534998f, -0.540171f, - -0.545325f, -0.550458f, -0.555570f, -0.560662f, -0.565732f, -0.570781f, - -0.575808f, -0.580814f, -0.585798f, -0.590760f, -0.595699f, -0.600616f, - -0.605511f, -0.610383f, -0.615232f, -0.620057f, -0.624859f, -0.629638f, - -0.634393f, -0.639124f, -0.643832f, -0.648514f, -0.653173f, -0.657807f, - -0.662416f, -0.667000f, -0.671559f, -0.676093f, -0.680601f, -0.685084f, - -0.689541f, -0.693971f, -0.698376f, -0.702755f, -0.707107f, -0.711432f, - -0.715731f, -0.720003f, -0.724247f, -0.728464f, -0.732654f, -0.736817f, - -0.740951f, -0.745058f, -0.749136f, -0.753187f, -0.757209f, -0.761202f, - -0.765167f, -0.769103f, -0.773010f, -0.776888f, -0.780737f, -0.784557f, - -0.788346f, -0.792107f, -0.795837f, -0.799537f, -0.803208f, -0.806848f, - -0.810457f, -0.814036f, -0.817585f, -0.821103f, -0.824589f, -0.828045f, - -0.831470f, -0.834863f, -0.838225f, -0.841555f, -0.844854f, -0.848120f, - -0.851355f, -0.854558f, -0.857729f, -0.860867f, -0.863973f, -0.867046f, - -0.870087f, -0.873095f, -0.876070f, -0.879012f, -0.881921f, -0.884797f, - -0.887640f, -0.890449f, -0.893224f, -0.895966f, -0.898674f, -0.901349f, - -0.903989f, -0.906596f, -0.909168f, -0.911706f, -0.914210f, -0.916679f, - -0.919114f, -0.921514f, -0.923880f, -0.926210f, -0.928506f, -0.930767f, - -0.932993f, -0.935184f, -0.937339f, -0.939459f, -0.941544f, -0.943593f, - -0.945607f, -0.947586f, -0.949528f, -0.951435f, -0.953306f, -0.955141f, - -0.956940f, -0.958703f, -0.960431f, -0.962121f, -0.963776f, -0.965394f, - -0.966976f, -0.968522f, -0.970031f, -0.971504f, -0.972940f, -0.974339f, - -0.975702f, -0.977028f, -0.978317f, -0.979570f, -0.980785f, -0.981964f, - -0.983105f, -0.984210f, -0.985278f, -0.986308f, -0.987301f, -0.988258f, - -0.989177f, -0.990058f, -0.990903f, -0.991710f, -0.992480f, -0.993212f, - -0.993907f, -0.994565f, -0.995185f, -0.995767f, -0.996313f, -0.996820f, - -0.997290f, -0.997723f, -0.998118f, -0.998476f, -0.998795f, -0.999078f, - -0.999322f, -0.999529f, -0.999699f, -0.999831f, -0.999925f, -0.999981f, - -1.000000f, -0.999981f, -0.999925f, -0.999831f, -0.999699f, -0.999529f, - -0.999322f, -0.999078f, -0.998795f, -0.998476f, -0.998118f, -0.997723f, - -0.997290f, -0.996820f, -0.996313f, -0.995767f, -0.995185f, -0.994565f, - -0.993907f, -0.993212f, -0.992480f, -0.991710f, -0.990903f, -0.990058f, - -0.989177f, -0.988258f, -0.987301f, -0.986308f, -0.985278f, -0.984210f, - -0.983105f, -0.981964f, -0.980785f, -0.979570f, -0.978317f, -0.977028f, - -0.975702f, -0.974339f, -0.972940f, -0.971504f, -0.970031f, -0.968522f, - -0.966976f, -0.965394f, -0.963776f, -0.962121f, -0.960431f, -0.958703f, - -0.956940f, -0.955141f, -0.953306f, -0.951435f, -0.949528f, -0.947586f, - -0.945607f, -0.943593f, -0.941544f, -0.939459f, -0.937339f, -0.935184f, - -0.932993f, -0.930767f, -0.928506f, -0.926210f, -0.923880f, -0.921514f, - -0.919114f, -0.916679f, -0.914210f, -0.911706f, -0.909168f, -0.906596f, - -0.903989f, -0.901349f, -0.898674f, -0.895966f, -0.893224f, -0.890449f, - -0.887640f, -0.884797f, -0.881921f, -0.879012f, -0.876070f, -0.873095f, - -0.870087f, -0.867046f, -0.863973f, -0.860867f, -0.857729f, -0.854558f, - -0.851355f, -0.848120f, -0.844854f, -0.841555f, -0.838225f, -0.834863f, - -0.831470f, -0.828045f, -0.824589f, -0.821103f, -0.817585f, -0.814036f, - -0.810457f, -0.806848f, -0.803208f, -0.799537f, -0.795837f, -0.792107f, - -0.788346f, -0.784557f, -0.780737f, -0.776888f, -0.773010f, -0.769103f, - -0.765167f, -0.761202f, -0.757209f, -0.753187f, -0.749136f, -0.745058f, - -0.740951f, -0.736817f, -0.732654f, -0.728464f, -0.724247f, -0.720003f, - -0.715731f, -0.711432f, -0.707107f, -0.702755f, -0.698376f, -0.693971f, - -0.689541f, -0.685084f, -0.680601f, -0.676093f, -0.671559f, -0.667000f, - -0.662416f, -0.657807f, -0.653173f, -0.648514f, -0.643832f, -0.639124f, - -0.634393f, -0.629638f, -0.624859f, -0.620057f, -0.615232f, -0.610383f, - -0.605511f, -0.600616f, -0.595699f, -0.590760f, -0.585798f, -0.580814f, - -0.575808f, -0.570781f, -0.565732f, -0.560662f, -0.555570f, -0.550458f, - -0.545325f, -0.540171f, -0.534998f, -0.529804f, -0.524590f, -0.519356f, - -0.514103f, -0.508830f, -0.503538f, -0.498228f, -0.492898f, -0.487550f, - -0.482184f, -0.476799f, -0.471397f, -0.465976f, -0.460539f, -0.455084f, - -0.449611f, -0.444122f, -0.438616f, -0.433094f, -0.427555f, -0.422000f, - -0.416430f, -0.410843f, -0.405241f, -0.399624f, -0.393992f, -0.388345f, - -0.382683f, -0.377007f, -0.371317f, -0.365613f, -0.359895f, -0.354164f, - -0.348419f, -0.342661f, -0.336890f, -0.331106f, -0.325310f, -0.319502f, - -0.313682f, -0.307850f, -0.302006f, -0.296151f, -0.290285f, -0.284408f, - -0.278520f, -0.272621f, -0.266713f, -0.260794f, -0.254866f, -0.248928f, - -0.242980f, -0.237024f, -0.231058f, -0.225084f, -0.219101f, -0.213110f, - -0.207111f, -0.201105f, -0.195090f, -0.189069f, -0.183040f, -0.177004f, - -0.170962f, -0.164913f, -0.158858f, -0.152797f, -0.146730f, -0.140658f, - -0.134581f, -0.128498f, -0.122411f, -0.116319f, -0.110222f, -0.104122f, - -0.098017f, -0.091909f, -0.085797f, -0.079682f, -0.073565f, -0.067444f, - -0.061321f, -0.055195f, -0.049068f, -0.042938f, -0.036807f, -0.030675f, - -0.024541f, -0.018407f, -0.012272f, -0.006136f}; -__attribute__((aligned(4))) const float cos_lut[LUT_SIZE] = { - 1.000000f, 0.999981f, 0.999925f, 0.999831f, 0.999699f, 0.999529f, - 0.999322f, 0.999078f, 0.998795f, 0.998476f, 0.998118f, 0.997723f, - 0.997290f, 0.996820f, 0.996313f, 0.995767f, 0.995185f, 0.994565f, - 0.993907f, 0.993212f, 0.992480f, 0.991710f, 0.990903f, 0.990058f, - 0.989177f, 0.988258f, 0.987301f, 0.986308f, 0.985278f, 0.984210f, - 0.983105f, 0.981964f, 0.980785f, 0.979570f, 0.978317f, 0.977028f, - 0.975702f, 0.974339f, 0.972940f, 0.971504f, 0.970031f, 0.968522f, - 0.966976f, 0.965394f, 0.963776f, 0.962121f, 0.960431f, 0.958703f, - 0.956940f, 0.955141f, 0.953306f, 0.951435f, 0.949528f, 0.947586f, - 0.945607f, 0.943593f, 0.941544f, 0.939459f, 0.937339f, 0.935184f, - 0.932993f, 0.930767f, 0.928506f, 0.926210f, 0.923880f, 0.921514f, - 0.919114f, 0.916679f, 0.914210f, 0.911706f, 0.909168f, 0.906596f, - 0.903989f, 0.901349f, 0.898674f, 0.895966f, 0.893224f, 0.890449f, - 0.887640f, 0.884797f, 0.881921f, 0.879012f, 0.876070f, 0.873095f, - 0.870087f, 0.867046f, 0.863973f, 0.860867f, 0.857729f, 0.854558f, - 0.851355f, 0.848120f, 0.844854f, 0.841555f, 0.838225f, 0.834863f, - 0.831470f, 0.828045f, 0.824589f, 0.821103f, 0.817585f, 0.814036f, - 0.810457f, 0.806848f, 0.803208f, 0.799537f, 0.795837f, 0.792107f, - 0.788346f, 0.784557f, 0.780737f, 0.776888f, 0.773010f, 0.769103f, - 0.765167f, 0.761202f, 0.757209f, 0.753187f, 0.749136f, 0.745058f, - 0.740951f, 0.736817f, 0.732654f, 0.728464f, 0.724247f, 0.720003f, - 0.715731f, 0.711432f, 0.707107f, 0.702755f, 0.698376f, 0.693971f, - 0.689541f, 0.685084f, 0.680601f, 0.676093f, 0.671559f, 0.667000f, - 0.662416f, 0.657807f, 0.653173f, 0.648514f, 0.643832f, 0.639124f, - 0.634393f, 0.629638f, 0.624859f, 0.620057f, 0.615232f, 0.610383f, - 0.605511f, 0.600616f, 0.595699f, 0.590760f, 0.585798f, 0.580814f, - 0.575808f, 0.570781f, 0.565732f, 0.560662f, 0.555570f, 0.550458f, - 0.545325f, 0.540171f, 0.534998f, 0.529804f, 0.524590f, 0.519356f, - 0.514103f, 0.508830f, 0.503538f, 0.498228f, 0.492898f, 0.487550f, - 0.482184f, 0.476799f, 0.471397f, 0.465976f, 0.460539f, 0.455084f, - 0.449611f, 0.444122f, 0.438616f, 0.433094f, 0.427555f, 0.422000f, - 0.416430f, 0.410843f, 0.405241f, 0.399624f, 0.393992f, 0.388345f, - 0.382683f, 0.377007f, 0.371317f, 0.365613f, 0.359895f, 0.354164f, - 0.348419f, 0.342661f, 0.336890f, 0.331106f, 0.325310f, 0.319502f, - 0.313682f, 0.307850f, 0.302006f, 0.296151f, 0.290285f, 0.284408f, - 0.278520f, 0.272621f, 0.266713f, 0.260794f, 0.254866f, 0.248928f, - 0.242980f, 0.237024f, 0.231058f, 0.225084f, 0.219101f, 0.213110f, - 0.207111f, 0.201105f, 0.195090f, 0.189069f, 0.183040f, 0.177004f, - 0.170962f, 0.164913f, 0.158858f, 0.152797f, 0.146730f, 0.140658f, - 0.134581f, 0.128498f, 0.122411f, 0.116319f, 0.110222f, 0.104122f, - 0.098017f, 0.091909f, 0.085797f, 0.079682f, 0.073565f, 0.067444f, - 0.061321f, 0.055195f, 0.049068f, 0.042938f, 0.036807f, 0.030675f, - 0.024541f, 0.018407f, 0.012272f, 0.006136f, 0.000000f, -0.006136f, - -0.012272f, -0.018407f, -0.024541f, -0.030675f, -0.036807f, -0.042938f, - -0.049068f, -0.055195f, -0.061321f, -0.067444f, -0.073565f, -0.079682f, - -0.085797f, -0.091909f, -0.098017f, -0.104122f, -0.110222f, -0.116319f, - -0.122411f, -0.128498f, -0.134581f, -0.140658f, -0.146730f, -0.152797f, - -0.158858f, -0.164913f, -0.170962f, -0.177004f, -0.183040f, -0.189069f, - -0.195090f, -0.201105f, -0.207111f, -0.213110f, -0.219101f, -0.225084f, - -0.231058f, -0.237024f, -0.242980f, -0.248928f, -0.254866f, -0.260794f, - -0.266713f, -0.272621f, -0.278520f, -0.284408f, -0.290285f, -0.296151f, - -0.302006f, -0.307850f, -0.313682f, -0.319502f, -0.325310f, -0.331106f, - -0.336890f, -0.342661f, -0.348419f, -0.354164f, -0.359895f, -0.365613f, - -0.371317f, -0.377007f, -0.382683f, -0.388345f, -0.393992f, -0.399624f, - -0.405241f, -0.410843f, -0.416430f, -0.422000f, -0.427555f, -0.433094f, - -0.438616f, -0.444122f, -0.449611f, -0.455084f, -0.460539f, -0.465976f, - -0.471397f, -0.476799f, -0.482184f, -0.487550f, -0.492898f, -0.498228f, - -0.503538f, -0.508830f, -0.514103f, -0.519356f, -0.524590f, -0.529804f, - -0.534998f, -0.540171f, -0.545325f, -0.550458f, -0.555570f, -0.560662f, - -0.565732f, -0.570781f, -0.575808f, -0.580814f, -0.585798f, -0.590760f, - -0.595699f, -0.600616f, -0.605511f, -0.610383f, -0.615232f, -0.620057f, - -0.624859f, -0.629638f, -0.634393f, -0.639124f, -0.643832f, -0.648514f, - -0.653173f, -0.657807f, -0.662416f, -0.667000f, -0.671559f, -0.676093f, - -0.680601f, -0.685084f, -0.689541f, -0.693971f, -0.698376f, -0.702755f, - -0.707107f, -0.711432f, -0.715731f, -0.720003f, -0.724247f, -0.728464f, - -0.732654f, -0.736817f, -0.740951f, -0.745058f, -0.749136f, -0.753187f, - -0.757209f, -0.761202f, -0.765167f, -0.769103f, -0.773010f, -0.776888f, - -0.780737f, -0.784557f, -0.788346f, -0.792107f, -0.795837f, -0.799537f, - -0.803208f, -0.806848f, -0.810457f, -0.814036f, -0.817585f, -0.821103f, - -0.824589f, -0.828045f, -0.831470f, -0.834863f, -0.838225f, -0.841555f, - -0.844854f, -0.848120f, -0.851355f, -0.854558f, -0.857729f, -0.860867f, - -0.863973f, -0.867046f, -0.870087f, -0.873095f, -0.876070f, -0.879012f, - -0.881921f, -0.884797f, -0.887640f, -0.890449f, -0.893224f, -0.895966f, - -0.898674f, -0.901349f, -0.903989f, -0.906596f, -0.909168f, -0.911706f, - -0.914210f, -0.916679f, -0.919114f, -0.921514f, -0.923880f, -0.926210f, - -0.928506f, -0.930767f, -0.932993f, -0.935184f, -0.937339f, -0.939459f, - -0.941544f, -0.943593f, -0.945607f, -0.947586f, -0.949528f, -0.951435f, - -0.953306f, -0.955141f, -0.956940f, -0.958703f, -0.960431f, -0.962121f, - -0.963776f, -0.965394f, -0.966976f, -0.968522f, -0.970031f, -0.971504f, - -0.972940f, -0.974339f, -0.975702f, -0.977028f, -0.978317f, -0.979570f, - -0.980785f, -0.981964f, -0.983105f, -0.984210f, -0.985278f, -0.986308f, - -0.987301f, -0.988258f, -0.989177f, -0.990058f, -0.990903f, -0.991710f, - -0.992480f, -0.993212f, -0.993907f, -0.994565f, -0.995185f, -0.995767f, - -0.996313f, -0.996820f, -0.997290f, -0.997723f, -0.998118f, -0.998476f, - -0.998795f, -0.999078f, -0.999322f, -0.999529f, -0.999699f, -0.999831f, - -0.999925f, -0.999981f, -1.000000f, -0.999981f, -0.999925f, -0.999831f, - -0.999699f, -0.999529f, -0.999322f, -0.999078f, -0.998795f, -0.998476f, - -0.998118f, -0.997723f, -0.997290f, -0.996820f, -0.996313f, -0.995767f, - -0.995185f, -0.994565f, -0.993907f, -0.993212f, -0.992480f, -0.991710f, - -0.990903f, -0.990058f, -0.989177f, -0.988258f, -0.987301f, -0.986308f, - -0.985278f, -0.984210f, -0.983105f, -0.981964f, -0.980785f, -0.979570f, - -0.978317f, -0.977028f, -0.975702f, -0.974339f, -0.972940f, -0.971504f, - -0.970031f, -0.968522f, -0.966976f, -0.965394f, -0.963776f, -0.962121f, - -0.960431f, -0.958703f, -0.956940f, -0.955141f, -0.953306f, -0.951435f, - -0.949528f, -0.947586f, -0.945607f, -0.943593f, -0.941544f, -0.939459f, - -0.937339f, -0.935184f, -0.932993f, -0.930767f, -0.928506f, -0.926210f, - -0.923880f, -0.921514f, -0.919114f, -0.916679f, -0.914210f, -0.911706f, - -0.909168f, -0.906596f, -0.903989f, -0.901349f, -0.898674f, -0.895966f, - -0.893224f, -0.890449f, -0.887640f, -0.884797f, -0.881921f, -0.879012f, - -0.876070f, -0.873095f, -0.870087f, -0.867046f, -0.863973f, -0.860867f, - -0.857729f, -0.854558f, -0.851355f, -0.848120f, -0.844854f, -0.841555f, - -0.838225f, -0.834863f, -0.831470f, -0.828045f, -0.824589f, -0.821103f, - -0.817585f, -0.814036f, -0.810457f, -0.806848f, -0.803208f, -0.799537f, - -0.795837f, -0.792107f, -0.788346f, -0.784557f, -0.780737f, -0.776888f, - -0.773010f, -0.769103f, -0.765167f, -0.761202f, -0.757209f, -0.753187f, - -0.749136f, -0.745058f, -0.740951f, -0.736817f, -0.732654f, -0.728464f, - -0.724247f, -0.720003f, -0.715731f, -0.711432f, -0.707107f, -0.702755f, - -0.698376f, -0.693971f, -0.689541f, -0.685084f, -0.680601f, -0.676093f, - -0.671559f, -0.667000f, -0.662416f, -0.657807f, -0.653173f, -0.648514f, - -0.643832f, -0.639124f, -0.634393f, -0.629638f, -0.624859f, -0.620057f, - -0.615232f, -0.610383f, -0.605511f, -0.600616f, -0.595699f, -0.590760f, - -0.585798f, -0.580814f, -0.575808f, -0.570781f, -0.565732f, -0.560662f, - -0.555570f, -0.550458f, -0.545325f, -0.540171f, -0.534998f, -0.529804f, - -0.524590f, -0.519356f, -0.514103f, -0.508830f, -0.503538f, -0.498228f, - -0.492898f, -0.487550f, -0.482184f, -0.476799f, -0.471397f, -0.465976f, - -0.460539f, -0.455084f, -0.449611f, -0.444122f, -0.438616f, -0.433094f, - -0.427555f, -0.422000f, -0.416430f, -0.410843f, -0.405241f, -0.399624f, - -0.393992f, -0.388345f, -0.382683f, -0.377007f, -0.371317f, -0.365613f, - -0.359895f, -0.354164f, -0.348419f, -0.342661f, -0.336890f, -0.331106f, - -0.325310f, -0.319502f, -0.313682f, -0.307850f, -0.302006f, -0.296151f, - -0.290285f, -0.284408f, -0.278520f, -0.272621f, -0.266713f, -0.260794f, - -0.254866f, -0.248928f, -0.242980f, -0.237024f, -0.231058f, -0.225084f, - -0.219101f, -0.213110f, -0.207111f, -0.201105f, -0.195090f, -0.189069f, - -0.183040f, -0.177004f, -0.170962f, -0.164913f, -0.158858f, -0.152797f, - -0.146730f, -0.140658f, -0.134581f, -0.128498f, -0.122411f, -0.116319f, - -0.110222f, -0.104122f, -0.098017f, -0.091909f, -0.085797f, -0.079682f, - -0.073565f, -0.067444f, -0.061321f, -0.055195f, -0.049068f, -0.042938f, - -0.036807f, -0.030675f, -0.024541f, -0.018407f, -0.012272f, -0.006136f, - -0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f, - 0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f, - 0.073565f, 0.079682f, 0.085797f, 0.091909f, 0.098017f, 0.104122f, - 0.110222f, 0.116319f, 0.122411f, 0.128498f, 0.134581f, 0.140658f, - 0.146730f, 0.152797f, 0.158858f, 0.164913f, 0.170962f, 0.177004f, - 0.183040f, 0.189069f, 0.195090f, 0.201105f, 0.207111f, 0.213110f, - 0.219101f, 0.225084f, 0.231058f, 0.237024f, 0.242980f, 0.248928f, - 0.254866f, 0.260794f, 0.266713f, 0.272621f, 0.278520f, 0.284408f, - 0.290285f, 0.296151f, 0.302006f, 0.307850f, 0.313682f, 0.319502f, - 0.325310f, 0.331106f, 0.336890f, 0.342661f, 0.348419f, 0.354164f, - 0.359895f, 0.365613f, 0.371317f, 0.377007f, 0.382683f, 0.388345f, - 0.393992f, 0.399624f, 0.405241f, 0.410843f, 0.416430f, 0.422000f, - 0.427555f, 0.433094f, 0.438616f, 0.444122f, 0.449611f, 0.455084f, - 0.460539f, 0.465976f, 0.471397f, 0.476799f, 0.482184f, 0.487550f, - 0.492898f, 0.498228f, 0.503538f, 0.508830f, 0.514103f, 0.519356f, - 0.524590f, 0.529804f, 0.534998f, 0.540171f, 0.545325f, 0.550458f, - 0.555570f, 0.560662f, 0.565732f, 0.570781f, 0.575808f, 0.580814f, - 0.585798f, 0.590760f, 0.595699f, 0.600616f, 0.605511f, 0.610383f, - 0.615232f, 0.620057f, 0.624859f, 0.629638f, 0.634393f, 0.639124f, - 0.643832f, 0.648514f, 0.653173f, 0.657807f, 0.662416f, 0.667000f, - 0.671559f, 0.676093f, 0.680601f, 0.685084f, 0.689541f, 0.693971f, - 0.698376f, 0.702755f, 0.707107f, 0.711432f, 0.715731f, 0.720003f, - 0.724247f, 0.728464f, 0.732654f, 0.736817f, 0.740951f, 0.745058f, - 0.749136f, 0.753187f, 0.757209f, 0.761202f, 0.765167f, 0.769103f, - 0.773010f, 0.776888f, 0.780737f, 0.784557f, 0.788346f, 0.792107f, - 0.795837f, 0.799537f, 0.803208f, 0.806848f, 0.810457f, 0.814036f, - 0.817585f, 0.821103f, 0.824589f, 0.828045f, 0.831470f, 0.834863f, - 0.838225f, 0.841555f, 0.844854f, 0.848120f, 0.851355f, 0.854558f, - 0.857729f, 0.860867f, 0.863973f, 0.867046f, 0.870087f, 0.873095f, - 0.876070f, 0.879012f, 0.881921f, 0.884797f, 0.887640f, 0.890449f, - 0.893224f, 0.895966f, 0.898674f, 0.901349f, 0.903989f, 0.906596f, - 0.909168f, 0.911706f, 0.914210f, 0.916679f, 0.919114f, 0.921514f, - 0.923880f, 0.926210f, 0.928506f, 0.930767f, 0.932993f, 0.935184f, - 0.937339f, 0.939459f, 0.941544f, 0.943593f, 0.945607f, 0.947586f, - 0.949528f, 0.951435f, 0.953306f, 0.955141f, 0.956940f, 0.958703f, - 0.960431f, 0.962121f, 0.963776f, 0.965394f, 0.966976f, 0.968522f, - 0.970031f, 0.971504f, 0.972940f, 0.974339f, 0.975702f, 0.977028f, - 0.978317f, 0.979570f, 0.980785f, 0.981964f, 0.983105f, 0.984210f, - 0.985278f, 0.986308f, 0.987301f, 0.988258f, 0.989177f, 0.990058f, - 0.990903f, 0.991710f, 0.992480f, 0.993212f, 0.993907f, 0.994565f, - 0.995185f, 0.995767f, 0.996313f, 0.996820f, 0.997290f, 0.997723f, - 0.998118f, 0.998476f, 0.998795f, 0.999078f, 0.999322f, 0.999529f, - 0.999699f, 0.999831f, 0.999925f, 0.999981f}; + #endif \ No newline at end of file diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index 7a7821e..449dc00 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -1,17 +1,17 @@ #include "Filter.h" #include "Can.h" #include "ang_sensor.h" +#include "config.h" #include #include #include -#include "config.h" + extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; -extern u32 system_runtime_us; +extern u32 motor_runtime_50us; EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f); EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f); - /*! \brief 中值滤波器函数 \param[in] values 待滤波的数据数组 @@ -126,10 +126,10 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); } \retval 当前角度估计值 (degree) */ float EncoderAngleKalman::getVelocity() const { - static u32 last_us100_counter = system_runtime_us; - float vel = (kalman.getState() - last_angle) * 1e4 * - (system_runtime_us - last_us100_counter); - last_us100_counter = system_runtime_us; + static u32 last_us50_counter = motor_runtime_50us; + float vel = (kalman.getState() - last_angle) * TIMER2_FREQUENCY * + (motor_runtime_50us - last_us50_counter); + last_us50_counter = motor_runtime_50us; return vel; } diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index f042e28..ab6bd24 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -59,6 +59,7 @@ project: - component: Device:GD32C10x_StdPeripherals:BKP - component: Device:GD32C10x_StdPeripherals:CRC - component: Device:GD32C10x_StdPeripherals:RTC + - component: CMSIS:DSP output: type: - elf @@ -76,6 +77,7 @@ project: - -D__MICROLIB - -Wno-register - -O3 # 添加优化级别 + - -DARM_MATH_CM4 # Cortex-M4核心定义 C: - -D__packed=__attribute__((packed)) ASM: @@ -86,3 +88,4 @@ project: - --entry=Reset_Handler - --remove # 移除未使用的代码段 - --diag_suppress=L6207,L6210,L6218,L6220,L6222,L6224,L6226,L6228,L6230,L6232,L6234,L6236,L6238,L6240,L6242,L6244,L6246,L6248,L6308,L6329,L6310,L6312,L6314,L6316,L6318,L6320,L6322,L6324,L6326,L6328,L6330,L6332,L6334,L6336,L6338,L6340,L6342,L6344,L6346,L6348,L6350,L6352,L6354,L6311,L6313,L6315,L6317,L6319,L6321,L6323,L6325,L6327,L6329,L6331,L6333,L6335,L6337,L6339,L6341,L6343,L6345,L6347,L6349,L6351,L6353 + # 移除了 __TARGET_FPU_VFP，因为它不是链接器参数 diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index fe19cad..8d6ebf8 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -2,6 +2,7 @@ #include "Can.h" #include "FOC.h" #include "Filter.h" +#include "arm_math.h" #include "config.h" #include "math.h" #include "systick.h" diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 7b350f2..eb765a6 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -444,6 +444,7 @@ u8 CAN_transmiter::init() { void CAN_transmiter::update(void) { target_angle_union.f = FOC.target_angle; target_velocity_union.f = FOC.target_velocity; + target_current_union.f = FOC.target_current; limit_velocity_union.f = FOC.limit_velocity; limit_voltage = FOC.limit_voltage * 1000; mcu_temp = sampler.get_mcu_temp() * 100; @@ -591,6 +592,7 @@ void CAN_transmiter::communication_check() { target_velocity_union.f = unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f; + FOC.target_current = FOC.target_velocity * 10.0f; } else if (receive_message.rx_efid == CANFD_RESET_ID) { u8 first_byte = receive_message.rx_data[0]; u8 eigth_byte = receive_message.rx_data[7]; diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 6d5282c..eeaf828 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -87,9 +87,10 @@ class CAN_transmiter { u8 Target_Control_Mode = 0; // 目标控制模式 u8 EEPROMStatus = 0; // EEPROM状态码 float_union_t target_velocity_union = {0}; // 目标速度 - float_union_t u_current_union = {0}; // U相电流 - float_union_t w_current_union = {0}; // W相电流 - float_union_t v_current_union = {0}; // V相电流 + float_union_t target_current_union = {0}; + float_union_t u_current_union = {0}; // U相电流 + float_union_t w_current_union = {0}; // W相电流 + float_union_t v_current_union = {0}; // V相电流 can_trasnmit_message_struct transmit_message; // 发送消息结构体 u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态 diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 1b41c42..18ca60e 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -12,7 +12,6 @@ #include #include - volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1 extern u16 adc_value[7]; extern FOC_Controlroller FOC; @@ -96,66 +95,75 @@ void TIMER2_IRQHandler(void) { if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) { // 清除更新中断标志 timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); - // 每100us执行一次的操作 - scheduler.taskFlags |= (1 << TASK_ANGLE_UPDATE) | - (1 << TASK_VELOCITY_UPDATE) | - (1 << TASK_MOTOR_CONTROL); - switch (total_us100_counter) { // 每1ms执行的代码，负载均衡 - case 0: { - scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE); - break; - } - case 1: { - scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE); - break; - } - case 2: { - scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE); - break; - } - case 3: { - break; - } - case 9: { - scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT); - break; - } - } + // 每50us执行一次的操作 - 电机控制 + scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL); + scheduler.taskFlags |= + (1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE); - total_us100_counter++; - if (total_us100_counter > 9) { - total_us100_counter = 0; - // 可以添加每1ms执行的代码，无负载均衡 - static u8 ms1_counter = 0; + // 使用标志位控制，每100us执行一次其他任务 + static bool isEvenCall = false; + isEvenCall = !isEvenCall; - switch (ms1_counter) { // 每10ms执行的代码，负载均衡 + if (isEvenCall) { + // 每100us执行一次的操作 - 保持原有频率 + + switch (total_us100_counter) { // 每1ms执行的代码，负载均衡 case 0: { - scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL); + scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE); break; } case 1: { - scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK); + scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE); break; } case 2: { + scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE); + break; + } + case 3: { + break; + } + case 9: { + scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT); break; } } - ms1_counter++; - if (ms1_counter > 9) { - ms1_counter = 0; - // 可以添加每10ms执行的代码，无负载均衡 - static u8 ms10_counter = 0; - ms10_counter++; - if (ms10_counter > 9) { - ms10_counter = 0; - // 可以添加每100ms执行的代码 - static u8 sec_counter = 0; - sec_counter++; - if (sec_counter > 9) { - sec_counter = 0; - // 可以添加每1s执行的代码 + + total_us100_counter++; + if (total_us100_counter > 9) { + total_us100_counter = 0; + // 可以添加每1ms执行的代码，无负载均衡 + static u8 ms1_counter = 0; + + switch (ms1_counter) { // 每10ms执行的代码，负载均衡 + case 0: { + scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL); + break; + } + case 1: { + scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK); + break; + } + case 2: { + break; + } + } + ms1_counter++; + if (ms1_counter > 9) { + ms1_counter = 0; + // 可以添加每10ms执行的代码，无负载均衡 + static u8 ms10_counter = 0; + ms10_counter++; + if (ms10_counter > 9) { + ms10_counter = 0; + // 可以添加每100ms执行的代码 + static u8 sec_counter = 0; + sec_counter++; + if (sec_counter > 9) { + sec_counter = 0; + // 可以添加每1s执行的代码 + } } } } diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 520db62..c1d9d6c 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -253,7 +253,7 @@ float angle_sensor::update(void) { float delta = current_angle_deg - m_lastAngle; // 处理角度绕回情况（0度和360度之间的跳变） - // 如果变化超过180度，说明发生了绕回 + // 如果变化超过300度，说明发生了绕回 if (delta > ANGLE_WRAP_THRESHOLD) { // 从接近360度绕回到接近0度 delta -= ANGLE_WRAP_VALUE; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index d844ece..12356ac 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -9,10 +9,11 @@ #include "health.h" #include "initial.h" #include "motor.h" -#include "sys/config.h" #include "sys/flash.h" #include "sys/initial.h" #include "systick.h" +#include +#include #include #include extern CAN_transmiter CAN0_transmiter; // CAN初始化 @@ -25,8 +26,8 @@ extern FOC_Controlroller FOC; // FOC控制 extern FlashEEPROM flashEEP; // EEPROM类 TaskScheduler scheduler = {0}; -volatile u32 system_runtime_us = 0; // 电机驱动计算轮次 -volatile u32 cnt_test = 0; // 负载检测 +volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次 +volatile u32 cnt_test = 0; // 负载检测 int main(void) { u8 init_status = hardware_init(); @@ -35,9 +36,8 @@ int main(void) { } OTA_initial(); // OTA初始化,A/B分区处理 - watchdog_init(50); // 看门狗初始化 angsens_MG_check(); // 角度传感器磁感应强度测试 - + watchdog_init(50); // 看门狗初始化 while (1) { __disable_irq(); u32 currentTasks = @@ -60,7 +60,6 @@ u8 hardware_init(void) { RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠 printf("continue sleep\r\n"); watchdog_init(5000); // 看门狗初始化 - delay_1ms(3000); prepare_deep_sleep(false); } User_timer_init(); // 任务Timer初始化 @@ -76,6 +75,7 @@ u8 hardware_init(void) { } void do_task(u32 currentTasks) { + cnt_test++; // 按照优先级和重要性处理任务 if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 sampler.update_current(); @@ -88,7 +88,7 @@ void do_task(u32 currentTasks) { FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity()); } if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) - system_runtime_us++; + motor_runtime_50us++; // 函数指针数组，包含各种控制模式对应的函数 static FOCFunctionPtr control_functions[] = { @@ -122,8 +122,8 @@ void do_task(u32 currentTasks) { if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 power_manage(); } - if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 - // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 + if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 + CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 cnt_test = 0; CAN0_transmiter.can0_send_flag = SET; CAN0_transmiter.communication_check(); diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index cc4d311..c0c66b4 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -3,7 +3,7 @@ #include // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 -// #define ARM_MATH_CM4 +#define ARM_MATH_CM4 #define EEP_DEBUG 1 #define OTA_DEBUG 1 @@ -39,9 +39,9 @@ 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 ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * PI; -constexpr float DEG2RAD = 1.0f / 360.0f * 2 * PI; -constexpr float RAD2DEG = 360.0f / 2 * PI; +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 float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3) constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值 constexpr u8 DEFAULT_VALUE = 0; @@ -89,7 +89,8 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f; // TIMER2-任务调度 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz -constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us +constexpr int TIMER2_PERIOD = 49; // 1MHz / 50 = 50us +constexpr float TIMER2_FREQUENCY = 1 / 5e-5; // TIMER1-PWM constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 0c67474..77a02dc 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -1,8 +1,8 @@ #ifndef __FLASH_H #define __FLASH_H +#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用 #include -#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用 // 修改后 - 强制4字节对齐 #define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 @@ -71,7 +71,7 @@ class FlashEEPROM { }; void flash_eeprom_example_main(void); -u8 OTA_SET(u32 addr, u32 data) ; +u8 OTA_SET(u32 addr, u32 data); fmc_state_enum move_app_data(void); #endif /* __FLASH_H */ \ No newline at end of file diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index f8d7d86..500fca6 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,4 +1,5 @@ #include "initial.h" +#include "Can.h" #include "config.h" #include "flash.h" #include "gd32c10x.h" @@ -7,7 +8,6 @@ #include "gd32c10x_rcu.h" #include #include -#include "can.h" #include extern CAN_transmiter CAN0_transmiter; // CAN初始化 @@ -97,9 +97,13 @@ u16 get_reset_cause(void) { } void OTA_initial(void) { + // 确认当前APP可用 OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL); OTA_SET(n_APP_LATEST_EN_ADDR, OTA_AVAL); + u8 transmit_data[2] = {0x0A, 0xAC}; + // 确认APP可用 + CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data); volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; u32 back_aval = *flash_ptr; @@ -108,9 +112,14 @@ void OTA_initial(void) { // 如果BACK不可用 if (back_aval == OTA_NOT_AVAL) { + std::printf("notaval\r\n"); move_app_data(); } else if (back_aval == OTA_AVAL && back_check == OTA_NOT_AVAL) { // BACK已刷写，但未检查 + std::printf("aval\r\n"); move_app_data(); } + else { + printf("check\r\n"); + } } diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index b7f6c4a..82ff49b 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -34,9 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "systick.h" #include "Can.h" +#include "core_cmFunc.h" #include "flash.h" #include "gd32c10x.h" #include "gd32c10x_exti.h" +#include "initial.h" #include "motor.h" #include @@ -157,6 +159,7 @@ void prepare_deep_sleep(bool save_data) { // 确保中断标志已被清除 exti_interrupt_flag_clear(EXTI_8); + watchdog_init(5000); // 看门狗初始化 // 进入深度睡眠 pmu_to_deepsleepmode(PMU_LDO_LOWPOWER, WFI_CMD); } @@ -177,9 +180,3 @@ void EXTI5_9_IRQHandler(void) { system_soft_reset(); } } - - -void OTA_initial(){ - - -} \ No newline at end of file From 9f84c85a28c2715cebf8345a88c19eba4b5a84a3 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Mon, 12 Jan 2026 01:03:43 +0800 Subject: [PATCH 13/16] =?UTF-8?q?feat(can):=20CAN=E4=BC=91=E7=9C=A0?= =?UTF-8?q?=E5=94=A4=E9=86=92=E7=A7=BB=E5=8A=A8=E5=88=B0BOOT=E4=BB=A5?= =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E4=BB=A5=E6=94=AF=E6=8C=8140k=E7=A1=AC?= =?UTF-8?q?=E4=BB=B6=E7=9C=8B=E9=97=A8=E7=8B=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 更新深度睡眠实现，在跳转到bootloader前记录睡眠状态到Flash - 添加gd32c10x_fmc.h头文件以支持Flash内存控制功能 - 将prepare_deep_sleep重命名为app_prepare_deep_sleep以提高命名清晰度 - 在CAN通信检测中添加应用层休眠准备功能 - 在接收OTA启动ID时解锁FMC、设置APP为非激活状态并锁定FMC - 移除main.cpp中的看门狗复位休眠逻辑 - 在config.h中添加睡眠配置相关的常量定义 - 改进OTA_SET函数的Flash写入逻辑，优化数据更新流程 - 在OTA初始化和应用移动过程中添加FMC解锁锁定操作 --- Robot_V2/device/Can.cpp | 6 ++- Robot_V2/main.cpp | 6 --- Robot_V2/sys/config.h | 8 +++- Robot_V2/sys/flash.cpp | 96 +++++++++++++++++++++---------------- Robot_V2/sys/flash.h | 1 + Robot_V2/sys/initial.cpp | 11 +++-- Robot_V2/sys/systick.cpp | 100 +++++++++++++++++++++------------------ Robot_V2/sys/systick.h | 2 +- 8 files changed, 132 insertions(+), 98 deletions(-) diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index eb765a6..598fc8e 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -8,6 +8,7 @@ #include "gd32c10x.h" #include "gd32c10x_can.h" #include "gd32c10x_dma.h" +#include "gd32c10x_fmc.h" #include "gd32c10x_gpio.h" #include "motor.h" #include "systick.h" @@ -611,10 +612,13 @@ void CAN_transmiter::communication_check() { Vel_kalman.init(0); // 角度滤波 } if (eigth_byte & 0x80 >> 7) { - prepare_deep_sleep(true); + app_prepare_deep_sleep(true); // CAN_Sleep(); } } else if (receive_message.rx_efid == CANFD_BOOT_ID) { + fmc_unlock(); + OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活 + fmc_lock(); jumpToBOOT(); } else if (receive_message.rx_efid == 0x00000102) { volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 12356ac..8b98faf 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -56,12 +56,6 @@ u8 hardware_init(void) { unused_pin_init(); // 未使用引脚初始化 USART0_init(); // 调试串口初始化 motor_init(); // 电机驱动初始化 - if (get_reset_cause() == - RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠 - printf("continue sleep\r\n"); - watchdog_init(5000); // 看门狗初始化 - prepare_deep_sleep(false); - } User_timer_init(); // 任务Timer初始化 u8 init_status = 0; init_status += KTH7823.init(); // 角度传感器初始化 diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index c0c66b4..d0d1fca 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -5,8 +5,9 @@ // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 #define ARM_MATH_CM4 -#define EEP_DEBUG 1 +#define EEP_DEBUG 0 #define OTA_DEBUG 1 +#define SLEEP_DEBUG 1 // Flash分区配置 constexpr u32 BOOTLOADER_SIZE = 0x4000; // 16KB @@ -15,6 +16,11 @@ constexpr u32 APPLATEST_START_ADDR = 0x08011000; // APP起始地址 constexpr u32 APPBACK_START_ADDR = 0x08005000; // APPBACK起始地址 constexpr u32 APP_MAX_SIZE = 0xC000; // 48KB constexpr u32 FMC_PAGE_SIZE = 0x400; // 1KB +constexpr u32 SLEEP_CONFIG_START = 0x0801D400; // 睡眠配置起始地址 +constexpr u32 SLEEP_CONFIG_END = 0x0801D800; // 睡眠配置结束地址 +constexpr u32 FLASH_EMPTY = 0xFFFFFFFF; // 空数据 +constexpr u32 SLEEP_ENABLE = 0x11111111; // 睡眠使能 +constexpr u32 SLEEP_DISABLE = 0x00000000; // 睡眠禁止 // BOOT 相关 constexpr u32 OTA_CONFIG_NUM = 6; diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 5ac1fde..36720a2 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -4,6 +4,7 @@ #include "config.h" #include "gd32c10x.h" // GD32主头文件，确保外设函数声明可用 #include "gd32c10x_crc.h" +#include "gd32c10x_fmc.h" #include #include #include @@ -13,7 +14,7 @@ extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; extern FOC_Controlroller FOC; // FOC控制 -inline fmc_state_enum write_ADDRESS(u32 address, u32 data) { +fmc_state_enum write_ADDRESS(u32 address, u32 data) { fmc_unlock(); fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); @@ -528,32 +529,40 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { } } +/** + * @brief 设置OTA配置参数到Flash + * + * 此函数用于更新存储在Flash中的OTA相关标志位（如APP可用、启用、校验等）。 + * GD32的Flash特性是：只能将1写为0，不能将0写为1。若要将0变为1，必须先擦除整个页面。 + * 因此，本函数通过读取同一页内的所有参数，擦除该页，然后重新写入有效的参数来实现“写1”操作。 + * + * @param addr: 要设置的Flash地址 + * @param data: 要写入的数据 (通常为OTA_AVAL=0x00000000 或 OTA_NOT_AVAL=0xFFFFFFFF) + * @retval FLASH_EEPROM_OK (0) 表示成功, 其他值表示失败 + */ u8 OTA_SET(u32 addr, u32 data) { fmc_state_enum fmc_status; - u32 temp_val[OTA_CONFIG_NUM]; + u32 temp_val[OTA_CONFIG_NUM]; // 临时数组，用于存储当前所有配置参数的值 volatile u32 *flash_ptr = (volatile u32 *)addr; + + // 1. 检查目标地址的值是否已与期望值相同，如果相同则无需操作 temp_val[0] = *flash_ptr; - if (temp_val[0] == data) { // 如果相同 -#if EEP_DEBUG - printf("OTA_SET%x same%X,skip\r\n", addr, data); -#endif - return FLASH_EEPROM_OK; // 成功 + if (temp_val[0] == data) { + return FLASH_EEPROM_OK; // 值已正确，直接返回成功 } - fmc_unlock(); - // 如果不同，且要写0，直接写 + // 2. 尝试直接写入数据。这适用于将1修改为0的情况（因为Flash允许1->0） if (data == OTA_AVAL) { - fmc_status = write_ADDRESS(addr, OTA_AVAL); - fmc_lock(); + fmc_status = write_ADDRESS(addr, data); + // 如果写入成功，则返回成功 if (fmc_status == FMC_READY) { -#if EEP_DEBUG - printf("OTA_SET%x 0\r\n", addr); -#endif - return FLASH_EEPROM_OK; // 成功可以直接退出 + return FLASH_EEPROM_OK; } + // 如果写入失败（例如地址已被写为0），则继续执行后续的擦除-重写流程 } - // 如果不同或写入出错，要写1，需要重置，然后再写 + // 3. 对于需要将0改回1的操作，必须先擦除整个Flash页 + // 首先读取同一页内所有相关配置参数的当前状态 flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; temp_val[0] = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; @@ -566,44 +575,50 @@ u8 OTA_SET(u32 addr, u32 data) { temp_val[4] = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; temp_val[5] = *flash_ptr; + #if EEP_DEBUG - printf("OTA_SET\r\n"); - printf("0x%x_0x%x_0x%x_0x%x\r\n", temp_val[0], temp_val[1], temp_val[2], - temp_val[3]); + printf("OTA_SET - Start updating configuration\r\n"); + // 修复：完整输出所有6个配置项的值，便于调试 + printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, " + "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n", + temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], temp_val[5]); #endif - fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); // 擦除页 + + // 4. 擦除包含所有配置参数的Flash页 + fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); if (fmc_status != FMC_READY) { #if EEP_DEBUG - printf("OTA_SET erase error\r\n"); + printf("OTA_SET - Page erase failed! Status: %d\r\n", fmc_status); #endif - return FLASH_EEPROM_ERASE_ERROR; // 擦除错误退出 + return FLASH_EEPROM_ERASE_ERROR; } + + // 5. 重新写入所有之前为有效(OTA_AVAL)的配置项 + // 注意：不包括当前正在设置的地址，因为它将在最后单独处理 for (int i = 0; i < OTA_CONFIG_NUM; i++) { - if (temp_val[i] == OTA_AVAL and - n_APP_LATEST_AVAL_ADDR + 4 * i != addr) { - fmc_status = write_ADDRESS(n_APP_LATEST_AVAL_ADDR + 4 * i, - OTA_AVAL); // 把需要写入0的写入0 + u32 current_addr = n_APP_LATEST_AVAL_ADDR + 4 * i; + if (temp_val[i] == OTA_AVAL && current_addr != addr) { + fmc_status = write_ADDRESS(current_addr, OTA_AVAL); if (fmc_status != FMC_READY) { - fmc_lock(); #if EEP_DEBUG - printf("OTA_SET write error\r\n"); + printf("OTA_SET - Write backup value failed at 0x%08X!\r\n", current_addr); #endif - return FLASH_EEPROM_WRITE_ERROR; // 写入错误退出 + return FLASH_EEPROM_WRITE_ERROR; } } } - flash_ptr = (volatile u32 *)addr; - temp_val[0] = *flash_ptr; - if (temp_val[0] != data) { + + // 6. 最后，写入本次调用的目标数据 + // 即使addr为0（理论上无效），也检查一下以避免潜在问题 + if (addr != 0) { + fmc_status = write_ADDRESS(addr, data); + if (fmc_status != FMC_READY) { #if EEP_DEBUG - printf("OTA_SET write error\r\n"); + printf("OTA_SET - Final write to target address 0x%08X failed!\r\n", addr); #endif - return FLASH_EEPROM_WRITE_ERROR; + return FLASH_EEPROM_WRITE_ERROR; + } } - fmc_lock(); -#if EEP_DEBUG - printf("OTA_SET%x ok\r\n", addr); -#endif return FLASH_EEPROM_OK; } @@ -613,7 +628,6 @@ u8 OTA_SET(u32 addr, u32 data) { * @retval 无 */ void jumpToBOOT() { - OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活 // 1. 关闭所有中断 __disable_irq(); for (int i = 0; i < 8; i++) { @@ -627,7 +641,7 @@ void jumpToBOOT() { // 4. 关闭一些关键外设时钟 rcu_periph_clock_disable(RCU_GPIOA); - rcu_periph_clock_disable(RCU_GPIOB); + // rcu_periph_clock_disable(RCU_GPIOB); rcu_periph_clock_disable(RCU_GPIOC); rcu_periph_clock_disable(RCU_USART0); rcu_periph_clock_disable(RCU_CAN0); @@ -741,8 +755,10 @@ fmc_state_enum move_app_data() { return (fmc_state_enum)0xFF; // CRC校验失败 } // 设置备份应用可用标志 + fmc_unlock(); OTA_SET(n_APP_BACKUP_AVAL_ADDR, OTA_AVAL); OTA_SET(n_APP_BACKUP_CHECK_ADDR, OTA_AVAL); + fmc_lock(); return fmc_status; } diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 77a02dc..4958a8f 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -73,5 +73,6 @@ class FlashEEPROM { void flash_eeprom_example_main(void); u8 OTA_SET(u32 addr, u32 data); fmc_state_enum move_app_data(void); +fmc_state_enum write_ADDRESS(u32 address, u32 data); #endif /* __FLASH_H */ \ No newline at end of file diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index 500fca6..e8d62ac 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -99,8 +99,10 @@ u16 get_reset_cause(void) { void OTA_initial(void) { // 确认当前APP可用 + fmc_unlock(); OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL); OTA_SET(n_APP_LATEST_EN_ADDR, OTA_AVAL); + fmc_lock(); u8 transmit_data[2] = {0x0A, 0xAC}; // 确认APP可用 CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data); @@ -112,14 +114,17 @@ void OTA_initial(void) { // 如果BACK不可用 if (back_aval == OTA_NOT_AVAL) { +#if OTA_DEBUG std::printf("notaval\r\n"); +#endif move_app_data(); } else if (back_aval == OTA_AVAL && back_check == OTA_NOT_AVAL) { // BACK已刷写，但未检查 +#if OTA_DEBUG std::printf("aval\r\n"); +#endif move_app_data(); - } - else { + } else { printf("check\r\n"); } -} +} \ No newline at end of file diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index 82ff49b..054b2ad 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -34,10 +34,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "systick.h" #include "Can.h" +#include "config.h" #include "core_cmFunc.h" #include "flash.h" #include "gd32c10x.h" #include "gd32c10x_exti.h" +#include "gd32c10x_fmc.h" #include "initial.h" #include "motor.h" #include @@ -125,58 +127,64 @@ void system_soft_reset(void) { \param[out] none \retval none */ -void prepare_deep_sleep(bool save_data) { +void app_prepare_deep_sleep(bool save_data) { if (save_data) { flashEEP.save_system_config(true); } // 将SIT1044设置为低功耗待机模式 // CAN0_transmiter.CAN_Sleep(); motor_sleep(); + // 在睡眠前调整看门狗超时时间为最大值，使其在唤醒前不会触发 + fwdgt_write_enable(); + fwdgt_config(0xFFF, FWDGT_PSC_DIV256); // 设置最大超时时间 + fwdgt_write_disable(); - // 关闭不必要的外设时钟 - rcu_periph_clock_disable(RCU_TIMER0); // 关闭定时器时钟 - rcu_periph_clock_disable(RCU_TIMER1); - rcu_periph_clock_disable(RCU_SPI0); // 关闭SPI时钟 - rcu_periph_clock_disable(RCU_I2C0); // 关闭I2C时钟 - rcu_periph_clock_disable(RCU_ADC0); // 关闭ADC时钟 - // 注意不要关闭GPIOB和AF的时钟，因为需要它们来处理唤醒中断 - - // 设置深睡眠模式下的电压以进一步降低功耗 - rcu_deepsleep_voltage_set(RCU_DEEPSLEEP_V_0_8); // 设置最低电压 - - // 配置PB8为输入模式，用于外部中断 - rcu_periph_clock_enable(RCU_GPIOB); - rcu_periph_clock_enable(RCU_AF); - gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_8); - - // 配置EXTI中断（连接到PB8引脚） - gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOB, - GPIO_PIN_SOURCE_8); // 配置GPIOB的PIN8作为EXTI8的源 - exti_init(EXTI_8, EXTI_INTERRUPT, EXTI_TRIG_RISING); // 边沿触发 - exti_interrupt_enable(EXTI_8); - nvic_irq_enable(EXTI5_9_IRQn, 0, 0); // - - // 确保中断标志已被清除 - exti_interrupt_flag_clear(EXTI_8); - - watchdog_init(5000); // 看门狗初始化 - // 进入深度睡眠 - pmu_to_deepsleepmode(PMU_LDO_LOWPOWER, WFI_CMD); -} - -/** - * @brief EXTI5_9中断处理函数 - * @param 无 - * @retval 无 - */ -void EXTI5_9_IRQHandler(void) { - if (exti_interrupt_flag_get(EXTI_8) != RESET) { - // 清除中断标志 - exti_interrupt_flag_clear(EXTI_8); - // 禁用EXTI线8的中断，防止重复触发 - exti_interrupt_disable(EXTI_8); - - printf("MCU wake up\r\n"); - system_soft_reset(); + __disable_irq(); + // 遍历最后一个数字，后一个地址写+1 + for (u32 address = SLEEP_CONFIG_END - SIZE_U32; + address >= SLEEP_CONFIG_START; address -= SIZE_U32) { // 遍历睡眠存储区 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 sleep_enable = *flash_ptr; + if (sleep_enable == FLASH_EMPTY and + address > SLEEP_CONFIG_START) { // 空数据且未到头跳过 + continue; + } else { // 有数据的话读取 +#if SLEEP_DEBUG + printf("app sleep found %08x at %08x\r\n", sleep_enable, address); +#endif + if (address == SLEEP_CONFIG_END - SIZE_U32) { + fmc_unlock(); + fmc_state_enum fmc_status = + fmc_page_erase(SLEEP_CONFIG_START); // 擦除已满的页 + if (fmc_status != FMC_READY) { // 擦除失败报错 + fmc_lock(); + flashEEP.EEPROMStatus = FLASH_EEPROM_ERASE_ERROR; + return; + } + address = SLEEP_CONFIG_START - SIZE_U32; // 重新开始写入 + } + address += SIZE_U32; +#if SLEEP_DEBUG + printf("app sleep prepare write %08x at %08x\r\n", SLEEP_ENABLE, + address); +#endif + fmc_state_enum fmc_status = + write_ADDRESS(address, SLEEP_ENABLE); // 写入数据 +#if SLEEP_DEBUG + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 sleep_check = *flash_ptr; + printf("app sleep wrote %08x at %08x\r\n", sleep_check, address); +#endif + if (fmc_status != FMC_READY) { // 写入失败报错 + flashEEP.EEPROMStatus = FLASH_EEPROM_WRITE_ERROR; +#if SLEEP_DEBUG + printf("app wrote %08x at %08x ERROR\r\n", SLEEP_ENABLE, + address); +#endif + return; + } + break; + } } + jumpToBOOT(); // 跳转到bootloader正式休眠，进入低功耗待机模式 } diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index 5cf51e5..99dd74c 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -55,7 +55,7 @@ uint8_t get_cpu_usage(void); /* 系统软复位 */ void system_soft_reset(void); /* 准备进入低功耗模式 */ -void prepare_deep_sleep(bool save_data); +void app_prepare_deep_sleep(bool save_data); /* 中断唤醒 */ void EXTI5_9_IRQHandler(void); From c0b5f6902b721ed79247caf063ff4b021cc81129 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 20 Jan 2026 01:28:52 +0800 Subject: [PATCH 14/16] =?UTF-8?q?docs(config):=20=E6=96=B0=E5=A2=9E?= =?UTF-8?q?=E8=BD=AF=E4=BB=B6=E5=92=8C=E7=A1=AC=E4=BB=B6=E7=89=88=E6=9C=AC?= =?UTF-8?q?=E5=8F=B7=E5=AE=9A=E4=B9=89=20-=20=E5=AE=9A=E4=B9=89SOFTWARE=5F?= =?UTF-8?q?VERSION=2026.1.19.1=20-=20=E5=AE=9A=E4=B9=89HARDWARE=5FVERSION?= =?UTF-8?q?=2025.12.01.0=20-=20=E5=AE=9A=E4=B9=89BOOT=5FVERSION=2026.1.12.?= =?UTF-8?q?0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(motor-control): 实现FOC控制算法速度环优化 refactor(can-protocol): 升级CANFD通信协议支持版本管理 - 实现版本号管理和CANFD_VERSION_ID协议 - 扩展CMD_ID消息解析支持PID参数动态配置 - 添加版本管理器实例并实现版本信息发送功能 - 优化CANFD异步发送数据长度判断逻辑 fix(angle-sensor): 修正角度传感器方向和偏移校准 - 添加ANGLESENS_OFFSET补偿参数 - 反转角度传感器读数方向(DIR = -1.0f) - 在角度计算中应用负向因子和偏移量 chore(memory-layout): 调整Flash存储布局和版本配置 - 扩展BOOTLOADER_SIZE从16KB到24KB - 更新APPLATEST_START_ADDR和APPBACK_START_ADDR偏移 - 重新分配OTA配置参数存储区域 - 增加EEP_DEBUG调试开关 refactor(pid-controller): 增强PID控制器功能接口 - 添加set_P_percent方法支持百分比参数设置 - 格式化PID计算代码提升可读性 - 保留积分和微分环节的约束保护机制 --- Robot_V2/Algorithm/FOC.cpp | 22 +-- Robot_V2/Algorithm/FOC.h | 24 ++- Robot_V2/Algorithm/pid.cpp | 30 ++-- Robot_V2/Algorithm/pid.h | 1 + .../Device/GD32C103CB/regions_GD32C103CB.h | 2 +- .../RTE/Device/GD32C103CB/system_gd32c10x.c | 2 +- Robot_V2/device/Can.cpp | 49 ++++-- Robot_V2/device/ang_sensor.cpp | 3 +- Robot_V2/main.cpp | 6 +- Robot_V2/sys/config.h | 57 +++++-- Robot_V2/sys/flash.cpp | 99 +++++++---- Robot_V2/sys/initial.cpp | 157 +++++++++++++++++- Robot_V2/sys/initial.h | 50 +++++- 13 files changed, 393 insertions(+), 109 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index a093f79..7fa485c 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -9,12 +9,14 @@ #include #include #include +#include "Filter.h" #include FOC_Controlroller FOC; // FOC控制初始化 extern sampler sampler; extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; +extern EncoderVelocityKalman Vel_kalman; // 速度滤波 // 初始变量及函数定义 #define _constrain(amt, low, high) \ @@ -54,7 +56,7 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b, I_q = I_beta * cos_el - I_alpha * sin_el; // arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el); - return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA + return I_q * 1000.0f - KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA } /** @@ -156,17 +158,17 @@ void FOC_Controlroller::update_power(void) { */ float FOC_Controlroller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 - // 预计算常量 1e-4 = 100微秒控制周期 - shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * - 1.0 / TIMER2_FREQUENCY); + shaft_angle = _normalizeAngle(shaft_angle + + target_velocity * ANGLE_FACTOR * + REDUCTION_RATIO * 1.0 / TIMER2_FREQUENCY); // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = voltage_power_supply / 2.0f; setPhaseVoltage(Uq, 0.0f); - // CAN0_transmiter.testfloat_union.f = - cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), - sampler.get_current('W')); + CAN0_transmiter.testfloat_union.f = shaft_angle; + // cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + // sampler.get_current('W')); return limit_voltage; } @@ -221,8 +223,8 @@ float FOC_Controlroller::angle_control_with_velocity_limit() { _constrain(target_velocity, -limit_velocity, limit_velocity); // 使用速度PID控制器计算最终输出电压 - float Uq = vel_inVelAngloop(target_velocity - rt_velocity); - // CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity; + float Uq = vel_inVelAngloop(rt_velocity - target_velocity); + CAN0_transmiter.testfloat_union.f = shaft_angle; // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); @@ -241,7 +243,7 @@ float FOC_Controlroller::angle_control_with_velocity_limit() { */ float FOC_Controlroller::velocity_control() { // PID 控制计算 - float Uq = vel_loop((target_velocity - rt_velocity) * 25); + float Uq = vel_loop((rt_velocity - target_velocity) * 25); // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index e882a55..7e83c68 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -37,24 +37,23 @@ class FOC_Controlroller { void resume(void); void drv_stop(void); float drv_hold(void); + PIDController vel_loop = PIDController{ + vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V + PIDController angle_loop = + PIDController{angle_loop_p * 100, 100.0f, 0.2f, 10000000, + voltage_power_supply}; // 12V + PIDController current_loop = PIDController{ + current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V + PIDController vel_inVelAngloop = PIDController{ + vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply}; + PIDController ang_inVelAngloop = + PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999}; protected: float shaft_angle = 0; float Ualpha, Ubeta = 0; float zero_electric_angle = 0; float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - - PIDController vel_loop = - PIDController{0.01f, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V - PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000, - voltage_power_supply}; // 12V - PIDController current_loop = - PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V - PIDController vel_inVelAngloop = - PIDController{0.12f, 3.0f, 5e-4, 1000000, voltage_power_supply}; - PIDController ang_inVelAngloop = - PIDController{100.0f, 0.0f, 5e-4, 1000000, 999}; - void setPhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); }; @@ -65,5 +64,4 @@ typedef float (FOC_Controlroller::*FOCFunctionPtr)(void); inline float _normalizeAngle(float angle); inline float _electricalAngle(); - #endif \ No newline at end of file diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 61595c1..e650a96 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -18,25 +18,27 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) float PIDController::operator()(float error) { // P环 float proportional = P * error; - + // Tustin 散点积分（I环） // 添加积分限幅，防止积分饱和 - float integral = integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev); + float integral = + integral_prev + I * TIME_SLICE * 0.5f * (error + error_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 + fmaxf(-max_delta, fminf(max_delta, output - output_prev)); - } + // 简化的斜坡限制（单行逻辑） + if (output_ramp > 0) { + float max_delta = output_ramp * TIME_SLICE; + output = output_prev + + fmaxf(-max_delta, fminf(max_delta, output - output_prev)); + } // 保存值（为了下一次循环） integral_prev = integral; output_prev = output; @@ -57,4 +59,12 @@ void PIDController::set_PID(float P, float I, float D) { this->P = P; this->I = I; this->D = D; -} \ No newline at end of file +} + +/** + * @brief 设置P参数 + * + * @param P: P参数 + * @return void + */ +void PIDController::set_P_percent(float pp) { this->P = pp; } \ No newline at end of file diff --git a/Robot_V2/Algorithm/pid.h b/Robot_V2/Algorithm/pid.h index 695b51d..d8fb2f1 100644 --- a/Robot_V2/Algorithm/pid.h +++ b/Robot_V2/Algorithm/pid.h @@ -9,6 +9,7 @@ public: float operator() (float error); void set_PID(float P, float I, float D); + void set_P_percent(float pp); float P; //!< 比例增益(P环增益) float I; //!< 积分增益（I环增益） diff --git a/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h b/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h index 13fc0b9..54b2c27 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h +++ b/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h @@ -14,7 +14,7 @@ // Base address <0x0-0xFFFFFFFF:8> // Defines base address of memory region. Default: 0x08000000 // Contains Startup and Vector Table -#define __ROM0_BASE 0x08011000 +#define __ROM0_BASE 0x08012000 // Region size [bytes] <0x0-0xFFFFFFFF:8> // Defines size of memory region. Default: 0x00020000 #define __ROM0_SIZE 0x0000C000 diff --git a/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c b/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c index 66e2e72..9142dca 100644 --- a/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c +++ b/Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c @@ -42,7 +42,7 @@ #define __SYS_OSC_CLK (__IRC8M) /* main oscillator frequency */ /* Vector Table base offset */ -#define APP_BASE ((uint32_t)0x08011000U) /*!< main FLASH base address */ +#define APP_BASE ((uint32_t)0x08012000U) /*!< main FLASH base address */ #define VECT_TAB_OFFSET (uint32_t)APP_BASE-FLASH_BASE /* This value must be a multiple of 0x200. */ /* select a system clock by uncommenting the following line */ diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 598fc8e..693538b 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -10,6 +10,7 @@ #include "gd32c10x_dma.h" #include "gd32c10x_fmc.h" #include "gd32c10x_gpio.h" +#include "initial.h" #include "motor.h" #include "systick.h" #include @@ -29,6 +30,7 @@ extern angle_sensor KTH7823; extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern FlashEEPROM flashEEP; +extern VersionManager Version; // 声明外部版本管理器实例 // 添加DMA相关变量 dma_parameter_struct dma_rx_parameter; @@ -574,48 +576,65 @@ void CAN_transmiter::communication_check() { // printf(" %02x", receive_message.rx_data[i]); // } if (receive_message.rx_efid == CANFD_CMD_ID) { + // 控制模式 FOC.control_mode = receive_message.rx_data[0]; - if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 != - FOC.limit_voltage) { - FOC.limit_voltage = - float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / - 1000; - } - if (unpack_bytes_to_float(receive_message.rx_data, 3).f != - FOC.limit_velocity) { - FOC.limit_velocity = - unpack_bytes_to_float(receive_message.rx_data, 3).f; - } + // 目标角度 + FOC.limit_voltage = + float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000; + // 目标电流&目标速度 + FOC.limit_velocity = + unpack_bytes_to_float(receive_message.rx_data, 3).f; + FOC.target_current = FOC.limit_velocity; + // 目标角度 target_angle_union.f = unpack_bytes_to_float(receive_message.rx_data, 7).f; FOC.target_angle = target_angle_union.f; - + // 目标速度 target_velocity_union.f = unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f; FOC.target_current = FOC.target_velocity * 10.0f; + + // 解析PID参数 + u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15); + FOC.current_loop.set_P_percent(current_loop_p * P_config); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 17); + FOC.vel_loop.set_P_percent(vel_loop_p * P_config); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); + FOC.angle_loop.set_P_percent(angle_loop_p * P_config); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); + FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config); } else if (receive_message.rx_efid == CANFD_RESET_ID) { u8 first_byte = receive_message.rx_data[0]; u8 eigth_byte = receive_message.rx_data[7]; + // 电机重置 if (first_byte & 0x01) { motor_reset(); } + // EEPROM重置 if (first_byte >> 1 & 0x01) { - flashEEP.format(); + // flashEEP.format(); } + // 系统参数保存 if (first_byte >> 2 & 0x01) { flashEEP.save_system_config(false); } + // 角度传感器重置 if (first_byte >> 3 & 0x01) { KTH7823.angle_set(0); Angle_kalman.init(0); // 角度滤波 Vel_kalman.init(0); // 角度滤波 } + // 休眠 if (eigth_byte & 0x80 >> 7) { app_prepare_deep_sleep(true); // CAN_Sleep(); } + } else if (receive_message.rx_efid == CANFD_VERSION_ID) { + // 处理版本号请求，发送当前系统版本信息 + Version.sendVersionInfoViaCan(); } else if (receive_message.rx_efid == CANFD_BOOT_ID) { + flashEEP.save_system_config(false); fmc_unlock(); OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活 fmc_lock(); @@ -805,9 +824,9 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { valid_length = 16; } else if (length > 16 and length < 21) { valid_length = 20; - } else if (length > 20 and length < 24) { + } else if (length > 20 and length < 25) { valid_length = 24; - } else if (length > 28 and length < 33) { + } else if (length > 24 and length < 33) { valid_length = 32; } else if (length > 32 and length < 49) { valid_length = 48; diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index c1d9d6c..a8f0056 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -1,6 +1,7 @@ #include "ang_sensor.h" #include "Can.h" #include "Filter.h" +#include "config.h" #include "flash.h" #include "gd32c10x_dma.h" #include "gd32c10x_gpio.h" @@ -222,7 +223,7 @@ inline float angle_sensor::read_sensorAngle(void) { if (result == 0) { // 单次浮点乘法优化：只计算一次度数，然后转换为弧度 - float angle_deg = (float)raw_data * ANGLESENS_FACTOR; + float angle_deg = -(float)raw_data * ANGLESENS_FACTOR + ANGLESENS_OFFSET; current_angle_deg = angle_deg; current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f return angle_deg; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 8b98faf..38392ee 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -24,7 +24,9 @@ extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern ThermalController thermalController; // 热量控制器 extern FOC_Controlroller FOC; // FOC控制 extern FlashEEPROM flashEEP; // EEPROM类 +extern VersionManager Version; +// 全局版本管理器实例 TaskScheduler scheduler = {0}; volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次 volatile u32 cnt_test = 0; // 负载检测 @@ -38,6 +40,7 @@ int main(void) { angsens_MG_check(); // 角度传感器磁感应强度测试 watchdog_init(50); // 看门狗初始化 + while (1) { __disable_irq(); u32 currentTasks = @@ -57,6 +60,7 @@ u8 hardware_init(void) { USART0_init(); // 调试串口初始化 motor_init(); // 电机驱动初始化 User_timer_init(); // 任务Timer初始化 + u8 init_status = 0; init_status += KTH7823.init(); // 角度传感器初始化 init_status += sampler.init(); // 采样器初始化 @@ -117,7 +121,7 @@ void do_task(u32 currentTasks) { power_manage(); } if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 - CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 + // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 cnt_test = 0; CAN0_transmiter.can0_send_flag = SET; CAN0_transmiter.communication_check(); diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index d0d1fca..5307ed3 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -5,40 +5,64 @@ // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 #define ARM_MATH_CM4 -#define EEP_DEBUG 0 +#define EEP_DEBUG 1 #define OTA_DEBUG 1 #define SLEEP_DEBUG 1 +// 版本号定义 - 可根据实际项目进行调整 +constexpr u16 SOFTWARE_VERSION_MAJOR = 26; +constexpr u16 SOFTWARE_VERSION_MINOR = 1; +constexpr u16 SOFTWARE_VERSION_PATCH = 19; +constexpr u16 SOFTWARE_VERSION_BUILD = 1; + +constexpr u16 HARDWARE_VERSION_MAJOR = 25; +constexpr u16 HARDWARE_VERSION_MINOR = 12; +constexpr u16 HARDWARE_VERSION_PATCH = 01; +constexpr u16 HARDWARE_VERSION_BUILD = 0; + +constexpr u16 BOOT_VERSION_MAJOR = 26; +constexpr u16 BOOT_VERSION_MINOR = 1; +constexpr u16 BOOT_VERSION_PATCH = 12; +constexpr u16 BOOT_VERSION_BUILD = 0; + // Flash分区配置 -constexpr u32 BOOTLOADER_SIZE = 0x4000; // 16KB +constexpr u32 BOOTLOADER_SIZE = 0x6000; // 24KB constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT起始地址 -constexpr u32 APPLATEST_START_ADDR = 0x08011000; // APP起始地址 -constexpr u32 APPBACK_START_ADDR = 0x08005000; // APPBACK起始地址 +constexpr u32 APPLATEST_START_ADDR = 0x08012000; // APP起始地址 +constexpr u32 APPBACK_START_ADDR = 0x08006000; // APPBACK起始地址 constexpr u32 APP_MAX_SIZE = 0xC000; // 48KB constexpr u32 FMC_PAGE_SIZE = 0x400; // 1KB -constexpr u32 SLEEP_CONFIG_START = 0x0801D400; // 睡眠配置起始地址 -constexpr u32 SLEEP_CONFIG_END = 0x0801D800; // 睡眠配置结束地址 +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; // 睡眠禁止 // BOOT 相关 constexpr u32 OTA_CONFIG_NUM = 6; -constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801D000; // 最新APP可用地址 -constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801D004; // 备份APP可用地址 -constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801D008; // 最新APP激活 -constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801D00C; // 备份APP激活 -constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801D010; // 最新APP校验 -constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801D014; // 备份APP校验 +constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801E000; // 最新APP可用地址 +constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801E004; // 备份APP可用地址 +constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801E008; // 最新APP激活 +constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801E00C; // 备份APP激活 +constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801E010; // 最新APP校验 +constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801E014; // 备份APP校验 constexpr u32 OTA_NOT_AVAL = 0xFFFFFFFF; constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 // 参数配置 constexpr u8 PP = 11; -constexpr float DIR = 1.0; +constexpr float DIR = -1.0f; constexpr u16 REDUCTION_RATIO = 25; constexpr float CURRENT_OFFSET = -1.615f; // 当前电流传感器的零点偏移 constexpr float HOLD_PWM_DUTY = 0.95f; +constexpr u16 ANGLESENS_OFFSET = +0; +constexpr float KE_FACTOR = 14.0f; +// PID配置 +constexpr float current_loop_p = 6e-2f / 100; // 电流P +constexpr float vel_loop_p = 0.01f / 100; // 速度P +constexpr float angle_loop_p = 20.0f / 100; // 角度P +constexpr float vel_angloop_p = 0.12f / 100; // 速度 速度-角度P +constexpr float ang_angloop_p = 100.0f / 100; // 角度 速度-角度P // 常量定义 constexpr u8 SIZE_FLOAT = sizeof(float); @@ -62,12 +86,15 @@ constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI)); constexpr u16 DEV_CAN0_ID = 0x0000; constexpr u16 DEV_CAN0_MASK = 0x0000; constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000; - -constexpr u32 CANFD_BOOT_ID = 0x00000000; +// 反馈ID constexpr u32 CANFD_STATUS_ID = 0x00010100; constexpr u32 CANFD_CONFIG_ID = 0x00010101; +constexpr u32 CANFD_VERSION_ID = 0x00010000; +// 命令ID constexpr u32 CANFD_CMD_ID = 0x00010200; constexpr u32 CANFD_RESET_ID = 0x00010301; +// OTA相关ID +constexpr u32 CANFD_BOOT_ID = 0x00000000; constexpr u32 CANFD_OTA_REQ_ID = 0x00000100; constexpr u32 CANFD_OTA_DATA_ID = 0x00000101; constexpr u32 CANFD_OTA_ACK_ID = 0x00000102; diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 36720a2..a03d875 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -5,6 +5,7 @@ #include "gd32c10x.h" // GD32主头文件，确保外设函数声明可用 #include "gd32c10x_crc.h" #include "gd32c10x_fmc.h" +#include "systick.h" #include #include #include @@ -531,16 +532,20 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { /** * @brief 设置OTA配置参数到Flash - * + * * 此函数用于更新存储在Flash中的OTA相关标志位（如APP可用、启用、校验等）。 * GD32的Flash特性是：只能将1写为0，不能将0写为1。若要将0变为1，必须先擦除整个页面。 * 因此，本函数通过读取同一页内的所有参数，擦除该页，然后重新写入有效的参数来实现“写1”操作。 - * + * * @param addr: 要设置的Flash地址 - * @param data: 要写入的数据 (通常为OTA_AVAL=0x00000000 或 OTA_NOT_AVAL=0xFFFFFFFF) + * @param data: 要写入的数据 (通常为OTA_AVAL=0x00000000 或 + * OTA_NOT_AVAL=0xFFFFFFFF) * @retval FLASH_EEPROM_OK (0) 表示成功, 其他值表示失败 */ u8 OTA_SET(u32 addr, u32 data) { +#if EEP_DEBUG + printf("OTA_prepare write%08x at %08x\r\n", data, addr); +#endif fmc_state_enum fmc_status; u32 temp_val[OTA_CONFIG_NUM]; // 临时数组，用于存储当前所有配置参数的值 volatile u32 *flash_ptr = (volatile u32 *)addr; @@ -562,7 +567,6 @@ u8 OTA_SET(u32 addr, u32 data) { } // 3. 对于需要将0改回1的操作，必须先擦除整个Flash页 - // 首先读取同一页内所有相关配置参数的当前状态 flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; temp_val[0] = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; @@ -577,13 +581,13 @@ u8 OTA_SET(u32 addr, u32 data) { temp_val[5] = *flash_ptr; #if EEP_DEBUG - printf("OTA_SET - Start updating configuration\r\n"); + printf("OTA_prepare write%08x at %08x\r\n", data, addr); // 修复：完整输出所有6个配置项的值，便于调试 printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, " "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n", - temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], temp_val[5]); + temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], + temp_val[5]); #endif - // 4. 擦除包含所有配置参数的Flash页 fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); if (fmc_status != FMC_READY) { @@ -593,32 +597,44 @@ u8 OTA_SET(u32 addr, u32 data) { return FLASH_EEPROM_ERASE_ERROR; } - // 5. 重新写入所有之前为有效(OTA_AVAL)的配置项 + // 5. 重新写入所有之前读取的配置项值（恢复原始状态） // 注意：不包括当前正在设置的地址，因为它将在最后单独处理 for (int i = 0; i < OTA_CONFIG_NUM; i++) { u32 current_addr = n_APP_LATEST_AVAL_ADDR + 4 * i; - if (temp_val[i] == OTA_AVAL && current_addr != addr) { - fmc_status = write_ADDRESS(current_addr, OTA_AVAL); + if (current_addr != addr and temp_val[i] != OTA_NOT_AVAL) { + fmc_status = write_ADDRESS(current_addr, temp_val[i]); if (fmc_status != FMC_READY) { #if EEP_DEBUG - printf("OTA_SET - Write backup value failed at 0x%08X!\r\n", current_addr); + printf("OTA_SET - Write backup value %08X failed at " + "0x%08X!status:%d\r\n", + temp_val[i], current_addr, fmc_status); #endif return FLASH_EEPROM_WRITE_ERROR; } } } - // 6. 最后，写入本次调用的目标数据 - // 即使addr为0（理论上无效），也检查一下以避免潜在问题 - if (addr != 0) { - fmc_status = write_ADDRESS(addr, data); - if (fmc_status != FMC_READY) { #if EEP_DEBUG - printf("OTA_SET - Final write to target address 0x%08X failed!\r\n", addr); + // 3. 对于需要将0改回1的操作，必须先擦除整个Flash页 + flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; + temp_val[0] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + temp_val[1] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR; + temp_val[2] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_EN_ADDR; + temp_val[3] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + temp_val[4] = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + temp_val[5] = *flash_ptr; + // 修复：完整输出所有6个配置项的值，便于调试 + printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, " + "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n", + temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], + temp_val[5]); #endif - return FLASH_EEPROM_WRITE_ERROR; - } - } + return FLASH_EEPROM_OK; } @@ -675,13 +691,16 @@ fmc_state_enum move_app_data() { // 数据大小: APP_MAX_SIZE (0xC000 = 48KB) fmc_state_enum fmc_status = FMC_READY; - +#if OTA_DEBUG + printf("move_app_data\r\n"); +#endif // 关闭全局中断以防止Flash操作期间的中断干扰 __disable_irq(); fmc_unlock(); // 首先擦除目标地址的Flash页面 - uint32_t num_pages = (APP_MAX_SIZE + FMC_PAGE_SIZE - 1) / FMC_PAGE_SIZE; // 向上取整 + uint32_t num_pages = + (APP_MAX_SIZE + FMC_PAGE_SIZE - 1) / FMC_PAGE_SIZE; // 向上取整 for (uint32_t i = 0; i < num_pages; i++) { uint32_t page_addr = APPBACK_START_ADDR + i * FMC_PAGE_SIZE; @@ -692,6 +711,7 @@ fmc_state_enum move_app_data() { fmc_lock(); #if OTA_DEBUG printf("move_app_data erase error\r\n"); + delay_1ms(1000); #endif __enable_irq(); // 重新开启中断 return fmc_status; @@ -701,28 +721,34 @@ fmc_state_enum move_app_data() { // 复制数据 - 按字（32位）进行复制以提高效率 uint32_t offset = 0; uint32_t write_count = 0; // 记录实际写入的字数 - const uint32_t* src_ptr = (const uint32_t*)APPLATEST_START_ADDR; - uint32_t* dst_ptr = (uint32_t*)APPBACK_START_ADDR; + const uint32_t *src_ptr = (const uint32_t *)APPLATEST_START_ADDR; + uint32_t *dst_ptr = (uint32_t *)APPBACK_START_ADDR; // 预计算源数据的CRC，避免重复访问内存 crc_deinit(); crc_data_register_reset(); - uint32_t src_crc = crc_block_data_calculate((uint32_t*)APPLATEST_START_ADDR, APP_MAX_SIZE/4); + uint32_t src_crc = crc_block_data_calculate( + (uint32_t *)APPLATEST_START_ADDR, APP_MAX_SIZE / 4); while (offset < APP_MAX_SIZE) { - uint32_t src_data = src_ptr[offset/4]; + uint32_t src_data = src_ptr[offset / 4]; // 检查源地址是否为有效的Flash数据（非0xFFFFFFFF表示有数据） if (src_data != 0xFFFFFFFF) { + fmc_unlock(); // 清除之前的标志位 - fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGAERR | FMC_FLAG_PGERR); - - fmc_status = fmc_word_program(APPBACK_START_ADDR + offset, src_data); + fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGAERR | + FMC_FLAG_PGERR); + fmc_status = + fmc_word_program(APPBACK_START_ADDR + offset, src_data); // 检查写入操作是否成功 if (fmc_status != FMC_READY) { #if OTA_DEBUG - printf("Flash write failed at offset 0x%08X with status: %d\r\n", offset, fmc_status); + printf( + "Flash write failed at offset 0x%08X with status: %d\r\n", + offset, fmc_status); + delay_1ms(1000); #endif fmc_lock(); __enable_irq(); // 重新开启中断 @@ -731,9 +757,9 @@ fmc_state_enum move_app_data() { write_count++; } offset += 4; - + // 每写入一定数量的数据后，喂狗一次 - if ((offset % 4096) == 0) { // 每4KB喂狗一次 + if ((offset % 4096) == 0) { // 每4KB喂狗一次 fwdgt_counter_reload(); } } @@ -745,15 +771,20 @@ fmc_state_enum move_app_data() { crc_data_register_reset(); // 确保CRC计算从初始状态开始 // 计算目标数据的CRC值 - uint32_t dst_crc = crc_block_data_calculate((uint32_t*)APPBACK_START_ADDR, APP_MAX_SIZE/4); + uint32_t dst_crc = crc_block_data_calculate((uint32_t *)APPBACK_START_ADDR, + APP_MAX_SIZE / 4); // 比较CRC值，确保数据完整性 if (src_crc != dst_crc) { #if OTA_DEBUG - printf("CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n", src_crc, dst_crc); + printf("APPbackup CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n", + src_crc, dst_crc); #endif return (fmc_state_enum)0xFF; // CRC校验失败 } +#if OTA_DEBUG + printf("APPbackup CRC OK\r\n"); +#endif // 设置备份应用可用标志 fmc_unlock(); OTA_SET(n_APP_BACKUP_AVAL_ADDR, OTA_AVAL); diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index e8d62ac..25811fe 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -6,11 +6,125 @@ #include "gd32c10x_fmc.h" #include "gd32c10x_gpio.h" #include "gd32c10x_rcu.h" +#include "initial.h" +#include #include -#include -#include +#include extern CAN_transmiter CAN0_transmiter; // CAN初始化 +VersionManager Version; + +VersionManager::VersionManager() { + // 初始化版本号 + setSoftwareVersion(SOFTWARE_VERSION_MAJOR, SOFTWARE_VERSION_MINOR, + SOFTWARE_VERSION_PATCH, SOFTWARE_VERSION_BUILD); + setHardwareVersion(HARDWARE_VERSION_MAJOR, HARDWARE_VERSION_MINOR, + HARDWARE_VERSION_PATCH, HARDWARE_VERSION_BUILD); + setBootVersion(BOOT_VERSION_MAJOR, BOOT_VERSION_MINOR, BOOT_VERSION_PATCH, + BOOT_VERSION_BUILD); +} + +VersionManager::VersionInfo VersionManager::getSoftwareVersion() const { + return softwareVersion; +} + +VersionManager::VersionInfo VersionManager::getHardwareVersion() const { + return hardwareVersion; +} + +VersionManager::VersionInfo VersionManager::getBootVersion() const { + return bootVersion; +} + +void VersionManager::setSoftwareVersion(u16 major, u16 minor, u16 patch, + u16 build) { + softwareVersion.major = major; + softwareVersion.minor = minor; + softwareVersion.patch = patch; + softwareVersion.build = build; +} + +void VersionManager::setHardwareVersion(u16 major, u16 minor, u16 patch, + u16 build) { + hardwareVersion.major = major; + hardwareVersion.minor = minor; + hardwareVersion.patch = patch; + hardwareVersion.build = build; +} + +void VersionManager::setBootVersion(u16 major, u16 minor, u16 patch, + u16 build) { + bootVersion.major = major; + bootVersion.minor = minor; + bootVersion.patch = patch; + bootVersion.build = build; +} + +char *VersionManager::versionToString(const VersionInfo &version, char *buffer, + size_t bufSize) const { + if (buffer == nullptr) { + return nullptr; + } + + snprintf(buffer, bufSize, "%d.%d.%d.%d", version.major, version.minor, + version.patch, version.build); + + return buffer; +} + +void VersionManager::sendVersionInfoViaCan() const { + // 准备发送版本信息 (ID: CANFD_VERSION_ID) + // 只使用必要的字节数（24字节）来传输三个版本的信息（每个版本6字节） + u8 versionData[24] = {0}; + + // 将版本信息打包到24字节数组中 + // 前8字节：软件版本信息 (4个uint16_t，每个占2字节) + versionData[0] = softwareVersion.major & 0xFF; // 低字节 + versionData[1] = (softwareVersion.major >> 8) & 0xFF; // 高字节 + versionData[2] = softwareVersion.minor & 0xFF; // 低字节 + versionData[3] = (softwareVersion.minor >> 8) & 0xFF; // 高字节 + versionData[4] = softwareVersion.patch & 0xFF; // 低字节 + versionData[5] = (softwareVersion.patch >> 8) & 0xFF; // 高字节 + versionData[6] = softwareVersion.build & 0xFF; // 低字节 + versionData[7] = (softwareVersion.build >> 8) & 0xFF; // 高字节 + + // 8-15字节：硬件版本信息 (4个uint16_t，每个占2字节) + versionData[8] = hardwareVersion.major & 0xFF; // 低字节 + versionData[9] = (hardwareVersion.major >> 8) & 0xFF; // 高字节 + versionData[10] = hardwareVersion.minor & 0xFF; // 低字节 + versionData[11] = (hardwareVersion.minor >> 8) & 0xFF; // 高字节 + versionData[12] = hardwareVersion.patch & 0xFF; // 低字节 + versionData[13] = (hardwareVersion.patch >> 8) & 0xFF; // 高字节 + versionData[14] = hardwareVersion.build & 0xFF; // 低字节 + versionData[15] = (hardwareVersion.build >> 8) & 0xFF; // 高字节 + + // 16-23字节：BOOT版本信息 (4个uint16_t，每个占2字节) + versionData[16] = bootVersion.major & 0xFF; // 低字节 + versionData[17] = (bootVersion.major >> 8) & 0xFF; // 高字节 + versionData[18] = bootVersion.minor & 0xFF; // 低字节 + versionData[19] = (bootVersion.minor >> 8) & 0xFF; // 高字节 + versionData[20] = bootVersion.patch & 0xFF; // 低字节 + versionData[21] = (bootVersion.patch >> 8) & 0xFF; // 高字节 + versionData[22] = bootVersion.build & 0xFF; // 低字节 + versionData[23] = (bootVersion.build >> 8) & 0xFF; // 高字节 + + // 使用CANFD发送函数发送版本信息 + CAN0_transmiter.CANFD_AsyncSend(CANFD_VERSION_ID, 24, versionData); +} + +void VersionManager::printVersions() const { + char versionStr[32]; + + printf( + "Software Version: %s\r\n", + versionToString(getSoftwareVersion(), versionStr, sizeof(versionStr))); + printf( + "Hardware Version: %s\r\n", + versionToString(getHardwareVersion(), versionStr, sizeof(versionStr))); + printf("BOOT Version: %s\r\n", + versionToString(getBootVersion(), versionStr, sizeof(versionStr))); +} + /* retarget the C library printf function to the USART */ u8 USART0_init() { gd_eval_com_init(EVAL_COM0); @@ -33,7 +147,7 @@ u8 watchdog_init(u32 time) { } int fputc(int ch, FILE *f) throw() { - usart_data_transmit(EVAL_COM0, (uint8_t)ch); + usart_data_transmit(EVAL_COM0, (u8)ch); while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE)) ; return ch; @@ -97,23 +211,52 @@ u16 get_reset_cause(void) { } void OTA_initial(void) { +#if OTA_DEBUG + u32 temp_val[6]; + // 3. 对于需要将0改回1的操作，必须先擦除整个Flash页 + volatile u32 *flash_ptr1 = (volatile u32 *)n_APP_LATEST_AVAL_ADDR; + temp_val[0] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + temp_val[1] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_LATEST_EN_ADDR; + temp_val[2] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_BACKUP_EN_ADDR; + temp_val[3] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + temp_val[4] = *flash_ptr1; + flash_ptr1 = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; + temp_val[5] = *flash_ptr1; + // 修复：完整输出所有6个配置项的值，便于调试 + printf("APP:CHECK LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, " + "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n", + temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4], + temp_val[5]); +#endif // 确认当前APP可用 + volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + u32 app_check = *flash_ptr; + if (app_check == OTA_NOT_AVAL) { // 新的APP刷入后第一次启动 +#if OTA_DEBUG + std::printf("APP enabled\r\n"); +#endif + move_app_data(); + } + fmc_unlock(); OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL); - OTA_SET(n_APP_LATEST_EN_ADDR, OTA_AVAL); fmc_lock(); + u8 transmit_data[2] = {0x0A, 0xAC}; // 确认APP可用 CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data); - volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; + flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR; u32 back_aval = *flash_ptr; flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR; u32 back_check = *flash_ptr; - // 如果BACK不可用 - if (back_aval == OTA_NOT_AVAL) { + if (back_aval == OTA_NOT_AVAL) { // 如果BACK不可用 #if OTA_DEBUG std::printf("notaval\r\n"); #endif diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 64e9491..b2b20f8 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -40,8 +40,56 @@ typedef struct { volatile u32 taskFlags; // 任务标志位(每个位代表一个任务) } TaskScheduler; +// 版本管理类 +class VersionManager { + public: + // 版本信息结构体 + typedef struct { + u16 major; + u16 minor; + u16 patch; + u16 build; + } VersionInfo; + + // 构造函数 + VersionManager(); + + // 获取软件版本 + VersionInfo getSoftwareVersion() const; + + // 获取硬件版本 + VersionInfo getHardwareVersion() const; + + // 获取BOOT版本 + VersionInfo getBootVersion() const; + + // 设置软件版本 + void setSoftwareVersion(u16 major, u16 minor, u16 patch, u16 build); + + // 设置硬件版本 + void setHardwareVersion(u16 major, u16 minor, u16 patch, u16 build); + + // 设置BOOT版本 + void setBootVersion(u16 major, u16 minor, u16 patch, u16 build); + + // 将版本信息转换为字符串 + char *versionToString(const VersionInfo &version, char *buffer, + size_t bufSize) const; + + // 打印所有版本信息 + void printVersions() const; + + // 通过CAN发送版本信息 + void sendVersionInfoViaCan() const; + + private: + VersionInfo softwareVersion; + VersionInfo hardwareVersion; + VersionInfo bootVersion; +}; + #ifdef __cplusplus } #endif -#endif +#endif \ No newline at end of file From 3f24292ac1ad5921b9ae794a018745e20b9a2a3b Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Wed, 21 Jan 2026 11:48:31 +0800 Subject: [PATCH 15/16] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0FOC?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E5=99=A8=E5=88=9D=E5=A7=8B=E5=8C=96=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E3=80=81=E6=94=AF=E6=8C=81=E7=94=B5=E5=8E=8B=E8=87=AA?= =?UTF-8?q?=E9=80=82=E5=BA=94PID=E8=B0=83=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 添加了FOC控制器的init()方法，用于统一初始化电机控制参数， 包括速度环、角度环和电流环的PID参数设置。同时优化了CAN通信 中的PID参数计算逻辑，引入电压补偿机制，提高了电机控制精度。 --- Robot_V2/Algorithm/FOC.cpp | 35 +++++++++++++++++++++++++++++------ Robot_V2/Algorithm/FOC.h | 1 + Robot_V2/device/Can.cpp | 9 +++++---- Robot_V2/main.cpp | 8 ++++---- Robot_V2/sys/config.h | 14 +++++++------- 5 files changed, 46 insertions(+), 21 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 7fa485c..d82f9a9 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -1,5 +1,6 @@ #include "FOC.h" #include "Can.h" +#include "Filter.h" #include "Timer.h" #include "adc.h" #include "ang_sensor.h" @@ -9,14 +10,14 @@ #include #include #include -#include "Filter.h" #include + FOC_Controlroller FOC; // FOC控制初始化 extern sampler sampler; extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; -extern EncoderVelocityKalman Vel_kalman; // 速度滤波 +extern EncoderVelocityKalman Vel_kalman; // 速度滤波 // 初始变量及函数定义 #define _constrain(amt, low, high) \ @@ -56,7 +57,29 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b, I_q = I_beta * cos_el - I_alpha * sin_el; // arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el); - return I_q * 1000.0f - KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA + return I_q * 1000.0f - + KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA +} + +/** + * @brief 电机控制类初始化函数 + * + * @return u8 0表示初始化成功，1表示初始化失败 + */ +u8 FOC_Controlroller::init() { + motor_init(); // 电机驱动初始化 + vel_loop.set_PID(vel_loop_p * 100 / limit_voltage, 0.20f, 5e-5); // 设置电机速度控制参数 + angle_loop.set_PID(angle_loop_p * 100 / limit_voltage, 100.0f, + 0.2f); // 设置电机角度控制参数 + current_loop.set_PID(current_loop_p * 100 / limit_voltage, 5e-7f, + 7e-8); // 设置电机电流控制参数 + + vel_inVelAngloop.set_PID(vel_angloop_p * 100 / limit_voltage, 3.0f, + 5e-4); // 设置电机速度控制参数 + ang_inVelAngloop.set_PID(ang_angloop_p * 100 / limit_voltage, 0.0f, + 5e-4); // 设置电机角度控制参数 + + return 0; } /** @@ -158,9 +181,9 @@ void FOC_Controlroller::update_power(void) { */ float FOC_Controlroller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 - shaft_angle = _normalizeAngle(shaft_angle + - target_velocity * ANGLE_FACTOR * - REDUCTION_RATIO * 1.0 / TIMER2_FREQUENCY); + shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * + REDUCTION_RATIO * 1.0 / + TIMER2_FREQUENCY); // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = voltage_power_supply / 2.0f; diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 7e83c68..27204a5 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -25,6 +25,7 @@ class FOC_Controlroller { 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; + u8 init(void); float velocity_open_loop(void); float angle_control(void); float angle_control_with_velocity_limit(void); diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 693538b..4e191c4 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -597,13 +597,14 @@ void CAN_transmiter::communication_check() { // 解析PID参数 u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15); - FOC.current_loop.set_P_percent(current_loop_p * P_config); + float voltage = FOC.limit_voltage; + FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage); P_config = unpack_bytes_to_u16(receive_message.rx_data, 17); - FOC.vel_loop.set_P_percent(vel_loop_p * P_config); + FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage); P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); - FOC.angle_loop.set_P_percent(angle_loop_p * P_config); + FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage); P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); - FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config); + FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config / voltage); } else if (receive_message.rx_efid == CANFD_RESET_ID) { u8 first_byte = receive_message.rx_data[0]; u8 eigth_byte = receive_message.rx_data[7]; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 38392ee..2a54fd6 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -40,7 +40,7 @@ int main(void) { angsens_MG_check(); // 角度传感器磁感应强度测试 watchdog_init(50); // 看门狗初始化 - + while (1) { __disable_irq(); u32 currentTasks = @@ -58,9 +58,8 @@ u8 hardware_init(void) { systick_config(); // 系统时钟初始化 unused_pin_init(); // 未使用引脚初始化 USART0_init(); // 调试串口初始化 - motor_init(); // 电机驱动初始化 User_timer_init(); // 任务Timer初始化 - + u8 init_status = 0; init_status += KTH7823.init(); // 角度传感器初始化 init_status += sampler.init(); // 采样器初始化 @@ -69,6 +68,7 @@ u8 hardware_init(void) { init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 init_status += CAN0_transmiter.init(); // CAN初始化 + init_status += FOC.init(); // FOC初始化 return init_status; } @@ -120,7 +120,7 @@ void do_task(u32 currentTasks) { if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 power_manage(); } - if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 + if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 cnt_test = 0; CAN0_transmiter.can0_send_flag = SET; diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index 5307ed3..fd15d78 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -12,7 +12,7 @@ // 版本号定义 - 可根据实际项目进行调整 constexpr u16 SOFTWARE_VERSION_MAJOR = 26; constexpr u16 SOFTWARE_VERSION_MINOR = 1; -constexpr u16 SOFTWARE_VERSION_PATCH = 19; +constexpr u16 SOFTWARE_VERSION_PATCH = 20; constexpr u16 SOFTWARE_VERSION_BUILD = 1; constexpr u16 HARDWARE_VERSION_MAJOR = 25; @@ -53,16 +53,16 @@ constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 constexpr u8 PP = 11; constexpr float DIR = -1.0f; constexpr u16 REDUCTION_RATIO = 25; -constexpr float CURRENT_OFFSET = -1.615f; // 当前电流传感器的零点偏移 +constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移 constexpr float HOLD_PWM_DUTY = 0.95f; constexpr u16 ANGLESENS_OFFSET = +0; constexpr float KE_FACTOR = 14.0f; // PID配置 -constexpr float current_loop_p = 6e-2f / 100; // 电流P -constexpr float vel_loop_p = 0.01f / 100; // 速度P -constexpr float angle_loop_p = 20.0f / 100; // 角度P -constexpr float vel_angloop_p = 0.12f / 100; // 速度 速度-角度P -constexpr float ang_angloop_p = 100.0f / 100; // 角度 速度-角度P +constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P +constexpr float vel_loop_p = 0.01f / 100 * 12; // 速度P +constexpr float angle_loop_p = 20.0f / 100 * 12; // 角度P +constexpr float vel_angloop_p = 0.12f / 100 * 12; // 速度 速度-角度P +constexpr float ang_angloop_p = 100.0f / 100 * 12; // 角度 速度-角度P // 常量定义 constexpr u8 SIZE_FLOAT = sizeof(float); From 1596b2beb080db3f3fe1a3a8f4b215ccd39d45db Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sat, 24 Jan 2026 01:50:39 +0800 Subject: [PATCH 16/16] =?UTF-8?q?DRV8313=E5=85=BC=E5=AE=B9=E7=89=88=20refa?= =?UTF-8?q?ctor(algorithm):=20=E7=A7=BB=E9=99=A4=E5=86=97=E4=BD=99?= =?UTF-8?q?=E5=A4=B4=E6=96=87=E4=BB=B6=20=E7=A7=BB=E9=99=A4=E4=B8=8D?= =?UTF-8?q?=E5=BF=85=E8=A6=81=E7=9A=84stdint.h=E5=A4=B4=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E5=BC=95=E7=94=A8=EF=BC=8C=E7=BB=9F=E4=B8=80=E4=BD=BF=E7=94=A8?= =?UTF-8?q??= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit refactor(sys): 清理系统级头文件依赖 移除未使用的头文件引用，统一整型类型定义，优化初始 化过程中的GPIO配置 --- Robot_V2/Algorithm/FOC.cpp | 4 ++-- Robot_V2/Algorithm/FOC.h | 15 ++++++++++++--- Robot_V2/Algorithm/Filter.cpp | 1 - Robot_V2/Algorithm/Filter.h | 2 +- Robot_V2/Algorithm/pid.cpp | 3 ++- Robot_V2/Algorithm/pid.h | 33 ++++++++++++++++----------------- Robot_V2/device/ADC.cpp | 3 +-- Robot_V2/device/Can.cpp | 10 +++++----- Robot_V2/device/Can.h | 2 -- Robot_V2/device/Timer.cpp | 3 --- Robot_V2/device/Timer.h | 4 ++-- Robot_V2/device/adc.h | 1 - Robot_V2/device/ang_sensor.h | 2 +- Robot_V2/device/motor.cpp | 18 ++++++++++++++++-- Robot_V2/device/motor.h | 5 ++++- Robot_V2/main.cpp | 13 ++++--------- Robot_V2/main.h | 2 +- Robot_V2/sys/config.h | 4 ++-- Robot_V2/sys/flash.h | 2 +- Robot_V2/sys/health.cpp | 1 - Robot_V2/sys/health.h | 2 +- Robot_V2/sys/initial.cpp | 4 ++-- Robot_V2/sys/initial.h | 4 ++-- Robot_V2/sys/systick.h | 2 +- 24 files changed, 76 insertions(+), 64 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index d82f9a9..e2e875e 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -10,7 +10,6 @@ #include #include #include -#include FOC_Controlroller FOC; // FOC控制初始化 @@ -58,7 +57,7 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b, // arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el); return I_q * 1000.0f - - KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA + KE_FACTOR * rt_velocity; // 返回Iq值，乘以100以转换为mA } /** @@ -79,6 +78,7 @@ u8 FOC_Controlroller::init() { ang_inVelAngloop.set_PID(ang_angloop_p * 100 / limit_voltage, 0.0f, 5e-4); // 设置电机角度控制参数 + return 0; } diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 27204a5..9c40f1c 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -3,12 +3,15 @@ #define _FOC_H #include "config.h" #include "pid.h" -#include +#include class FOC_Controlroller { public: FOC_Controlroller(); + + // 初始化函数 + u8 init(); u8 control_mode = 0; float limit_voltage = 0.0f; @@ -22,10 +25,9 @@ class FOC_Controlroller { bool voltage_error = false; bool is_shut_down = false; float voltage_power_supply = 24.0f; // 电源电压 - float v_safe = voltage_power_supply * 1.05f; + float v_safe = voltage_power_supply * 0.95f; float duty_a = 0, duty_b = 0, duty_c = 0; float Ua = 0, Ub = 0, Uc = 0; - u8 init(void); float velocity_open_loop(void); float angle_control(void); float angle_control_with_velocity_limit(void); @@ -50,6 +52,13 @@ class FOC_Controlroller { PIDController ang_inVelAngloop = PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999}; + void update_pid_limits() { + vel_loop.limit = voltage_power_supply; + angle_loop.limit = voltage_power_supply; + current_loop.limit = voltage_power_supply; + vel_inVelAngloop.limit = voltage_power_supply; + } + protected: float shaft_angle = 0; float Ualpha, Ubeta = 0; diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index 449dc00..4a5f776 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -4,7 +4,6 @@ #include "config.h" #include #include -#include extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index 26096bb..a0b3fb8 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -14,7 +14,7 @@ extern "C" { #ifdef __cplusplus #include -#include +#include float medium_filter(float values[], u8 size); diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index e650a96..82406bd 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -2,7 +2,8 @@ #define _constrain(amt, low, high) \ ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) #include "Can.h" -#include "math.h" +#include +#include extern CAN_transmiter CAN0_transmiter; constexpr float TIME_SLICE = 1e-4; diff --git a/Robot_V2/Algorithm/pid.h b/Robot_V2/Algorithm/pid.h index d8fb2f1..59dfecf 100644 --- a/Robot_V2/Algorithm/pid.h +++ b/Robot_V2/Algorithm/pid.h @@ -1,26 +1,25 @@ #ifndef PID_H #define PID_H -class PIDController -{ -public: - PIDController(float P, float I, float D, float ramp, float limit); - ~PIDController() = default; +class PIDController { + public: + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; - float operator() (float error); + float operator()(float error); void set_PID(float P, float I, float D); void set_P_percent(float pp); - float P; //!< 比例增益(P环增益) - float I; //!< 积分增益（I环增益） - float D; //!< 微分增益（D环增益） - float output_ramp; - float limit; -protected: - float error_prev; //!< 最后的跟踪误差值 - float output_prev; //!< 最后一个 pid 输出值 - float integral_prev; //!< 最后一个积分分量值 - unsigned long timestamp_prev; //!< 上次执行时间戳 -}; + float P; //!< 比例增益(P环增益) + float I; //!< 积分增益（I环增益） + float D; //!< 微分增益（D环增益） + float output_ramp; + float limit; + protected: + float error_prev; //!< 最后的跟踪误差值 + float output_prev; //!< 最后一个 pid 输出值 + float integral_prev; //!< 最后一个积分分量值 + unsigned long timestamp_prev; //!< 上次执行时间戳 +}; #endif \ No newline at end of file diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 8d6ebf8..2ea86bf 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -2,13 +2,12 @@ #include "Can.h" #include "FOC.h" #include "Filter.h" -#include "arm_math.h" #include "config.h" #include "math.h" #include "systick.h" #include #include -#include +#include constexpr float ADC_SCALE = 3.3f / 4096.0f; constexpr float TEMP_FACTOR = 1000.0f / 4.3f; diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 4e191c4..648afc3 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -17,9 +17,6 @@ #include #include #include -#include -#include -#include constexpr u16 CANX = CAN0; @@ -55,8 +52,10 @@ u8 can_gpio_init() { gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_9); // tx gpio_pin_remap_config(GPIO_CAN0_PARTIAL_REMAP, ENABLE); /* enable the EN GPIO clock */ - gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_13); + gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, + GPIO_PIN_13 | GPIO_PIN_14); gpio_bit_reset(GPIOB, GPIO_PIN_13); + gpio_bit_reset(GPIOB, GPIO_PIN_14); return 0; } @@ -604,7 +603,8 @@ void CAN_transmiter::communication_check() { P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage); P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); - FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config / voltage); + FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config / + voltage); } else if (receive_message.rx_efid == CANFD_RESET_ID) { u8 first_byte = receive_message.rx_data[0]; u8 eigth_byte = receive_message.rx_data[7]; diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index eeaf828..d897add 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -7,8 +7,6 @@ extern "C" { #endif #include "gd32c10x_can.h" -#include "stdint.h" -#include // 定义一个联合体类型用于float和byte数组之间的转换 typedef union { diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 18ca60e..5838bdd 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -8,9 +8,6 @@ #include "gd32c10x_rcu.h" #include "gd32c10x_timer.h" #include "initial.h" -#include -#include -#include volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1 extern u16 adc_value[7]; diff --git a/Robot_V2/device/Timer.h b/Robot_V2/device/Timer.h index 9029aeb..248b317 100644 --- a/Robot_V2/device/Timer.h +++ b/Robot_V2/device/Timer.h @@ -5,10 +5,10 @@ extern "C" { #endif -#include +#include void User_timer_init(void); -void Set_Pwm(uint16_t channel, float duty_cycle); +void Set_Pwm(u16 channel, float duty_cycle); void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2); void TIMER2_IRQHandler(void); diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h index 65e3834..3283034 100644 --- a/Robot_V2/device/adc.h +++ b/Robot_V2/device/adc.h @@ -5,7 +5,6 @@ extern "C" { #endif -#include "stdint.h" #include "config.h" class sampler { diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index b9cd594..2503b12 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -4,7 +4,7 @@ extern "C" { #endif #include "gd32c10x_eval.h" -#include "stdio.h" +#include #include "config.h" u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord); diff --git a/Robot_V2/device/motor.cpp b/Robot_V2/device/motor.cpp index 083740e..1dffc51 100644 --- a/Robot_V2/device/motor.cpp +++ b/Robot_V2/device/motor.cpp @@ -1,6 +1,7 @@ #include "motor.h" #include "Timer.h" #include "gd32c10x_eval.h" +#include "gd32c10x_gpio.h" #include /** @@ -16,6 +17,11 @@ void motor_init(void) { /* 配置GPIOB的PB14为推挽输出 SPICS */ gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_14 | GPIO_PIN_12); + + // 使能GPIOC时钟，用于电机驱动使能引脚 + rcu_periph_clock_enable(RCU_MOTOR_EN_GPIO); + // 配置PC13为推挽输出，用于控制电机驱动器使能 + gpio_init(MOTOR_EN_GPIO_PORT, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, MOTOR_EN_GPIO_PIN); } /** @@ -30,9 +36,17 @@ void motor_reset(void) { /** * @brief 电机休眠 */ -void motor_sleep(void) { gpio_bit_reset(GPIOB, GPIO_PIN_12); } +void motor_sleep(void) { + gpio_bit_reset(GPIOB, GPIO_PIN_12); + // 同时禁用电机驱动器 + gpio_bit_reset(MOTOR_EN_GPIO_PORT, MOTOR_EN_GPIO_PIN); +} /** * @brief 电机启动 */ -void motor_resume(void) { gpio_bit_set(GPIOB, GPIO_PIN_12); } \ No newline at end of file +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 diff --git a/Robot_V2/device/motor.h b/Robot_V2/device/motor.h index dc198a3..d4c74c1 100644 --- a/Robot_V2/device/motor.h +++ b/Robot_V2/device/motor.h @@ -1,11 +1,14 @@ #ifndef _MOTOR_H #define _MOTOR_H +// 定义电机驱动使能引脚 +#define MOTOR_EN_GPIO_PORT GPIOC +#define MOTOR_EN_GPIO_PIN GPIO_PIN_13 +#define RCU_MOTOR_EN_GPIO RCU_GPIOC void motor_init(void); void motor_reset(void); void motor_resume(void); void motor_sleep(void); - #endif \ No newline at end of file diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 2a54fd6..ba0b5af 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -9,13 +9,8 @@ #include "health.h" #include "initial.h" #include "motor.h" -#include "sys/flash.h" -#include "sys/initial.h" #include "systick.h" -#include -#include -#include -#include + extern CAN_transmiter CAN0_transmiter; // CAN初始化 extern sampler sampler; // 采样器 extern angle_sensor KTH7823; // 角度传感器 @@ -96,11 +91,11 @@ void do_task(u32 currentTasks) { // 角度闭环控制，带速度限制 &FOC_Controlroller::velocity_control, // 2: 速度闭环控制 &FOC_Controlroller::current_control, // 3: 电流闭环控制 - &FOC_Controlroller::velocity_open_loop // 4: 速度开环控制 + &FOC_Controlroller::velocity_open_loop, // 4: 速度开环控制 }; // 边界检查，防止数组越界 - if (FOC.control_mode >= 0 && FOC.control_mode < 5) { + if (FOC.control_mode >= 0 && FOC.control_mode < 5) { // 更新为7个控制模式 // 调用对应的控制函数 (FOC.*control_functions[FOC.control_mode])(); } @@ -120,7 +115,7 @@ void do_task(u32 currentTasks) { if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 power_manage(); } - if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 + if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 cnt_test = 0; CAN0_transmiter.can0_send_flag = SET; diff --git a/Robot_V2/main.h b/Robot_V2/main.h index f39ea43..ab1c246 100644 --- a/Robot_V2/main.h +++ b/Robot_V2/main.h @@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MAIN_H #define MAIN_H -#include "stdint.h" +#include void do_task(u32 currentTasks); u8 hardware_init(void); diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index fd15d78..9a4e872 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -1,6 +1,6 @@ #ifndef __CONFIG_H #define __CONFIG_H -#include +#include // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 #define ARM_MATH_CM4 @@ -12,7 +12,7 @@ // 版本号定义 - 可根据实际项目进行调整 constexpr u16 SOFTWARE_VERSION_MAJOR = 26; constexpr u16 SOFTWARE_VERSION_MINOR = 1; -constexpr u16 SOFTWARE_VERSION_PATCH = 20; +constexpr u16 SOFTWARE_VERSION_PATCH = 24; constexpr u16 SOFTWARE_VERSION_BUILD = 1; constexpr u16 HARDWARE_VERSION_MAJOR = 25; diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 4958a8f..849fb09 100644 --- a/Robot_V2/sys/flash.h +++ b/Robot_V2/sys/flash.h @@ -2,7 +2,7 @@ #define __FLASH_H #include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用 -#include +#include // 修改后 - 强制4字节对齐 #define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index 2b7af17..cd0a76b 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -1,6 +1,5 @@ #include "health.h" #include "config.h" -#include #include "Can.h" #include "FOC.h" #include "adc.h" diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h index a3b22cf..81925b4 100644 --- a/Robot_V2/sys/health.h +++ b/Robot_V2/sys/health.h @@ -1,7 +1,7 @@ #ifndef _HEALTH_H #define _HEALTH_H -#include "stdint.h" +#include typedef int32_t q16_16_t; // Q16.16格式 // 定点数运算宏 #define Q16_16_ONE 65536 // 1.0 in Q16.16 diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index 25811fe..a1d8118 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -162,10 +162,10 @@ void unused_pin_init() { gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_15); gpio_init(GPIOC, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, - GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15); + GPIO_PIN_14 | GPIO_PIN_15); gpio_bit_reset(GPIOA, GPIO_PIN_8 | GPIO_PIN_11 | GPIO_PIN_12); gpio_bit_reset(GPIOB, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_15); - gpio_bit_reset(GPIOC, GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15); + gpio_bit_reset(GPIOC, GPIO_PIN_14 | GPIO_PIN_15); } /*! diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index b2b20f8..088ab54 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -7,8 +7,8 @@ extern "C" { #include "config.h" #include "gd32c10x_eval.h" -#include "stdint.h" -#include +#include +#include // 任务标志定义 typedef enum { diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index 99dd74c..494cf61 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef __cplusplus extern "C" { #endif -#include +#include /* configure systick */ void systick_config(void);