cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request 'DRV8313兼容性合并' (#1) from DRV8323 into master

Browse Source 
Reviewed-on: http://192.168.31.10:8418/cbb/Robot_v2/pulls/1
This commit is contained in:
cbb
2026-01-24 01:55:30 +08:00
parent 871e33e10f 1596b2beb0
commit 25e2edfd0c
38 changed files with 2731 additions and 1445 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2.csolution.yml
+2 -3
View File
@@ -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:
Robot_V2/Algorithm/DengFOC.cpp
-227
View File
@@ -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 <cmath>
#include <gd32c10x.h>
#include <stdint.h>
constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
constexpr float _1_SQRT3 = 0.57735026919f;
constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11;
constexpr float DIR = 1.0;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr float zero_electric_angle = 0.0f;
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
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;
}
Robot_V2/Algorithm/DengFOC.h
-410
View File
@@ -1,410 +0,0 @@
// 函数声明
#ifndef _DENGFOC_H
#define _DENGFOC_H
#include "pid.h"
#include <stdint.h>
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
Robot_V2/Algorithm/FOC.cpp
+293
View File
@@ -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 <arm_math.h>
#include <cmath>
#include <gd32c10x.h>
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;
}
Robot_V2/Algorithm/FOC.h
+77
View File
@@ -0,0 +1,77 @@
// 函数声明
#ifndef _FOC_H
#define _FOC_H
#include "config.h"
#include "pid.h"
#include <cstdint>
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
Robot_V2/Algorithm/Filter.cpp
+17 -72
View File
@@ -1,14 +1,15 @@
#include "Filter.h"
#include "Can.h"
#include "ang_sensor.h"
#include "gd32c10x_can.h"
#include "config.h"
#include <algorithm>
#include <cstring>
#include <stdint.h>
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 设置测量噪声协方差
Robot_V2/Algorithm/Filter.h
+15 -38
View File
@@ -1,7 +1,6 @@
#ifndef _FILTER_H
#define _FILTER_H
#include <cstdio>
#ifdef __cplusplus
extern "C" {
#endif
@@ -14,9 +13,8 @@ extern "C" {
#endif
#ifdef __cplusplus
#include <array>
#include <cstring>
#include <stdint.h>
#include <cstdint>
float medium_filter(float values[], u8 size);
@@ -26,12 +24,12 @@ float medium_filter(float values[], u8 size);
*/
template <typename T> class KalmanFilter {
private:
T x; // 状态估计
T P; // 估计误差协方差
T Q; // 过程噪声协方差
T R; // 测量噪声协方差
public:
T x; // 状态估计
/*!
\brief 构造函数
\param[in] initial_state 初始状态值
@@ -105,35 +103,6 @@ template <typename T> 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<float> 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<float> 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
Robot_V2/Algorithm/pid.cpp
+22 -11
View File
@@ -2,7 +2,8 @@
#define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#include "Can.h"
#include "math.h"
#include <algorithm>
#include <cmath>
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;
}
}
/**
* @brief 设置P参数
*
* @param P: P参数
* @return void
*/
void PIDController::set_P_percent(float pp) { this->P = pp; }
Robot_V2/Algorithm/pid.h
+25
View File
@@ -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
Robot_V2/RTE/Device/GD32C103CB/ac6_linker_script.sct.src
+2 -1
View File
@@ -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)
}
Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c
+2 -1
View File
@@ -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{
Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h
+2 -2
View File
@@ -14,10 +14,10 @@
// <o> Base address <0x0-0xFFFFFFFF:8>
// <i> Defines base address of memory region. Default: 0x08000000
// <i> Contains Startup and Vector Table
#define __ROM0_BASE 0x08000000
#define __ROM0_BASE 0x08012000
// <o> Region size [bytes] <0x0-0xFFFFFFFF:8>
// <i> Defines size of memory region. Default: 0x00020000
#define __ROM0_SIZE 0x00020000
#define __ROM0_SIZE 0x0000C000
// </h>
// <h> __ROM1 (unused)
Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c
+2 -1
View File
@@ -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 */
Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h
+7 -1
View File
@@ -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 */
Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h
+7 -1
View File
@@ -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 */
Robot_V2/Robot_V2.cproject.yml
+23 -8
View File
@@ -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，因为它不是链接器参数
Robot_V2/device/ADC.cpp
+33 -28
View File
@@ -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 <cstring>
#include <gd32c10x_dma.h>
#include <stdint.h>
#include <cstdint>
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;
}
Robot_V2/device/Can.cpp
+302 -174
View File
@@ -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 <cstdio>
#include <cstdlib>
#include <cstring>
#include <random>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
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);
}
}
Robot_V2/device/Can.h
+42 -37
View File
@@ -7,8 +7,6 @@ extern "C" {
#endif
#include "gd32c10x_can.h"
#include "stdint.h"
#include <stdio.h>
// 定义一个联合体类型用于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
Robot_V2/device/Timer.cpp
+70 -87
View File
@@ -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 <cstdio>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
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比较中断标志
Robot_V2/device/Timer.h
+2 -6
View File
@@ -5,10 +5,10 @@
extern "C" {
#endif
#include <stdint.h>
#include <cstdint>
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
Robot_V2/device/adc.h
+9 -10
View File
@@ -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);
Robot_V2/device/ang_sensor.cpp
+44 -74
View File
@@ -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 <cstdio>
#include <cstring>
#include <stdint.h>
#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;
}
Robot_V2/device/ang_sensor.h
+4 -6
View File
@@ -4,28 +4,26 @@
extern "C" {
#endif
#include "gd32c10x_eval.h"
#include "stdio.h"
#include <cstring>
#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;
Robot_V2/device/motor.cpp
+52
View File
@@ -0,0 +1,52 @@
#include "motor.h"
#include "Timer.h"
#include "gd32c10x_eval.h"
#include "gd32c10x_gpio.h"
#include <cstdio>
/**
* @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);
}
Robot_V2/device/motor.h
+14
View File
@@ -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
Robot_V2/main.cpp
+105 -80
View File
@@ -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(); // 看门狗复位
}
}
Robot_V2/main.h
+3
View File
@@ -34,6 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef MAIN_H
#define MAIN_H
#include <cstdint>
void do_task(u32 currentTasks);
u8 hardware_init(void);
#endif /* MAIN_H */
Robot_V2/pid.h
-25
View File
@@ -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
Robot_V2/sys/config.h
+135
View File
@@ -0,0 +1,135 @@
#ifndef __CONFIG_H
#define __CONFIG_H
#include <cstdint>
// 如果使用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
Robot_V2/sys/flash.cpp
+795
View File
@@ -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 <Can.h>
#include <cstdio>
#include <cstring>
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;
}
Robot_V2/sys/flash.h
+78
View File
@@ -0,0 +1,78 @@
#ifndef __FLASH_H
#define __FLASH_H
#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用
#include <cstdint>
// 修改后 - 强制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 */
Robot_V2/sys/health.cpp
+61 -15
View File
@@ -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();
}
}
Robot_V2/sys/health.h
+17 -6
View File
@@ -1,21 +1,32 @@
#ifndef _HEALTH_H
#define _HEALTH_H
#include "stdint.h"
#include <cstdint>
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
Robot_V2/sys/initial.cpp
+242 -18
View File
@@ -1,14 +1,128 @@
#include "initial.h"
#include "Timer.h"
#include "ang_sensor.h"
#include <stdint.h>
#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 <cstdint>
#include <cstdio>
#include <cstring>
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");
}
}
Robot_V2/sys/initial.h
+63 -16
View File
@@ -4,23 +4,12 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "config.h"
#include "gd32c10x_eval.h"
#include "stdint.h"
#include <stdio.h>
#include <cstdint>
#include <cstdio>
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
Robot_V2/sys/systick.cpp
+159 -86
View File
@@ -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 <cstdio>
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();
}
}
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正式休眠，进入低功耗待机模式
}
Robot_V2/sys/systick.h
+5 -1
View File
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <cstdint>
/* 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
}