diff --git a/Robot_V2.csolution.yml b/Robot_V2.csolution.yml index b255718..54608ea 100644 --- a/Robot_V2.csolution.yml +++ b/Robot_V2.csolution.yml @@ -30,12 +30,11 @@ solution: target-set: - set: images: - - project-context: Robot_V2.Debug + - 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/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp deleted file mode 100644 index 3580c04..0000000 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include "DengFOC.h" -#include "Can.h" -#include "Filter.h" -#include "Timer.h" -#include "adc.h" -#include "ang_sensor.h" -#include "initial.h" -#include "pid.h" -#include -#include -#include - -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; -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))) -#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); - // 取余运算可以用于归一化，列出特殊值例子算便知 - return a >= 0 ? a : (a + 2 * PI); -} -/** - * @brief 电机控制类构造函数 - * - */ -FOC_Controlroller::FOC_Controlroller() { ; } - -/** - * @brief 设置电机输出，使用优化的SVPWM算法 - * - * @param Uq: q轴电压 - * @param Ud: d轴电压 - * @param angle_el: 电机轴角度 - */ -inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { - // 计算电气角度 - float angle_el = _normalizeAngle(shaft_angle * PP); - - // 帕克逆变换 (直接计算，避免函数调用开销) - 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]; - float Ualpha = Ud * cos_el - Uq * sin_el; - float Ubeta = Ud * sin_el + Uq * cos_el; - - // 克拉克逆变换 (使用预计算常量优化) - Ua = Ualpha + v_half; - Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half; - 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); - // 设置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); - // std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c); -} - -/** - * @brief 电机关机 - * - */ -void FOC_Controlroller::shut_down() { - thermal_uncontrol = 0; - is_shut_down = 1; - gpio_bit_reset(GPIOB, GPIO_PIN_12); -} -/** - * @brief 占空比置0 - * - */ -void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); } -/** - * @brief 电机锁止 - */ -void FOC_Controlroller::drv_hold(float duty) { - if (duty > 0.95f) { - duty = 0.95f; - } else if (duty < 0.0f) { - duty = 0.0f; - } - Set_3Pwm(duty, duty, duty); -} -/** - * @brief 电机恢复 - * - */ -void FOC_Controlroller::resume() { - thermal_uncontrol = false; - voltage_error = false; - is_shut_down = false; - gpio_bit_set(GPIOB, GPIO_PIN_12); -} -/** - * @brief 更新电源电压 - * - */ -void FOC_Controlroller::update_power(void) { - voltage_power_supply = sampler.get_vbus_voltage(); - if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) { - voltage_error = false; - v_half = voltage_power_supply * 0.5f; - v_safe = voltage_power_supply * 0.95f; - } else { - voltage_error = true; - } -} - -/** - * @brief 速度开环控制 - * - * @param target_velocity: 目标速度 - * @return float: q轴电压 - */ -float FOC_Controlroller::velocity_open_loop() { - // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 - // 预计算常量 1e-4 = 100微秒控制周期 - shaft_angle = - _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f); - - // 因为在开环控制中，Ud始终为0，只控制Uq - const float Uq = voltage_power_supply / 2.0f; - - setPhaseVoltage(Uq, 0.0f); - - return limit_voltage; -} - -/** - * @brief 电机初始化 - * - * @param _PP: 电机极队数 - */ -// void FOC_Controlroller::DFOC_alignSensor(int _PP) { -// u8 PP = _PP; -// setPhaseVoltage(3, 0, _3PI_2); -// delay_1ms(3000); -// zero_electric_angle = _electricalAngle(); -// setPhaseVoltage(0, 0, _3PI_2); -// std::printf("0电角度:%f", zero_electric_angle); -// } - -/** - * @brief 角度闭环控制 - * - * @param target_angle: 目标角度 - * @return float: 当前角度 - */ -float FOC_Controlroller::angle_control(float target_angle) { - // PID 控制计算 - float Uq = angle_loop(target_angle - rt_angle); - - // 获取电机轴角度 - 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; -} - -/** - * @brief 速度闭环控制 - * - * @param target_velocity: 目标速度 - * @return float: 当前速度 - */ -float FOC_Controlroller::velocity_control(float target_velocity) { - // PID 控制计算 - float Uq = vel_loop(target_velocity - rt_velocity); - - // 获取电机轴角度 - shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; - // 设置相电压 - setPhaseVoltage(Uq, 0.0f); - return rt_velocity; -} \ No newline at end of file diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h deleted file mode 100644 index 446c891..0000000 --- a/Robot_V2/Algorithm/DengFOC.h +++ /dev/null @@ -1,410 +0,0 @@ -// 函数声明 -#ifndef _DENGFOC_H -#define _DENGFOC_H -#include "pid.h" -#include - -class FOC_Controlroller { - - public: - FOC_Controlroller(); - - u8 control_mode = 0; - float limit_voltage = 0.0f; - float limit_velocity = 0.0f; - float target_angle = 0.0f; - float target_velocity = 0.0f; - float rt_angle = 0.0f; - float rt_velocity = 0.0f; - bool thermal_uncontrol = false; - bool voltage_error = false; - bool is_shut_down = false; - float voltage_power_supply = 24.0f; // 电源电压 - - float velocity_open_loop(); - float angle_control(float target_angle); - float velocity_control(float target_velocity); - float angle_control_NPID(float target_angle); - - 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); - - 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 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 - - void setPhaseVoltage(float Uq, float Ud); -}; - -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); - -__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/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp new file mode 100644 index 0000000..e2e875e --- /dev/null +++ b/Robot_V2/Algorithm/FOC.cpp @@ -0,0 +1,293 @@ +#include "FOC.h" +#include "Can.h" +#include "Filter.h" +#include "Timer.h" +#include "adc.h" +#include "ang_sensor.h" +#include "config.h" +#include "motor.h" +#include "pid.h" +#include +#include +#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) \ + ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) +#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, 360.0f); + // 取余运算可以用于归一化，列出特殊值例子算便知 + return a >= 0 ? a : (a + 360.0f); +} + +// 通过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); + + float I_alpha, I_beta; + // Clarke变换 (使用三相电流) + // 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; + 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 - + KE_FACTOR * rt_velocity; // 返回Iq值，乘以100以转换为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; +} + +/** + * @brief 电机控制类构造函数 + * + */ +FOC_Controlroller::FOC_Controlroller() { ; } + +/** + * @brief 设置电机输出，使用优化的SVPWM算法 + * + * @param Uq: q轴电压 + * @param Ud: d轴电压 + * @param angle_el: 电机轴角度 + */ +inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { + // 计算电气角度 + float angle_el = _normalizeAngle(shaft_angle * PP); + + // 帕克逆变换 (直接计算，避免函数调用开销) + float sin_el, cos_el; + arm_sin_cos_f32(angle_el, &sin_el, &cos_el); + float Ualpha = Ud * cos_el - Uq * sin_el; + float Ubeta = Ud * sin_el + Uq * cos_el; + + // 克拉克逆变换 (使用预计算常量优化) + Ua = Ualpha + v_half; + Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half; + Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half; + + // 计算占空比并限制范围 + 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; + 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); +} + +/** + * @brief 电机关机 + * + */ +void FOC_Controlroller::shut_down() { + is_shut_down = 1; + motor_sleep(); +} +/** + * @brief 占空比置0 + * + */ +void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); } +/** + * @brief 电机锁止 + */ +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(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty); + return hold_pwm_duty; +} +/** + * @brief 电机恢复 + * + */ +void FOC_Controlroller::resume() { + over_current = false; + voltage_error = false; + is_shut_down = false; + motor_resume(); +} +/** + * @brief 更新电源电压 + * + */ +void FOC_Controlroller::update_power(void) { + voltage_power_supply = sampler.get_vbus_voltage(); + if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) { + voltage_error = false; + v_half = voltage_power_supply * 0.5f; + v_safe = voltage_power_supply * 0.95f; + } else { + voltage_error = true; + } +} + +/** + * @brief 速度开环控制 + * + * @param target_velocity: 目标速度 + * @return float: q轴电压 + */ +float FOC_Controlroller::velocity_open_loop() { + // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 + 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 = shaft_angle; + // cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + // sampler.get_current('W')); + + return limit_voltage; +} + +/** + * @brief 电机初始化 + * + * @param _PP: 电机极队数 + */ +// void FOC_Controlroller::DFOC_alignSensor(int _PP) { +// u8 PP = _PP; +// setPhaseVoltage(3, 0, _3PI_2); +// delay_1ms(3000); +// zero_electric_angle = _electricalAngle(); +// setPhaseVoltage(0, 0, _3PI_2); +// std::printf("0电角度:%f", zero_electric_angle); +// } + +/** + * @brief 角度闭环控制 + * + * @param target_angle: 目标角度 + * @return float: 当前角度 + */ +float FOC_Controlroller::angle_control() { + // PID 控制计算 + float Uq = angle_loop(target_angle - rt_angle); + + // 获取电机轴角度 + shaft_angle = KTH7823.getcurrent_sensorangle(); + // 设置相电压 + setPhaseVoltage(Uq, 0.0f); + return shaft_angle; +} + +/** + * @brief 带速度限制的角度闭环控制 + * + * @param target_angle: 目标角度 + * @param max_velocity: 最大速度限制 (单位: rad/s) + * @return float: 当前角度 + */ +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); + + // 使用速度PID控制器计算最终输出电压 + float Uq = vel_inVelAngloop(rt_velocity - target_velocity); + CAN0_transmiter.testfloat_union.f = shaft_angle; + + // 获取电机轴角度 + shaft_angle = KTH7823.getcurrent_sensorangle(); + + // 设置相电压 + setPhaseVoltage(Uq, 0.0f); + + return shaft_angle; +} + +/** + * @brief 速度闭环控制 + * + * @param target_velocity: 目标速度 + * @return float: 当前速度 + */ +float FOC_Controlroller::velocity_control() { + // PID 控制计算 + float Uq = vel_loop((rt_velocity - target_velocity) * 25); + + // 获取电机轴角度 + 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')); + + return rt_velocity; +} + +float FOC_Controlroller::current_control() { + // 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(); + // 设置相电压 + setPhaseVoltage(Uq, 0.0f); + return 1; +} \ No newline at end of file diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h new file mode 100644 index 0000000..9c40f1c --- /dev/null +++ b/Robot_V2/Algorithm/FOC.h @@ -0,0 +1,77 @@ +// 函数声明 +#ifndef _FOC_H +#define _FOC_H +#include "config.h" +#include "pid.h" +#include + +class FOC_Controlroller { + + public: + FOC_Controlroller(); + + // 初始化函数 + u8 init(); + + u8 control_mode = 0; + float limit_voltage = 0.0f; + 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; + bool voltage_error = false; + bool is_shut_down = false; + float voltage_power_supply = 24.0f; // 电源电压 + 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; + 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); + 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}; + + 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; + float zero_electric_angle = 0; + float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 + void setPhaseVoltage(float Uq, float Ud); + float cal_Iq_Id(float current_a, float current_b, float current_c); +}; + +// 定义函数指针类型 +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/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index c234abf..4a5f776 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -1,14 +1,15 @@ #include "Filter.h" #include "Can.h" #include "ang_sensor.h" -#include "gd32c10x_can.h" +#include "config.h" #include #include -#include extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; -constexpr u8 SIZE_FLOAT = sizeof(float); +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 中值滤波器函数 @@ -28,57 +29,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类的实现 /*! \brief 构造函数，初始化编码器速度卡尔曼滤波器参数 @@ -109,7 +59,7 @@ float EncoderVelocityKalman::update(float measured_velocity) { /*! \brief 获取当前速度估计值 - \retval 当前速度估计值 (rad/s) + \retval 当前速度估计值 (deg/s) */ float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); } @@ -148,24 +98,20 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle, measurement_noise), last_angle(initial_angle), initialized(true) {} - /*! \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. 预测步骤 - 假设角度符合随机游走模型（角速度为0） - // 由于角度变化相对缓慢，我们可以简单预测角度保持不变 - kalman.predict(kalman.getState()); + // 1. 预测步骤 - 速度估计 + kalman.predict(last_angle); // 2. 更新步骤 - 使用处理后的角度值更新 float filtered_angle = kalman.update(measured_angle); - - return filtered_angle; } @@ -178,21 +124,20 @@ 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 { + 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; +} /*! \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 9d8dbc5..a0b3fb8 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,9 +13,8 @@ extern "C" { #endif #ifdef __cplusplus -#include #include -#include +#include float medium_filter(float values[], u8 size); @@ -26,12 +24,12 @@ float medium_filter(float values[], u8 size); */ template class KalmanFilter { private: - T x; // 状态估计 T P; // 估计误差协方差 T Q; // 过程噪声协方差 T R; // 测量噪声协方差 public: + T x; // 状态估计 /*! \brief 构造函数 \param[in] initial_state 初始状态值 @@ -105,35 +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 编码器速度卡尔曼滤波器类 \note 用于滤除编码器速度测量噪声的卡尔曼滤波器实现 @@ -154,6 +123,11 @@ class EncoderVelocityKalman { void setProcessNoise(float q); void setMeasurementNoise(float r); + + u8 init(float init_velocity) { + kalman.x = init_velocity; + return 0; + } }; /*! @@ -161,25 +135,28 @@ class EncoderVelocityKalman { \note 用于滤除编码器角度测量噪声的卡尔曼滤波器实现 */ class EncoderAngleKalman { -private: + private: KalmanFilter kalman; - float last_angle; bool initialized; + float last_angle; -public: + public: EncoderAngleKalman(float initial_angle, float initial_error_covariance, float process_noise, float measurement_noise); float update(float measured_angle); float getAngle() const; - float getlastAngle() const; float getVelocity() const; - void setProcessNoise(float q); + float getstate() const { return kalman.getState(); }; void setMeasurementNoise(float r); + u8 init(float init_angle) { + kalman.x = init_angle; + return 0; + } }; #endif // __cplusplus diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 61595c1..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; @@ -18,25 +19,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 +60,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 new file mode 100644 index 0000000..59dfecf --- /dev/null +++ b/Robot_V2/Algorithm/pid.h @@ -0,0 +1,25 @@ +#ifndef PID_H +#define PID_H + +class PIDController { + public: + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + 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; //!< 上次执行时间戳 +}; +#endif \ No newline at end of file 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/gd32c10x_fmc.c b/Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c index 3a3eae2..7f29736 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) @@ -988,7 +989,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/RTE/Device/GD32C103CB/regions_GD32C103CB.h b/Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h index 89512d0..54b2c27 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 0x08012000 // 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..9142dca 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)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 */ /* 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 f92e443..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.11.0 + * Generated by: csolution version 2.12.0+p41-gfeadfc5e * * 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 ce727a6..ab6bd24 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -12,6 +12,9 @@ project: - file: sys/initial.h - file: sys/health.h - file: sys/health.cpp + - file: sys/flash.h + - file: sys/flash.cpp + - file: sys/config.h - group: device files: - file: device/Can.cpp @@ -23,22 +26,26 @@ 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 - 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: pid.h + - 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 @@ -46,15 +53,18 @@ 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 + - component: Device:GD32C10x_StdPeripherals:CRC + - component: Device:GD32C10x_StdPeripherals:RTC + - component: CMSIS:DSP output: type: - elf - hex + - bin - map packs: - pack: ARM::CMSIS @@ -66,11 +76,16 @@ project: - -fshort-wchar - -D__MICROLIB - -Wno-register + - -O3 # 添加优化级别 + - -DARM_MATH_CM4 # Cortex-M4核心定义 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 + # 移除了 __TARGET_FPU_VFP，因为它不是链接器参数 diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 32017f1..2ea86bf 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -1,27 +1,22 @@ #include "adc.h" #include "Can.h" +#include "FOC.h" #include "Filter.h" +#include "config.h" #include "math.h" +#include "systick.h" #include #include -#include +#include -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; -sampler sampler; // 采样器初始化 - +extern FOC_Controlroller FOC; +sampler sampler; // 采样器初始化 u16 adc_value[6] = {0}; // 显式初始化数组元素为0 /*! \brief configure the DMA peripheral @@ -109,11 +104,12 @@ 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++) { + 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; @@ -129,7 +125,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温度 @@ -137,7 +135,7 @@ sampler::sampler() { * 温度值，单位为摄氏度，范围应在-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; @@ -153,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; @@ -171,15 +169,18 @@ float sampler::get_drv_temp() { float sampler::get_current(char type) { switch (type) { case 'U': { - tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1); + // return currU_samples[current_sample_index]; + tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1); break; } case 'V': { - tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1); + // return currV_samples[current_sample_index]; + tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1); break; } case 'W': { - tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1); + // return currW_samples[current_sample_index]; + tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1); break; } default: { @@ -189,6 +190,7 @@ float sampler::get_current(char type) { if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) { return ERROR_ADC_VAL; } + return tempora_sample; } @@ -200,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; @@ -217,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; } @@ -227,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; } @@ -238,6 +240,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; @@ -248,15 +251,17 @@ 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; } + // 卡尔曼滤波更新 + float voltage = FOC.voltage_power_supply; } /** * @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 28647eb..648afc3 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -1,31 +1,33 @@ #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" #include "gd32c10x_dma.h" +#include "gd32c10x_fmc.h" +#include "gd32c10x_gpio.h" +#include "initial.h" +#include "motor.h" #include "systick.h" #include #include #include #include -#include -#include -#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初始化 +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; +extern VersionManager Version; // 声明外部版本管理器实例 // 添加DMA相关变量 dma_parameter_struct dma_rx_parameter; @@ -50,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; } @@ -152,8 +156,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); @@ -168,6 +175,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; @@ -230,6 +238,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); } /*! @@ -260,7 +271,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 +424,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,145 +437,32 @@ 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; } /*! \brief CAN 数据更新 */ 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; 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) == SET) ? 0 : 1; - + MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET; + EEPROMStatus = flashEEP.EEPROMStatus; 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(); -} - -/*! - \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) { - // 没有可用的mailbox，可能所有mailbox都在使用中 - printf("No available mailbox for CANFD transmission\r\n"); - return 0; - } - return 1; } /** @@ -581,31 +479,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; } @@ -618,32 +524,31 @@ 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; - // 打包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)); - pack_u16_to_bytes(transmit_data, 6, u16(w_current_union.f)); - pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f)); - transmit_data[10] = (transmit_data[10] & ~0x07) | - ((MotorStatus & 0x01) | - ((MagStatus & 0x01) << 1) | - ((ThermalStatus & 0x01) << 2)); - + 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) | ((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); 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; } @@ -657,9 +562,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; } @@ -669,32 +574,114 @@ 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 == 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; - can0_receive_flag = RESET; + // 解析PID参数 + u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15); + 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 / voltage); + 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); + } 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(); + } + // 系统参数保存 + 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(); + 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); } + can0_receive_flag = RESET; } } +/** + * @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 无 @@ -741,7 +728,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++) { @@ -756,7 +742,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 < 25) { + valid_length = 24; + } else if (length > 24 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 56dc00a..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 { @@ -53,13 +51,14 @@ 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发送) +void jumpToBOOT(); void can_loopback_init(void); void can_loopback_test(void); @@ -71,46 +70,52 @@ 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; // 目标控制模式 - 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 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状态 u8 transmit_data[64]; // CANFD最大64字节 - 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_all_Send(); // CANFD发送 + void update(); // 更新数据 + u8 CAN_Receive(); // CAN接收 + u8 CANFD_Receive(); // CANFD接收 public: // 添加构造函数声明 - CAN_transmiter(); + 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进入睡眠 + 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/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 431df26..5838bdd 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" @@ -8,31 +8,14 @@ #include "gd32c10x_rcu.h" #include "gd32c10x_timer.h" #include "initial.h" -#include -#include -#include -#include -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 -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%位置触发中断 - -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 设置引脚中断优先级 @@ -45,11 +28,11 @@ 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); } /** - * @brief 配置定时器2 + * @brief 配置定时器2-任务管理 * @return void * *配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务 @@ -90,7 +73,7 @@ void TIMER2_config(void) { } /** - * @brief 配置TIMER2中断 + * @brief 配置TIMER2-任务管理中断 * @return void */ void User_timer_init(void) { @@ -100,16 +83,8 @@ 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中断处理函数 + * @brief TIMER2-任务管理中断处理函数 * @return void */ void TIMER2_IRQHandler(void) { @@ -117,67 +92,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: { - CAN0_transmiter.can0_send_flag = SET; - 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执行的代码 + } } } } @@ -186,7 +169,7 @@ void TIMER2_IRQHandler(void) { } /** - * @brief TIMER1 GPIO配置函数 + * @brief TIMER1-PWM GPIO配置函数 */ void timer1_gpio_config(void) { // 时钟使能 @@ -210,7 +193,7 @@ void timer1_gpio_config(void) { } /** - * @brief TIMER1 配置函数 + * @brief TIMER1-PWM 配置函数 */ void timer1_pwm_config(void) { timer_parameter_struct timer_initpara; @@ -254,11 +237,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); @@ -274,12 +257,12 @@ void timer1_pwm_config(void) { timer_enable(TIMER1); } -// 添加TIMER1中断处理函数 +// 添加TIMER1-PWM中断处理函数 /** * @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/Timer.h b/Robot_V2/device/Timer.h index ed220de..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); @@ -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 9c126e2..3283034 100644 --- a/Robot_V2/device/adc.h +++ b/Robot_V2/device/adc.h @@ -5,23 +5,22 @@ 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; 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 a82d181..a8f0056 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -1,34 +1,30 @@ #include "ang_sensor.h" +#include "Can.h" #include "Filter.h" -#include "core_cmFunc.h" +#include "config.h" +#include "flash.h" #include "gd32c10x_dma.h" #include "gd32c10x_gpio.h" #include "gd32c10x_spi.h" -#include "initial.h" #include #include #include +#define ANG_SENS_DEBUG 1 - -#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) -#define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15) - -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); +#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) // DMA传输完成标志 volatile bool spi_dma_tx_complete = false; volatile bool spi_dma_rx_complete = false; angle_sensor KTH7823; // 角度传感器初始化 -EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f); -EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f); +extern FlashEEPROM flashEEP; +extern EncoderAngleKalman Angle_kalman; +extern EncoderVelocityKalman Vel_kalman; +extern CAN_transmiter CAN0_transmiter; // CAN初始化 /** * @brief SPI DMA initialization @@ -194,47 +190,52 @@ void KTH7823_spi_init(void) { } /** - * @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初始化 + return 0; } /** - * @brief 角度传感器构造函数 + * @ 角度传感器类 设置当前角度 + * @param angle 输出级角度值 + * @return 0成功/1失败 */ -angle_sensor::angle_sensor(void) { KTH7823_init(); } +u8 angle_sensor::angle_set(float angle) { + m_cumulativeAngle = angle * REDUCTION_RATIO; // 从Flash读取上次保存的角度 + m_firstUpdate = true; + 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 * ANGLESENS_FACTOR + ANGLESENS_OFFSET; + current_angle_deg = angle_deg; + current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f + return angle_deg; + } + + // 仅在调试模式下输出错误，避免发布版本中的I/O开销 + if (result != 0) { + // 使用更高效的串口输出替代printf（如果可用） printf("Error reading angle from sensor.\n"); - } - #endif - - return (float)FAULT_RECEIVE; + } + + return (float)FAULT_RECEIVE; } /** * @brief 更新传感器角度（输出级） @@ -253,7 +254,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; @@ -262,9 +263,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 +283,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 角度值(原始级) @@ -312,11 +290,3 @@ float angle_sensor::getRawVelocity(void) const { 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/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index c29a23c..2503b12 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -4,28 +4,26 @@ 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); 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: - 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; + 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/device/motor.cpp b/Robot_V2/device/motor.cpp new file mode 100644 index 0000000..1dffc51 --- /dev/null +++ b/Robot_V2/device/motor.cpp @@ -0,0 +1,52 @@ +#include "motor.h" +#include "Timer.h" +#include "gd32c10x_eval.h" +#include "gd32c10x_gpio.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); + + // 使能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); +} + +/** + * @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); + // 同时禁用电机驱动器 + gpio_bit_reset(MOTOR_EN_GPIO_PORT, MOTOR_EN_GPIO_PIN); +} + +/** + * @brief 电机启动 + */ +void motor_resume(void) { + gpio_bit_set(GPIOB, GPIO_PIN_12); + // 同时启用电机驱动器 + gpio_bit_set(MOTOR_EN_GPIO_PORT, MOTOR_EN_GPIO_PIN); +} \ No newline at end of file diff --git a/Robot_V2/device/motor.h b/Robot_V2/device/motor.h new file mode 100644 index 0000000..d4c74c1 --- /dev/null +++ b/Robot_V2/device/motor.h @@ -0,0 +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 0468896..ba0b5af 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -1,100 +1,125 @@ #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 "sys/health.h" -#include "sys/systick.h" +#include "motor.h" #include "systick.h" +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类 +extern VersionManager Version; + +// 全局版本管理器实例 TaskScheduler scheduler = {0}; - +volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次 +volatile u32 cnt_test = 0; // 负载检测 int main(void) { - systick_config(); // 系统时钟初始化 - USART0_init(); // 调试串口初始化 - User_timer_init(); // Timer初始化 - driver_init(); // 电机驱动初始化 - extern FOC_Controlroller FOC; // FOC控制 - extern sampler sampler; // 采样器 - extern EncoderVelocityKalman Vel_kalman; // 速度滤波 - extern EncoderAngleKalman Angle_kalman; // 角度滤波 - extern angle_sensor KTH7823; // 角度传感器 - extern CAN_transmiter CAN0_transmiter; // CAN - extern ThermalController thermalController; // 热量控制器 + u8 init_status = hardware_init(); + + while (init_status) { // 初始化检测 + std::printf("初始化失败\r\n"); + } + OTA_initial(); // OTA初始化,A/B分区处理 + + angsens_MG_check(); // 角度传感器磁感应强度测试 + watchdog_init(50); // 看门狗初始化 - delay_1ms(5); - 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(); - // 按照优先级和重要性处理任务 - 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)) { // 电机控制任务(最高优先级) - switch (FOC.control_mode) { - case 0: { - FOC.drv_hold(0.95); // 占空比置0 - break; - } - case 1: { - FOC.angle_control(FOC.target_angle); // 角度闭环控制 - break; - } - case 2: { - FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 - break; - } - case 3: { - FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 - 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电压值 - sampler.update_vbus_voltage(); - FOC.update_power(); - } - if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 - CAN0_transmiter.communication_check(); - fwdgt_counter_reload(); // 看门狗复位 + do_task(currentTasks); + } + return 0; +} + +u8 hardware_init(void) { + __enable_irq(); + systick_config(); // 系统时钟初始化 + unused_pin_init(); // 未使用引脚初始化 + USART0_init(); // 调试串口初始化 + 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初始化 + init_status += FOC.init(); // FOC初始化 + return init_status; +} + +void do_task(u32 currentTasks) { + cnt_test++; + // 按照优先级和重要性处理任务 + 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)) { // 电机控制任务(最高优先级) + motor_runtime_50us++; + + // 函数指针数组，包含各种控制模式对应的函数 + 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) { // 更新为7个控制模式 + // 调用对应的控制函数 + (FOC.*control_functions[FOC.control_mode])(); } } + 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..ab1c246 100644 --- a/Robot_V2/main.h +++ b/Robot_V2/main.h @@ -34,6 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MAIN_H #define MAIN_H +#include +void do_task(u32 currentTasks); +u8 hardware_init(void); #endif /* MAIN_H */ diff --git a/Robot_V2/pid.h b/Robot_V2/pid.h deleted file mode 100644 index 695b51d..0000000 --- a/Robot_V2/pid.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef PID_H -#define PID_H - -class PIDController -{ -public: - PIDController(float P, float I, float D, float ramp, float limit); - ~PIDController() = default; - - float operator() (float error); - void set_PID(float P, float I, float D); - - 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/sys/config.h b/Robot_V2/sys/config.h new file mode 100644 index 0000000..9a4e872 --- /dev/null +++ b/Robot_V2/sys/config.h @@ -0,0 +1,135 @@ +#ifndef __CONFIG_H +#define __CONFIG_H +#include + +// 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 +#define ARM_MATH_CM4 + +#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 = 24; +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 = 0x6000; // 24KB +constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT起始地址 +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 = 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 = 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.0f; +constexpr u16 REDUCTION_RATIO = 25; +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 * 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); +constexpr u8 SIZE_U32 = sizeof(uint32_t); +constexpr float ADC_MAX = 65536.0f; +constexpr float PI = 3.14159265358979323846f; +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; +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; +// 反馈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; +constexpr u32 CANFD_OTA_WAIT_ID = 0x00000103; + +// 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 = 49; // 1MHz / 50 = 50us +constexpr float TIMER2_FREQUENCY = 1 / 5e-5; + +// 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 new file mode 100644 index 0000000..a03d875 --- /dev/null +++ b/Robot_V2/sys/flash.cpp @@ -0,0 +1,795 @@ +#include "flash.h" +#include "FOC.h" +#include "ang_sensor.h" +#include "config.h" +#include "gd32c10x.h" // GD32主头文件，确保外设函数声明可用 +#include "gd32c10x_crc.h" +#include "gd32c10x_fmc.h" +#include "systick.h" +#include +#include +#include + +FlashEEPROM flashEEP = FlashEEPROM(); +extern angle_sensor KTH7823; +extern CAN_transmiter CAN0_transmiter; +extern FOC_Controlroller FOC; // FOC控制 + +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() { + // 构造函数可以为空，或者做一些初始化工作 +} + +/** + * @brief 初始化Flash EEPROM模拟 + * @param None + * @retval None + */ +u8 FlashEEPROM::init() { + rcu_periph_clock_enable(RCU_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[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]); + 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 < CONFIG_SAVED_NUM + 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_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; // 上一个位置为有效存储的数据 + 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, CONFIG_SAVED_NUM); + +#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 < CONFIG_SAVED_NUM; i++) { + data[i] = DEFAULT_VALUE; + } + EEPROMStatus = FLASH_EEPROM_CRC_ERROR; + } else { // CRC正确 + EEPROMStatus = FLASH_EEPROM_OK; + } + } else { + // 读取失败，报错 + EEPROMStatus = status_read[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; +} + +/** + * @brief 从Flash EEPROM读取数据 + * @param virtualAddr: 虚拟地址 + * @param data: 指向存储读取数据的指针 + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { + // 参数检查 + if (data == nullptr) { + 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 & DATA_ADDR_MASK); + // 找到匹配的虚拟地址 + if (addr == virtualAddr) { + *data = (u16)(data_word & DATA_U16_MASK); + return FLASH_EEPROM_OK; + } + } + } + return FLASH_EEPROM_DATA_NOT_FOUND; +} + +/** + * @brief 从Flash EEPROM读取float数据 + * @param virtualAddr: 虚拟地址 + * @param data: 指向存储读取数据的指针 + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const { + // 检查参数有效性 + if (data == nullptr) { + return FLASH_EEPROM_INVALID_ADDR; + } + + // 从Flash末尾开始向前搜索，找到最新的数据 + for (u32 address = EEPROM_END_ADDRESS - SIZE_U32; + address >= EEPROM_START_ADDRESS; address -= SIZE_U32) { + // 使用volatile确保每次都从内存读取，防止编译器优化 + volatile u32 *flash_ptr = (volatile u32 *)address; + u32 data_word = *flash_ptr; + + // 检查是否是有效数据（非擦除状态） + if (data_word != 0xFFFFFFFF) { + u8 addr = (u8)(data_word & DATA_ADDR_MASK); + // 找到匹配的虚拟地址 + if (addr == virtualAddr) { + // 清除最低的8位地址信息，保留高24位数据 + u32 float_data = data_word & DATA_FLOAT_MASK; + // 使用内存拷贝将处理后的数据解释为float + memcpy(data, &float_data, SIZE_FLOAT); +#if EEP_DEBUG + printf("read:0x%x=%f\r\n", address, *data); +#endif + return FLASH_EEPROM_OK; + } + } + } + // 未找到指定虚拟地址的数据 + return FLASH_EEPROM_DATA_NOT_FOUND; +} + +/** + * @brief 向Flash EEPROM写入数据 + * @param virtualAddr: 虚拟地址 + * @param data: 要写入的数据 + * @retval FlashEepromStatus: 操作状态 + */ +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 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); +#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 = *flash_ptr; + // 找到空位置 + if (data_word == 0xFFFFFFFF) { + next_address = address; + break; + } + // 更新下一个可能的写入位置（如果需要格式化） + next_address = address + SIZE_U32; + } + + // 如果有足够空间，直接写入 + if (next_address < EEPROM_END_ADDRESS) { + u32 data_word = ((u32)data << 16) | (u32)virtualAddr; + + 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 EEP_DEBUG + printf("no enough space\r\n"); +#endif + FlashEepromStatus status = format(); + if (status == FLASH_EEPROM_OK) { + status = save_system_config(false); + return status; + } else { + return FLASH_EEPROM_ERASE_ERROR; + } + } +} + +/** + * @brief 向Flash EEPROM写入float数据 + * @param virtualAddr: 虚拟地址 + * @param data: 要写入的数据 + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) { + // 查找下一个可写入的位置 + u32 next_address = EEPROM_START_ADDRESS; + u32 data_word; + + // 将完整的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_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 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); +#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_read == 0xFFFFFFFF) { + next_address = address; + break; + } + // 更新下一个可能的写入位置（如果需要格式化） + next_address = address + SIZE_U32; + } + + // 如果有足够空间，直接写入 + if (next_address < EEPROM_END_ADDRESS) { + 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); + 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 EEP_DEBUG + printf("no enough space\r\n"); +#endif + FlashEepromStatus status = format(); + if (status == FLASH_EEPROM_OK) { + status = save_system_config(false); + return status; + } else { + return FLASH_EEPROM_ERASE_ERROR; + } + } +} + +/** + * @brief 格式化Flash EEPROM页面 + * @param None + * @retval FlashEepromStatus: 操作状态 + */ +FlashEepromStatus FlashEEPROM::format() { + fmc_state_enum fmc_status; +#if EEP_DEBUG + printf("擦除页面\r\n"); +#endif + + fmc_unlock(); + fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); // 擦除第一页 + if (fmc_status != FMC_READY) { + fmc_lock(); + return FLASH_EEPROM_ERASE_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; +} + +/** + * @brief 更新系统配置并保存到Flash + * @param config 新的系统配置值 + */ +FlashEepromStatus FlashEEPROM::save_system_config(bool saved) { + u32 CRC_NUM = 0x0000; + 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) { + 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[CONFIG_SAVED_NUM]; + + // 按照writeFloat函数的方式处理数据 + for (u8 i = 0; i < CONFIG_SAVED_NUM; 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, CONFIG_SAVED_NUM); + writeCRC(CRC_NUM, saved); + + EEPROMStatus = FLASH_EEPROM_OK; + return FLASH_EEPROM_OK; +} + +/** + * @brief 读取角度数据从Flash + * @return float: 角度数据 + */ +float FlashEEPROM::read_angle() { + float angle = DEFAULT_VALUE; + if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) { + angle = DEFAULT_VALUE; + } + return angle; +} + +/** + * @brief 保存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_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; + return FLASH_EEPROM_WRITE_ERROR; + } + } else { + // 空间不足，需要格式化页面 +#if EEP_DEBUG + 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; + } +} + +/** + * @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) { +#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; + + // 1. 检查目标地址的值是否已与期望值相同，如果相同则无需操作 + temp_val[0] = *flash_ptr; + if (temp_val[0] == data) { + return FLASH_EEPROM_OK; // 值已正确，直接返回成功 + } + + // 2. 尝试直接写入数据。这适用于将1修改为0的情况（因为Flash允许1->0） + if (data == OTA_AVAL) { + fmc_status = write_ADDRESS(addr, data); + // 如果写入成功，则返回成功 + if (fmc_status == FMC_READY) { + return FLASH_EEPROM_OK; + } + // 如果写入失败（例如地址已被写为0），则继续执行后续的擦除-重写流程 + } + + // 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; + +#if EEP_DEBUG + 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]); +#endif + // 4. 擦除包含所有配置参数的Flash页 + fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); + if (fmc_status != FMC_READY) { +#if EEP_DEBUG + printf("OTA_SET - Page erase failed! Status: %d\r\n", fmc_status); +#endif + return FLASH_EEPROM_ERASE_ERROR; + } + + // 5. 重新写入所有之前读取的配置项值（恢复原始状态） + // 注意：不包括当前正在设置的地址，因为它将在最后单独处理 + for (int i = 0; i < OTA_CONFIG_NUM; i++) { + u32 current_addr = n_APP_LATEST_AVAL_ADDR + 4 * i; + 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 %08X failed at " + "0x%08X!status:%d\r\n", + temp_val[i], current_addr, fmc_status); +#endif + return FLASH_EEPROM_WRITE_ERROR; + } + } + } + +#if EEP_DEBUG + // 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_OK; +} + +/** + * @brief 跳转应用处理模块 + * @param 无 + * @retval 无 + */ +void jumpToBOOT() { + // 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大小区域内的数据搬运到0x08005000地址 + * @param None + * @retval fmc_state_enum: 操作状态 + */ +fmc_state_enum move_app_data() { + // 源地址: APPLATEST_START_ADDR (0x08011000) + // 目标地址: APPBACK_START_ADDR (0x08005000) + // 数据大小: 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; // 向上取整 + + for (uint32_t i = 0; i < num_pages; i++) { + 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"); + delay_1ms(1000); +#endif + __enable_irq(); // 重新开启中断 + return fmc_status; + } + } + + // 复制数据 - 按字（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; + + // 预计算源数据的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_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); + + // 检查写入操作是否成功 + if (fmc_status != FMC_READY) { +#if OTA_DEBUG + printf( + "Flash write failed at offset 0x%08X with status: %d\r\n", + offset, fmc_status); + delay_1ms(1000); +#endif + fmc_lock(); + __enable_irq(); // 重新开启中断 + return fmc_status; + } + write_count++; + } + offset += 4; + + // 每写入一定数量的数据后，喂狗一次 + if ((offset % 4096) == 0) { // 每4KB喂狗一次 + fwdgt_counter_reload(); + } + } + + fmc_lock(); + + // 重新开启全局中断 + __enable_irq(); + crc_data_register_reset(); // 确保CRC计算从初始状态开始 + + // 计算目标数据的CRC值 + 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("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); + 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 new file mode 100644 index 0000000..849fb09 --- /dev/null +++ b/Robot_V2/sys/flash.h @@ -0,0 +1,78 @@ +#ifndef __FLASH_H +#define __FLASH_H + +#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用 +#include + +// 修改后 - 强制4字节对齐 +#define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐 + +// EEPROM操作状态 +typedef enum { + FLASH_EEPROM_OK = 0, // 操作成功 + 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; + +// Flash EEPROM类 +class FlashEEPROM { + public: + // EEPROM emulation constants + 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; // 控制模式 + 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_VEL_LIMIT = 0x05; // 角度数据存储地址 + + bool angle_saved = false; + bool system_config_saved = false; + u8 EEPROMStatus; + // 构造函数 + FlashEEPROM(); + + // 初始化Flash EEPROM模拟 + u8 init(); + + // 从Flash EEPROM中读取数据 + FlashEepromStatus read(u8 virtualAddr, u16 *data) const; + // 向Flash EEPROM写入数据 + 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(); + + 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); +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/health.cpp b/Robot_V2/sys/health.cpp index e931133..cd0a76b 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -1,29 +1,42 @@ #include "health.h" +#include "config.h" #include "Can.h" -#include "DengFOC.h" +#include "FOC.h" #include "adc.h" +#include "flash.h" + extern sampler sampler; extern CAN_transmiter CAN0_transmiter; extern FOC_Controlroller FOC; +extern FlashEEPROM flashEEP; 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 +45,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; // 过热停机 @@ -55,4 +88,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_system_config(true); + } else { + FOC.resume(); + } } \ No newline at end of file diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h index 66bbb65..81925b4 100644 --- a/Robot_V2/sys/health.h +++ b/Robot_V2/sys/health.h @@ -1,21 +1,32 @@ #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 +#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; @@ -24,6 +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 0b47d45..a1d8118 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,14 +1,128 @@ #include "initial.h" -#include "Timer.h" -#include "ang_sensor.h" -#include +#include "Can.h" +#include "config.h" +#include "flash.h" +#include "gd32c10x.h" +#include "gd32c10x_fmc.h" +#include "gd32c10x_gpio.h" +#include "gd32c10x_rcu.h" +#include "initial.h" +#include +#include +#include +extern CAN_transmiter CAN0_transmiter; // CAN初始化 +VersionManager Version; -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); +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 */ @@ -32,18 +146,128 @@ u8 watchdog_init(u32 time) { return status; } - -/** - * @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); + usart_data_transmit(EVAL_COM0, (u8)ch); while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE)) ; 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_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_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; +} + +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); + fmc_lock(); + + u8 transmit_data[2] = {0x0A, 0xAC}; + // 确认APP可用 + CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data); + + 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; + + if (back_aval == OTA_NOT_AVAL) { // 如果BACK不可用 +#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 { + printf("check\r\n"); + } +} \ No newline at end of file diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 38ea28c..088ab54 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -4,23 +4,12 @@ #ifdef __cplusplus extern "C" { #endif + +#include "config.h" #include "gd32c10x_eval.h" -#include "stdint.h" -#include +#include +#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 tle5210b_init(void); -u8 USART0_init(void); -u8 watchdog_init(u32 time); -void driver_init(void); // 任务标志定义 typedef enum { TASK_MOTOR_CONTROL = 0, @@ -36,13 +25,71 @@ 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; // 任务标志位(每个位代表一个任务) } 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 diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index cab013a..054b2ad 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -1,117 +1,190 @@ /*! - \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 "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 volatile static uint32_t delay; +extern CAN_transmiter CAN0_transmiter; +extern FlashEEPROM flashEEP; /*! - \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(); - } -} \ No newline at end of file +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(); + } +} + +/*! + \brief prepare system to enter deep sleep mode + \param[in] none + \param[out] none + \retval none +*/ +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(); + + __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 5294ccb..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); @@ -54,6 +54,10 @@ void idle_task_count(void); uint8_t get_cpu_usage(void); /* 系统软复位 */ void system_soft_reset(void); +/* 准备进入低功耗模式 */ +void app_prepare_deep_sleep(bool save_data); +/* 中断唤醒 */ +void EXTI5_9_IRQHandler(void); #ifdef __cplusplus }