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

基础响应功能的实现

Browse Source
This commit is contained in:
cbb
2025-12-27 19:20:23 +08:00
parent 26e73f451e
commit a25ec5f029
762 changed files with 1165 additions and 3750 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
-290
View File
@@ -1,290 +0,0 @@
#include "DengFOC.h"
#include "Can.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>
#include "motor.h"
constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
constexpr float _1_SQRT3 = 0.57735026919f;
constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11;
constexpr float DIR = 1.0;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr float zero_electric_angle = 0.0f;
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
// 初始变量及函数定义
#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);
}
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
float current_c) {
// 计算电气角度
float angle_el = _normalizeAngle(shaft_angle * PP);
// Clarke变换 (使用三相电流)
float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
(1.0f / 3.0f) * current_c;
float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
// Park变换
float sin_el, cos_el;
cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
float I_q = I_beta * cos_el - I_alpha * sin_el;
return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA
}
/**
* @brief 电机控制类构造函数
*
*/
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;
// 计算占空比并限制范围
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);
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_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 电机锁止
*/
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() {
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中，在此之前，还需要对轴角度归一化
// 预计算常量 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);
// CAN0_transmiter.testfloat_union.f =
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(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: 目标角度
* @param max_velocity: 最大速度限制 (单位: rad/s)
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) {
// 计算角度误差
float angle_error = target_angle - rt_angle;
// 使用角度PID控制器计算目标速度
float target_velocity = ang_inVelAngloop(angle_error);
// 对目标速度进行限幅
target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity);
// 使用速度PID控制器计算最终输出电压
float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
/**
* @brief 角度闭环控制
*
* @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)*25);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
return rt_velocity;
}
float FOC_Controlroller::current_control(float target_current) {
// PID 控制计算
float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
float Uq = current_loop(target_current - Iq);
CAN0_transmiter.testfloat_union.f = Iq;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return 1;
}
Robot_V2/Algorithm/DengFOC.h
-411
View File
@@ -1,411 +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 over_current = false;
bool voltage_error = false;
bool is_shut_down = false;
float voltage_power_supply = 24.0f; // 电源电压
float v_safe = voltage_power_supply * 1.05f;
float duty_a = 0, duty_b = 0, duty_c = 0;
float Ua = 0, Ub = 0, Uc = 0;
float velocity_open_loop();
float angle_control(float target_angle);
float angle_control_with_velocity_limit(float target_angle);
float velocity_control(float target_velocity);
float current_control(float target_current);
float angle_control_NPID(float target_angle);
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:
float shaft_angle = 0;
float Ualpha, Ubeta = 0;
float zero_electric_angle = 0;
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
PIDController vel_loop =
PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V
PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
voltage_power_supply}; // 12V
PIDController current_loop =
PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
PIDController vel_inVelAngloop =
PIDController{0.35f, 5.0f,1e-3, 1000000, voltage_power_supply};
PIDController ang_inVelAngloop =
PIDController{30.0f, 0.0f, 1e-3, 1000000, 999};
void setPhaseVoltage(float Uq, float Ud);
float cal_Iq_Id(float current_a, float current_b, float current_c);
};
float _normalizeAngle(float angle);
inline float _electricalAngle();
constexpr u16 LUT_SIZE = 1024;
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
__attribute__((aligned(4))) const float sin_lut[LUT_SIZE] = {
0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f,
0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f,
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/Filter.cpp
-151
View File
@@ -1,151 +0,0 @@
#include "Filter.h"
#include "Can.h"
#include "ang_sensor.h"
#include <algorithm>
#include <cstring>
#include <stdint.h>
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern u32 system_runtime_us;
EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
constexpr u8 SIZE_FLOAT = sizeof(float);
/*!
\brief 中值滤波器函数
\param[in] values 待滤波的数据数组
\param[in] size 数组大小
\retval 滤波后的中值
\note 使用部分排序优化算法提高效率
*/
float medium_filter(float values[], u8 size) {
// 使用局部数组避免动态内存分配
float sorted[size];
memcpy(sorted, values, size * SIZE_FLOAT);
// 这比完整排序更高效，时间复杂度从O(n log n)降低到O(n)
std::nth_element(sorted, sorted + size / 2, sorted + size);
return sorted[size / 2]; // 返回中值
}
// EncoderVelocityKalman类的实现
/*!
\brief 构造函数，初始化编码器速度卡尔曼滤波器参数
\param[in] initial_velocity 初始速度值 (rad/s)
\param[in] T 控制周期(s)
\param[in] Q 过程噪声协方差(典型值: 1e-4 ~ 1e-2)
\param[in] R 测量噪声协方差(典型值: 速度范围的1%~5%)
\retval none
*/
EncoderVelocityKalman::EncoderVelocityKalman(float initial_velocity, float T,
float Q, float R)
: kalman(initial_velocity, T, Q, R) {}
/*!
\brief 更新卡尔曼滤波器状态
\param[in] measured_velocity 测量的速度值 (rad/s)
\retval 估计的速度值 (rad/s)
\note 实现卡尔曼滤波的预测和更新步骤，假设速度变化符合随机游走模型
*/
float EncoderVelocityKalman::update(float measured_velocity) {
// 1. 预测步骤 - 假设速度符合随机游走模型（加速度为0）
// 由于速度变化缓慢，我们可以简单预测速度保持不变
kalman.predict(kalman.getState());
// 2. 更新步骤 - 直接使用测量值更新
return kalman.update(measured_velocity);
}
/*!
\brief 获取当前速度估计值
\retval 当前速度估计值 (deg/s)
*/
float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); }
/*!
\brief 设置过程噪声协方差
\param[in] q 过程噪声协方差值
\retval none
*/
void EncoderVelocityKalman::setProcessNoise(float q) {
kalman.setProcessNoise(q);
}
/*!
\brief 设置测量噪声协方差
\param[in] r 测量噪声协方差值
\retval none
*/
void EncoderVelocityKalman::setMeasurementNoise(float r) {
kalman.setMeasurementNoise(r);
}
// EncoderAngleKalman类的实现
/*!
\brief 构造函数，初始化编码器角度卡尔曼滤波器参数
\param[in] initial_angle 初始角度值 (degree)
\param[in] initial_error_covariance 初始误差协方差
\param[in] process_noise 过程噪声协方差
\param[in] measurement_noise 测量噪声协方差
\retval none
*/
EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
float initial_error_covariance,
float process_noise,
float measurement_noise)
: kalman(initial_angle, initial_error_covariance, process_noise,
measurement_noise),
last_angle(initial_angle), initialized(true) {}
/*!
\brief 更新卡尔曼滤波器状态
\param[in] measured_angle 测量的角度值 (degree)
\retval 估计的角度值 (degree)
\note 实现卡尔曼滤波的预测和更新步骤
*/
float EncoderAngleKalman::update(float measured_angle) {
// 更新last_angle以支持速度计算
last_angle = kalman.getState();
// 1. 预测步骤 - 速度估计
kalman.predict(last_angle);
// 2. 更新步骤 - 使用处理后的角度值更新
float filtered_angle = kalman.update(measured_angle);
return filtered_angle;
}
/*!
\brief 获取当前角度估计值
\retval 当前角度估计值 (degree)
*/
float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
/*!
\brief 获取当前角度估计值
\retval 当前角度估计值 (degree)
*/
float EncoderAngleKalman::getVelocity() const {
static u32 last_us100_counter = system_runtime_us;
float vel = (kalman.getState() - last_angle) * 1e4 *
(system_runtime_us - last_us100_counter);
last_us100_counter = system_runtime_us;
return vel;
}
/*!
\brief 设置过程噪声协方差
\param[in] q 过程噪声协方差值
\retval none
*/
void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
/*!
\brief 设置测量噪声协方差
\param[in] r 测量噪声协方差值
\retval none
*/
void EncoderAngleKalman::setMeasurementNoise(float r) {
kalman.setMeasurementNoise(r);
}
Robot_V2/Algorithm/Filter.h
-164
View File
@@ -1,164 +0,0 @@
#ifndef _FILTER_H
#define _FILTER_H
#ifdef __cplusplus
extern "C" {
#endif
// C风格函数声明可以放在这里
// 例如: void some_c_function(float* data, int size);
#ifdef __cplusplus
} // extern "C"
#endif
#ifdef __cplusplus
#include <cstring>
#include <stdint.h>
float medium_filter(float values[], u8 size);
/*!
\brief 通用卡尔曼滤波器模板类
\note 可用于各种基于线性系统的状态估计
*/
template <typename T> class KalmanFilter {
private:
T P; // 估计误差协方差
T Q; // 过程噪声协方差
T R; // 测量噪声协方差
public:
T x; // 状态估计
/*!
\brief 构造函数
\param[in] initial_state 初始状态值
\param[in] initial_error_covariance 初始误差协方差
\param[in] process_noise 过程噪声协方差
\param[in] measurement_noise 测量噪声协方差
*/
KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
T measurement_noise)
: x(initial_state), P(initial_error_covariance), Q(process_noise),
R(measurement_noise) {}
/*!
\brief 预测步骤
\param[in] state_prediction 状态预测值
\retval none
*/
void predict(T state_prediction) {
x = state_prediction;
P += Q;
}
/*!
\brief 更新步骤
\param[in] measurement 测量值
\param[in] measurement_prediction 测量预测值
\retval 估计的状态值
*/
T update(T measurement, T measurement_prediction) {
T y = measurement - measurement_prediction; // 测量残差
T S = P + R; // 残差协方差
T K = P / S; // 卡尔曼增益
x += K * y; // 更新状态估计
P = (1 - K) * P; // 更新误差协方差
return x;
}
/*!
\brief 直接更新步骤（适用于状态直接可测的情况）
\param[in] measurement 测量值
\retval 估计的状态值
*/
T update(T measurement) {
T K = P / (P + R); // 卡尔曼增益
x += K * (measurement - x); // 更新状态估计
P = (1 - K) * P; // 更新误差协方差
return x;
}
/*!
\brief 获取当前状态估计值
\retval 当前状态估计值
*/
T getState() const { return x; }
/*!
\brief 设置过程噪声协方差
\param[in] process_noise 过程噪声协方差值
\retval none
*/
void setProcessNoise(T process_noise) { Q = process_noise; }
/*!
\brief 设置测量噪声协方差
\param[in] measurement_noise 测量噪声协方差值
\retval none
*/
void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
};
/*!
\brief 编码器速度卡尔曼滤波器类
\note 用于滤除编码器速度测量噪声的卡尔曼滤波器实现
*/
class EncoderVelocityKalman {
private:
KalmanFilter<float> kalman;
public:
EncoderVelocityKalman(float initial_velocity,
float initial_error_covariance, float process_noise,
float measurement_noise);
float update(float measured_velocity);
float getVelocity() const;
void setProcessNoise(float q);
void setMeasurementNoise(float r);
u8 init(float init_velocity) {
kalman.x = init_velocity;
return 0;
}
};
/*!
\brief 编码器角度卡尔曼滤波器类
\note 用于滤除编码器角度测量噪声的卡尔曼滤波器实现
*/
class EncoderAngleKalman {
private:
KalmanFilter<float> kalman;
bool initialized;
float last_angle;
public:
EncoderAngleKalman(float initial_angle, float initial_error_covariance,
float process_noise, float measurement_noise);
float update(float measured_angle);
float getAngle() const;
float getVelocity() const;
void setProcessNoise(float q);
float getstate() const { return kalman.getState(); };
void setMeasurementNoise(float r);
u8 init(float init_angle) {
kalman.x = init_angle;
return 0;
}
};
#endif // __cplusplus
#endif // _FILTER_H
Robot_V2/Algorithm/pid.cpp
-60
View File
@@ -1,60 +0,0 @@
#include "pid.h"
#define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#include "Can.h"
#include "math.h"
extern CAN_transmiter CAN0_transmiter;
constexpr float TIME_SLICE = 1e-4;
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
,
limit(limit) // PID控制器输出限幅
,
error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
// PID 控制器函数
float PIDController::operator()(float error) {
// P环
float proportional = P * error;
// Tustin 散点积分（I环）
// 添加积分限幅，防止积分饱和
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));
}
// 保存值（为了下一次循环）
integral_prev = integral;
output_prev = output;
error_prev = error;
return output;
}
/**
* @brief 设置PID参数
*
* @param P: P参数
* @param I: I参数
* @param D: D参数
* @return void
*/
void PIDController::set_PID(float P, float I, float D) {
this->P = P;
this->I = I;
this->D = D;
}
Robot_V2/Algorithm/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/Algorithm/thermal.cpp
View File
Robot_V2/Algorithm/thermal.h
-7
View File
@@ -1,7 +0,0 @@
#ifndef THERMAL_H
#define THERMAL_H
#endif
Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h
-2
View File
@@ -15,8 +15,6 @@
*/
#define CMSIS_device_header "gd32c10x.h"
/* 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 */
Robot_V2/Robot_V2.cproject.yml
+2 -3
View File
@@ -32,11 +32,10 @@ project:
- component: Device:GD32C10x_StdPeripherals:RCU
- 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:BKP
- component: Device:GD32C10x_StdPeripherals:CRC
- component: Device:GD32C10x_StdPeripherals:FWDGT
- component: Device:GD32C10x_StdPeripherals:DBG
output:
type:
- elf
Robot_V2/device/ADC.cpp
-274
View File
@@ -1,274 +0,0 @@
#include "adc.h"
#include "Can.h"
#include "DengFOC.h"
#include "Filter.h"
#include "math.h"
#include <cstring>
#include <gd32c10x_dma.h>
#include <stdint.h>
#include "systick.h"
constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
constexpr float VCC_FACTOR = 11.0f;
constexpr float CURRENT_OFFSET = -1.60f;
constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 40.0f;
constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
\brief configure the DMA peripheral
\param[in] none
\param[out] none
\retval none
*/
void dma_config(void) {
/* enable DMA0 clock */
rcu_periph_clock_enable(RCU_DMA0);
/* ADC_DMA_channel configuration */
dma_parameter_struct dma_data_parameter;
/* ADC DMA_channel configuration */
dma_deinit(DMA0, DMA_CH0);
/* initialize DMA single data mode */
dma_data_parameter.periph_addr = (uint32_t)(&ADC_RDATA(ADC0));
dma_data_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
dma_data_parameter.memory_addr = (uint32_t)(adc_value);
dma_data_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dma_data_parameter.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
dma_data_parameter.memory_width = DMA_MEMORY_WIDTH_16BIT;
dma_data_parameter.direction = DMA_PERIPHERAL_TO_MEMORY;
dma_data_parameter.number = 6;
dma_data_parameter.priority = DMA_PRIORITY_HIGH;
dma_init(DMA0, DMA_CH0, &dma_data_parameter);
dma_circulation_enable(DMA0, DMA_CH0);
/* enable DMA channel */
dma_channel_enable(DMA0, DMA_CH0);
}
/*!
\brief configure the ADC peripheral
\param[in] none
\param[out] none
\retval none
*/
void adc_config(void) {
/* enable ADC clock */
rcu_periph_clock_enable(RCU_ADC0);
/* config ADC clock */
rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV4);
/* 复位ADC */
adc_deinit(ADC0);
/* ADC模式配置 */
adc_mode_config(ADC_MODE_FREE);
/* 使能ADC连续转换功能 */
adc_special_function_config(ADC0, ADC_CONTINUOUS_MODE, ENABLE);
/* 使能ADC扫描模式 */
adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE);
/* ADC数据右对齐配置 */
adc_data_alignment_config(ADC0, ADC_DATAALIGN_RIGHT);
/* 使能ADC温度传感器和内部参考电压 */
adc_tempsensor_vrefint_enable();
/* 配置ADC通道长度 */
adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6);
/* 配置ADC规则通道 */
adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16,
ADC_SAMPLETIME_239POINT5);
adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 4, ADC_CHANNEL_5, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_6, ADC_SAMPLETIME_28POINT5);
/* ADC触发配置 */
adc_external_trigger_source_config(ADC0, ADC_REGULAR_CHANNEL,
ADC0_1_EXTTRIG_REGULAR_NONE);
adc_external_trigger_config(ADC0, ADC_REGULAR_CHANNEL, ENABLE);
/* 使能ADC接口 */
adc_enable(ADC0);
/* ADC校准 */
adc_calibration_enable(ADC0);
/* 使能ADC DMA功能 */
adc_dma_mode_enable(ADC0);
/* 使能ADC软件触发 */
adc_software_trigger_enable(ADC0, ADC_REGULAR_CHANNEL);
}
/**
* @brief 采样类初始化
*/
u8 sampler::init() {
dma_config(); // DMA采样ADC初始化
adc_config(); // ADC初始化
delay_1ms(1);
// 初始采样
for (int i = 0; i < FILTER_SIZE + 1; i++) {
mcu_temp_samples[i] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
currU_samples[i] =
(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currV_samples[i] =
(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currW_samples[i] =
(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
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温度
* @return float
* 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
*/
float sampler::get_mcu_temp() {
tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
}
return tempora_sample;
}
/**
* @brief 获取电机驱动温度
* @return float 温度值，单位为摄氏度
*
* 通过热敏电阻分压电路计算温度值，使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
float sampler::get_drv_temp() {
tempora_sample = medium_filter(drv_temp_samples, FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
}
return tempora_sample;
}
/**
* @brief 获取电流值
* @param type 电流类型，'U'表示U相电流，'V'表示V相电流，'W'表示W相电流
* @return float 电流值，单位为安培
*
* 通过ADC采样计算各相电流值
*/
float sampler::get_current(char type) {
switch (type) {
case 'U': {
// return currU_samples[current_sample_index];
tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1);
break;
}
case 'V': {
// return currV_samples[current_sample_index];
tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1);
break;
}
case 'W': {
// return currW_samples[current_sample_index];
tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1);
break;
}
default: {
return ERROR_ADC_VAL;
}
}
if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) {
return ERROR_ADC_VAL;
}
return tempora_sample;
}
/**
* @brief 获取供电电压
* @return float 电压值，单位为伏特
*
* 通过ADC采样计算供电电压值，使用迭代方式确保获取有效的电压读数。
* 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
*/
float sampler::get_vbus_voltage() {
tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
// 检查电压读数是否在有效范围内
if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC or
vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
return ERROR_ADC_VAL;
}
return tempora_sample;
}
/**
* @brief 更新MCU温度
*
* 通过ADC采样计算MCU温度值，使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
void sampler::update_mcu_temp() {
memmove(mcu_temp_samples, mcu_temp_samples + 1,
FILTER_SIZE * sizeof(float));
mcu_temp_samples[FILTER_SIZE] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
}
/**
* @brief 更新MCU温度
*/
void sampler::update_drv_temp() {
memmove(drv_temp_samples, drv_temp_samples + 1,
FILTER_SIZE * sizeof(float));
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[FILTER_SIZE] =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
}
/**
* @brief 更新相电流
*/
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;
currU_samples[idx] = adc_value[2] * scale + offset;
currV_samples[idx] = adc_value[3] * scale + offset;
currW_samples[idx] = adc_value[4] * scale + offset;
// 更新索引
current_sample_index++;
if (current_sample_index >= 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;
}
Robot_V2/device/Can.cpp
+100 -418
View File
@@ -1,14 +1,15 @@
#include "Can.h"
#include "OTA.h"
#include "config.h"
#include "flash.h"
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "systick.h"
#include "main.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <random>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
@@ -17,9 +18,10 @@ constexpr u16 CANX = CAN0;
constexpr u16 DEV_CAN0_ID = 0x0000;
constexpr u16 DEV_CAN0_MASK = 0x0000;
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
CAN_transmiter CAN0_transmiter; // CAN初始化
extern BootloaderSystem sys;
extern OTAHandler ota;
extern FlashEEPROM flashEEP;
// 添加DMA相关变量
@@ -96,65 +98,6 @@ void can_dma_init(void) {
// can_dma_enable(CAN0, CAN_DMA_RECEIVE);
}
/*!
\brief 初始化CAN0 2.0B 模式
\param[in] none
\param[out] none
\retval none
*/
void can_config(void) {
can_gpio_init();
can_dma_init(); // 初始化DMA
can_parameter_struct can_parameter;
can_filter_parameter_struct can_filter;
can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
/* initialize CAN register */
can_deinit(CAN0);
/* initialize CAN parameters */
can_parameter.time_triggered = DISABLE;
can_parameter.auto_bus_off_recovery = DISABLE;
can_parameter.auto_wake_up = DISABLE;
can_parameter.auto_retrans = DISABLE;
can_parameter.rec_fifo_overwrite = DISABLE;
can_parameter.trans_fifo_order = DISABLE;
can_parameter.working_mode = CAN_NORMAL_MODE;
/* configure CAN0 baud rate 1MBps, sample point at 80% */
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
can_parameter.prescaler = 6;
/* initialize CAN */
if (can_init(CAN0, &can_parameter) != SUCCESS) {
while (1)
printf("can init error \r\n");
}
/* initialize filter */
/* configure filter mode */
can_filter.filter_mode = CAN_FILTERMODE_MASK;
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
/* configure filter mask */
can_filter.filter_mask_high = 0x0000;
can_filter.filter_mask_low = 0x0000;
/* select receiver fifo */
can_filter.filter_fifo_number = CAN_FIFO0;
can_filter.filter_enable = ENABLE;
/* configure filter number */
can_filter.filter_number = 0;
can_filter_init(&can_filter);
/* configure CAN0 NVIC */
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);
}
/*!
\brief 初始化CAN0 CANFD模式
\param[in] none
@@ -163,6 +106,7 @@ void can_config(void) {
*/
void canfd_config(void) {
can_gpio_init();
can_dma_init();
can_parameter_struct can_parameter;
can_fdframe_struct can_fd_parameter;
can_fd_tdc_struct can_fd_tdc_parameter;
@@ -225,183 +169,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);
}
/*!
\brief 初始化CAN0回环测试
\param[in] none
\param[out] none
\retval none
*/
void can_loopback_init(void) {
can_gpio_init();
can_parameter_struct can_parameter;
can_filter_parameter_struct can_filter;
/* initialize CAN register */
can_deinit(CANX);
/* initialize CAN */
can_parameter.time_triggered = DISABLE;
can_parameter.auto_bus_off_recovery = DISABLE;
can_parameter.auto_wake_up = DISABLE;
can_parameter.auto_retrans = DISABLE;
can_parameter.rec_fifo_overwrite = DISABLE;
can_parameter.trans_fifo_order = DISABLE;
can_parameter.working_mode = CAN_NORMAL_MODE;
/* baudrate 1Mbps, sample piont at 80% */
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
can_parameter.prescaler = 6;
if (can_init(CANX, &can_parameter) != SUCCESS) {
u16 i = 200;
while (--i > 0) {
delay_1ms(10);
printf("can init error \r\n");
}
system_soft_reset();
}
/* initialize filter */
/* configure filter mode */
can_filter.filter_mode = CAN_FILTERMODE_MASK;
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
/* configure filter ID */
can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U);
can_filter.filter_list_low =
((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2);
/* configure filter mask */
can_filter.filter_mask_high = 0x0000;
can_filter.filter_mask_low = 0x0000;
/* select receiver fifo */
can_filter.filter_fifo_number = CAN_FIFO0;
can_filter.filter_enable = ENABLE;
/* configure filter number */
can_filter.filter_number = 0;
can_filter_init(&can_filter);
}
/*!
\brief CAN0 FD回环测试
\param[in] none
\param[out] none
\retval ErrStatus
*/
void canfd_loopback_test(void) {
can_trasnmit_message_struct transmit_message;
can_receive_message_struct receive_message;
uint32_t timeout = 0xFFFF;
uint8_t transmit_mailbox = 0;
std::mt19937 gen;
std::uniform_int_distribution<int> dis(0, 255);
can_struct_para_init(CAN_FD_FRAME_STRUCT, &transmit_message);
transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID;
transmit_message.tx_efid = 0x00;
transmit_message.tx_ft = CAN_FT_DATA;
transmit_message.tx_ff = CAN_FF_EXTENDED;
transmit_message.tx_dlen = 7;
transmit_message.fd_flag = 1;
transmit_message.fd_brs = 1;
transmit_message.fd_esi = 0;
int i = 0;
while (i++ < transmit_message.tx_dlen) {
transmit_message.tx_data[i] = dis(gen);
std::printf("%x\n", transmit_message.tx_data[i]);
}
/* transmit message */
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
/* waiting for transmit completed */
while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) &&
(0 != timeout)) {
timeout--;
}
timeout = 0xFFFF;
/* waiting for receive completed */
while ((can_receive_message_length_get(CAN0, CAN_FIFO0) < 1) &&
(0 != timeout)) {
timeout--;
}
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
can_message_receive(CAN0, CAN_FIFO0, &receive_message);
/* check the receive message */
if ((transmit_message.tx_sfid == receive_message.rx_sfid) &&
(transmit_message.tx_ff == receive_message.rx_ff) &&
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
/* loopback test is success */
std::printf("CAN LO0PBACK SUCCESS\r\n");
} else {
/* loopback test is failed */
std::printf("CAN LO0PBACK FAIL\r\n");
}
}
/**
\brief CAN0 回环测试
\param[in] none
\param[out] none
\retval none
*/
void can_loopback_test(void) {
can_trasnmit_message_struct transmit_message;
can_receive_message_struct receive_message;
uint32_t timeout = 0xFF;
uint8_t transmit_mailbox = 0;
std::mt19937 gen(rand()); // 使用随机设备生成的种子
std::uniform_int_distribution<int> dis(0, 0xff);
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
/* initialize transmit message */
transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID;
transmit_message.tx_ft = CAN_FT_DATA;
transmit_message.tx_ff = CAN_FF_STANDARD;
transmit_message.tx_dlen = 7;
int i = 0;
while (i++ < transmit_message.tx_dlen) {
// transmit_message.tx_data[i] = dis(gen);
transmit_message.tx_data[i] = i;
}
/* transmit message */
transmit_mailbox = can_message_transmit(CANX, &transmit_message);
/* waiting for transmit completed */
while ((CAN_TRANSMIT_OK != can_transmit_states(CANX, transmit_mailbox)) &&
(0 != timeout)) {
printf("sending:%x...\n", can_transmit_states(CANX, transmit_mailbox));
timeout--;
}
timeout = 0xFF;
/* waiting for receive completed */
while ((can_receive_message_length_get(CANX, CAN_FIFO0) < 1) &&
(0 != timeout)) {
timeout--;
}
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
can_message_receive(CANX, CAN_FIFO0, &receive_message);
/* check the receive message */
if ((DEV_CAN_LOOPBACK_ID == receive_message.rx_sfid) &&
(CAN_FF_STANDARD == receive_message.rx_ff) &&
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
i = transmit_message.tx_dlen;
while (i-- > 0) {
std::printf("rx %d:%x:%x\n", i, transmit_message.tx_data[i],
receive_message.rx_data[i]);
}
if (std::memcmp(receive_message.rx_data, transmit_message.tx_data,
transmit_message.tx_dlen) == 0) {
/* loopback test is success */
std::printf("loopback success\n");
} else {
std::printf("loopback failed\n");
}
} else {
/* loopback test is failed */
std::printf("loopback failed\n");
}
// 使能DMA中断
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF);
}
/*!
@@ -410,7 +180,6 @@ void can_loopback_test(void) {
*/
u8 CAN_transmiter::init() {
// 初始化CAN硬件
// can_config();
canfd_config();
// 初始化发送消息结构体
@@ -424,148 +193,6 @@ u8 CAN_transmiter::init() {
return 0;
}
/*!
\brief CAN 数据发送
\param id CAN ID
\param length 数据长度
\param data 数据
*/
u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
// 检查数据长度是否符合CAN规范
if (length > 8) {
printf("CAN data length exceeds maximum of 8 bytes\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
return 0;
}
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式
transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_dlen = length; // 设置数据长度
// 复制数据到发送缓冲区
for (int i = 0; i < length && i < 8; i++) {
transmit_message.tx_data[i] = *(data + i);
}
// 将数据复制到DMA缓冲区
memcpy(can_tx_buffer, data, length);
// 配置DMA发送参数
dma_tx_parameter.number = length;
dma_init(DMA0, DMA_CH5, &dma_tx_parameter);
// 启动DMA传输
dma_channel_enable(DMA0, DMA_CH5);
// 自动选择空闲mailbox发送消息，不需要人为指定
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
if (CAN_NOMAILBOX == transmit_mailbox) {
// 没有可用的mailbox，可能所有mailbox都在使用中
printf("No available mailbox for CAN transmission\r\n");
return 0;
}
return 1;
}
/*!
\brief CANFD 数据发送
\param id CAN消息ID
\param length CAN消息长度
\param data CAN消息数据
*/
u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
// 检查CANFD模式是否启用
if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
printf("CANFD mode not enabled\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
return 0;
}
// 检查数据长度是否符合CANFD规范
if (length > 64) {
printf("CANFD data length exceeds maximum of 64 bytes\r\n");
return 0;
}
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
// transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_efid = id; // 扩展帧ID
transmit_message.tx_dlen = length; // 设置数据长度
// 设置CANFD特定参数
transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式
transmit_message.fd_brs = ENABLE; // 使能比特率切换
transmit_message.fd_esi = DISABLE; // 错误状态指示
// 复制数据到发送缓冲区
for (int i = 0; i < length && i < 64; i++) {
transmit_message.tx_data[i] = *(data + i);
}
// 将数据复制到DMA缓冲区
memcpy(can_tx_buffer, data, length);
// 配置DMA发送参数
dma_tx_parameter.number = length;
dma_init(DMA0, DMA_CH5, &dma_tx_parameter);
// 启动DMA传输
dma_channel_enable(DMA0, DMA_CH5);
// 自动选择空闲mailbox发送消息，不需要人为指定
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
if (CAN_NOMAILBOX == transmit_mailbox) {
static u32 count = 0;
// 没有可用的mailbox，可能所有mailbox都在使用中
count++;
// printf("No available mailbox for CANFD transmission\r\n");
if (count > 0xff and !is_sleep) {
printf("No available mailbox for CANFD transmission\r\n");
init();
count = 0;
}
return 0;
}
return 1;
}
/**
* @brief CANFD发送函数
* @retval 0: 发送失败
* 1: 发送成功
*/
u8 CAN_transmiter::CANFD_all_Send() {
u32 success_flag = 0;
constexpr u32 STATUS_ID = 0x00010100;
constexpr u32 CONFIG_ID = 0x00010101;
// 打包status数据 - 只清零实际使用的32字节
memset(transmit_data, 0, 32);
success_flag += CANFD_Send(STATUS_ID, 32, transmit_data);
return success_flag;
}
/**
* @brief CAN通信检查函数
* @retval 0: 无新消息
@@ -576,20 +203,24 @@ void CAN_transmiter::communication_check() {
if (SET == can0_send_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
// CAN0_transmiter.CAN_all_Send();
CANFD_all_Send();
// CANFD_all_Send();
can0_send_flag = RESET;
}
// CAN0 接收数据标志
if (SET == can0_receive_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
// for (i = 0; i < receive_message.rx_dlen; i++) {
// printf(" %02x", receive_message.rx_data[i]);
// }
if (receive_message.rx_efid == 0x00010200) {
;
} else if (receive_message.rx_efid == 0x00010301) {
;
printf("\r\n can0 %X receive data:", receive_message.rx_efid);
for (int i = 0; i < receive_message.rx_dlen; i++) {
printf(" %02x", receive_message.rx_data[i]);
}
if (receive_message.rx_efid == CANFD_OTA_DATA_ID) {
ota.handleOTAMessage(receive_message.rx_efid,
receive_message.rx_data,
receive_message.rx_dlen);
} else if (receive_message.rx_efid == CANFD_OTA_REQ_ID) {
ota.handleOTAMessage(receive_message.rx_efid,
receive_message.rx_data,
receive_message.rx_dlen);
} else {
printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
}
@@ -601,33 +232,9 @@ u8 CAN_transmiter::sendACK(u32 CANFD_OTA_ACK_ID, u32 packet_num) {
u32_union_t pack_data;
pack_data.u = packet_num;
memset(transmit_data, 0, 4);
pack_u32_to_bytes(transmit_data, 3, pack_data);
pack_u32_to_bytes(transmit_data, 0, pack_data);
return CANFD_Send(CANFD_OTA_ACK_ID, 4, transmit_data);
}
/**
* @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;
return CANFD_AsyncSend(CANFD_OTA_ACK_ID, 4, transmit_data);
}
/**
@@ -691,7 +298,82 @@ 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 异步CANFD数据发送(使用DMA和中断机制)
* @param id CAN消息ID
* @param length CAN消息长度
* @param data CAN消息数据
*/
u8 CAN_transmiter::CANFD_AsyncSend(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_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);
}
// 自动选择空闲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
+3 -22
View File
@@ -24,7 +24,7 @@ static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
// 添加辅助函数用于将float类型数据打包到u8数组中
static inline void pack_u32_to_bytes(u8 *buffer, u16 offset,
u32_union_t &u32_union) {
u32_union_t &u32_union) {
buffer[offset] = u32_union.b[0];
buffer[offset + 1] = u32_union.b[1];
buffer[offset + 2] = u32_union.b[2];
@@ -54,16 +54,11 @@ static inline u32_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
}
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 can_loopback_init(void);
void can_loopback_test(void);
/*!
\brief CAN通信类
\note 封装CAN发送和接收功能
@@ -71,18 +66,10 @@ void can_loopback_test(void);
class CAN_transmiter {
private:
bool is_sleep = false; // 是否休眠
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_all_Send(); // CAN发送
u8 CANFD_all_Send(); // CANFD发送
public:
// 添加构造函数声明
u8 init(void);
@@ -90,15 +77,9 @@ class CAN_transmiter {
can_receive_message_struct receive_message; // 接收消息结构体
FlagStatus can0_receive_flag; // CAN0接收标志
FlagStatus can0_send_flag; // CAN0发送标志
u32_union_t testfloat_union = {0}; // 测试用
bool MagStatus = 0; // 磁状态码
bool ThermalStatus = 0; // 热能状态码
void communication_check(); // 通信检查
u8 CAN_Sleep(); // CAN进入睡眠
u8 CAN_Wakeup(); // CAN进入睡眠
u8 CAN_Send(u32 ID, u8 length, u8 *data); // CAN发送
u8 CANFD_Send(u32 ID, u8 length, u8 *data); // CANFD发送
void communication_check(); // 通信检查
u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA)
u8 sendACK(u32 CANFD_OTA_ACK_ID, u32 packet_num);
};
Robot_V2/device/Timer.cpp
-109
View File
@@ -1,109 +0,0 @@
#include "Timer.h"
#include "Can.h"
#include "Timer.h"
#include "gd32c10x.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_misc.h"
#include "gd32c10x_rcu.h"
#include "gd32c10x_timer.h"
#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 TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
constexpr int TIMER1_CH0_PULSE = 0;
constexpr int TIMER1_CH1_PULSE = 0;
constexpr int TIMER1_CH2_PULSE = 0;
constexpr int TIMER1_CH3_COMPARE_VALUE =
(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
extern CAN_transmiter CAN0_transmiter;
// 添加系统运行时间计数器
extern u32 cnt_test;
/**
* @brief 设置引脚中断优先级
* @return void
*
*设置引脚中断优先级
*/
void nvic_config(void) {
/* 设置NVIC优先级分组，1位抢占优先级，3位响应优先级 */
nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
nvic_irq_enable(TIMER2_IRQn, 0, 1);
nvic_irq_enable(TIMER1_IRQn, 0, 2);
}
/**
* @brief 配置定时器2
* @return void
*
*配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务
*/
void TIMER2_config(void) {
/* ----------------------------------------------------------------------------
TIMER2 Configuration:
TIMER2CLK = SystemCoreClock/120 = 1MHz, the period is 100us(100/1000000 =
100us).
----------------------------------------------------------------------------
*/
timer_parameter_struct timer_struct;
/* 使能TIMER2时钟 */
rcu_periph_clock_enable(RCU_TIMER2);
/* 复位TIMER2外设到默认状态 */
timer_deinit(TIMER2);
/* 初始化TIMER初始化参数结构体 */
timer_struct_para_init(&timer_struct);
/* TIMER2配置参数设置 */
timer_struct.prescaler =
TIMER2_PRESCALER; /* 预分频值：120分频(从0开始计数) */
timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */
timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
timer_struct.period =
TIMER2_PERIOD; /* 自动重装载值：100次计数(从0开始计数) */
timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
timer_init(TIMER2, &timer_struct);
/* 清除更新中断标志位 */
timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_UP);
/* 使能更新中断 */
timer_interrupt_enable(TIMER2, TIMER_INT_UP);
/* 使能TIMER2 */
timer_enable(TIMER2);
}
/**
* @brief 配置TIMER2中断
* @return void
*/
void User_timer_init(void) {
/* 配置TIMER2 */
TIMER2_config();
/* 配置TIMER2中断 */
nvic_config();
}
/**
* @brief TIMER2中断处理函数
* @return void
*/
void TIMER2_IRQHandler(void) {
// 检查更新中断标志并处理
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 每100us执行一次的操作
}
}
Robot_V2/device/Timer.h
-27
View File
@@ -1,27 +0,0 @@
#ifndef _TIMER_H
#define _TIMER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
void User_timer_init(void);
void Set_Pwm(uint16_t channel, float duty_cycle);
void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2);
void TIMER2_IRQHandler(void);
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
#endif
Robot_V2/device/adc.h
-40
View File
@@ -1,40 +0,0 @@
#ifndef _ADC_H
#define _ADC_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stdint.h"
constexpr u8 FILTER_SIZE = 6;
constexpr u8 BLOCK_SIZE = FILTER_SIZE * sizeof(float);
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];
u8 current_sample_index;
float tempora_sample;
public:
u8 init(void);
void update_mcu_temp(void);
void update_drv_temp(void);
void update_vbus_voltage(void);
void update_current(void);
float get_mcu_temp(void);
float get_drv_temp(void);
float get_vbus_voltage(void);
float get_current(char);
};
#ifdef __cplusplus
}
#endif
#endif
Robot_V2/device/ang_sensor.cpp
-306
View File
@@ -1,306 +0,0 @@
#include "ang_sensor.h"
#include "Filter.h"
#include "core_cmFunc.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "initial.h"
#include "flash.h"
#include <cstdio>
#include <cstring>
#include "Can.h"
#include <stdint.h>
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
constexpr u16 DEFAULT_COMMAND = 0x0000;
constexpr u16 FAULT_RECEIVE = 0xFFFF;
constexpr float VELOCITY_SCALE = 1e4f;
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
constexpr float ANGLE_WRAP_VALUE = 360.0f;
constexpr u8 SIZE_FLOAT = sizeof(float);
// DMA传输完成标志
volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
angle_sensor KTH7823; // 角度传感器初始化
extern FlashEEPROM flashEEP;
extern EncoderAngleKalman Angle_kalman;
extern EncoderVelocityKalman Vel_kalman;
extern CAN_transmiter CAN0_transmiter; // CAN初始化
/**
* @brief SPI DMA initialization
*/
void spi_dma_init(void) {
dma_parameter_struct dma_init_struct;
// 使能DMA时钟
rcu_periph_clock_enable(RCU_DMA0);
// 初始化发送DMA通道
dma_deinit(DMA0, DMA_CH2);
dma_struct_para_init(&dma_init_struct);
dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
dma_init_struct.memory_addr = (uint32_t)0; // 实际地址将在传输时设置
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_16BIT;
dma_init_struct.number = 1;
dma_init_struct.periph_addr = (uint32_t)(&SPI_DATA(SPI0));
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
dma_init_struct.priority = DMA_PRIORITY_HIGH;
dma_init(DMA0, DMA_CH2, &dma_init_struct);
// 初始化接收DMA通道
dma_deinit(DMA0, DMA_CH1);
dma_struct_para_init(&dma_init_struct);
dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
dma_init_struct.memory_addr = (uint32_t)0; // 实际地址将在传输时设置
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_16BIT;
dma_init_struct.number = 1;
dma_init_struct.periph_addr = (uint32_t)(&SPI_DATA(SPI0));
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
dma_init_struct.priority = DMA_PRIORITY_HIGH;
dma_init(DMA0, DMA_CH1, &dma_init_struct);
// 使能SPI的DMA发送和接收功能
spi_dma_enable(SPI0, SPI_DMA_TRANSMIT);
spi_dma_enable(SPI0, SPI_DMA_RECEIVE);
// 配置DMA中断
nvic_irq_enable(DMA0_Channel2_IRQn, 0, 1);
nvic_irq_enable(DMA0_Channel1_IRQn, 0, 1);
// 使能DMA传输完成中断
dma_interrupt_enable(DMA0, DMA_CH2, DMA_INT_FTF);
dma_interrupt_enable(DMA0, DMA_CH1, DMA_INT_FTF);
}
/**
* @brief Send or read half word through SPI using DMA
*
* @param[in] spi_periph SPI peripheral
* @param[in] tx_data Data to transmit
* @param[out] rx_data Pointer to store received data
* @return 0 if success, -1 if error
*/
int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
uint16_t *rx_data) {
// 重置完成标志
spi_dma_tx_complete = false;
spi_dma_rx_complete = false;
// 配置发送DMA
dma_channel_disable(DMA0, DMA_CH2);
dma_memory_address_config(DMA0, DMA_CH2, (uint32_t)&tx_data);
dma_transfer_number_config(DMA0, DMA_CH2, 1);
dma_channel_enable(DMA0, DMA_CH2);
// 配置接收DMA
dma_channel_disable(DMA0, DMA_CH1);
dma_memory_address_config(DMA0, DMA_CH1, (uint32_t)rx_data);
dma_transfer_number_config(DMA0, DMA_CH1, 1);
dma_channel_enable(DMA0, DMA_CH1);
// 等待传输完成，超时时间为100个循环
uint32_t timeout = 100;
while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
// 空循环等待DMA传输完成
}
// 检查是否超时
if (timeout == 0) {
// 禁用DMA通道
dma_channel_disable(DMA0, DMA_CH2);
dma_channel_disable(DMA0, DMA_CH1);
printf("SPI DMA transfer timeout\r\n");
return -1;
}
return 0;
}
/**
* @brief DMA0_Channel2中断处理函数
* @param 无
*/
extern "C" void DMA0_Channel2_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH2, DMA_INT_FLAG_FTF)) {
dma_interrupt_flag_clear(DMA0, DMA_CH2, DMA_INT_FLAG_FTF);
spi_dma_tx_complete = true;
}
}
/**
* @brief DMA0_Channel1中断处理函数
* @param 无
*/
extern "C" void DMA0_Channel1_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH1, DMA_INT_FLAG_FTF)) {
dma_interrupt_flag_clear(DMA0, DMA_CH1, DMA_INT_FLAG_FTF);
spi_dma_rx_complete = true;
}
}
/**
* @brief 角度传感器初始化
* @param 无
*/
void KTH7823_gpio_init(void) {
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_SPI0);
rcu_periph_clock_enable(RCU_AF);
/* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
/* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
/* PA15 → NSS, 推挽输出 */
gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
/* PB3 → SCK, PB5 → MOSI, 推挽输出 */
gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ,
GPIO_PIN_3 | GPIO_PIN_5);
/* PB4 → MISO, 浮空输入 */
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ,
GPIO_PIN_4 | GPIO_PIN_10 | GPIO_PIN_11);
}
/**
* @brief SPI 初始化
*
* @details Configures SPI hardware and initializes necessary settings
*/
void KTH7823_spi_init(void) {
spi_parameter_struct spi_init_str;
spi_struct_para_init(&spi_init_str);
/* SPI参数：16bit, 主机, 全双工, CPOL=1 CPHA=1, MSB first */
spi_init_str.device_mode = SPI_MASTER;
spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE;
spi_init_str.nss = SPI_NSS_SOFT;
spi_init_str.endian = SPI_ENDIAN_MSB;
spi_init_str.prescale = 24; /* 5MHz @ 120MHz */
spi_init(SPI0, &spi_init_str);
spi_nss_output_enable(SPI0);
spi_nssp_mode_enable(SPI0);
spi_enable(SPI0);
}
/**
* @ 角度传感器类 初始化
* @return 0:初始化成功 1:初始化失败
*/
u8 angle_sensor::init(void) {
KTH7823_gpio_init(); // GPIO初始化
KTH7823_spi_init(); // SPI初始化
spi_dma_init(); // DMA初始化
return 0;
}
/**
* @ 角度传感器类 设置当前角度
* @param angle 输出级角度值
* @return 0成功/1失败
*/
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;
ANG_SPI_CS_ENABLE();
u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
ANG_SPI_CS_DISABLE();
if (result == 0) {
// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
float angle_deg = (float)raw_data * ANGLE_FACTOR;
current_angle_deg = angle_deg;
current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
return angle_deg;
}
#ifdef DEBUG
// 仅在调试模式下输出错误，避免发布版本中的I/O开销
if (result != 0) {
// 使用更高效的串口输出替代printf（如果可用）
printf("Error reading angle from sensor.\n");
}
#endif
return (float)FAULT_RECEIVE;
}
/**
* @brief 更新传感器角度（输出级）
* @return 累积角度值
*/
float angle_sensor::update(void) {
read_sensorAngle();
if (m_firstUpdate) {
m_lastAngle = current_angle_deg;
m_firstUpdate = false;
return m_cumulativeAngle;
}
// 计算角度变化
float delta = current_angle_deg - m_lastAngle;
// 处理角度绕回情况（0度和360度之间的跳变）
// 如果变化超过180度，说明发生了绕回
if (delta > ANGLE_WRAP_THRESHOLD) {
// 从接近360度绕回到接近0度
delta -= ANGLE_WRAP_VALUE;
} else if (delta < -ANGLE_WRAP_THRESHOLD) {
// 从接近0度绕到接近360度
delta += ANGLE_WRAP_VALUE;
}
m_cumulativeAngle += delta;
m_lastAngle = current_angle_deg;
return m_cumulativeAngle;
}
/**
* @brief 获取累积角度值(输出级)
* @return 累积角度值
*/
float angle_sensor::getAngle(void) const {
return m_cumulativeAngle / REDUCTION_RATIO;
}
/**
* @brief 获取累积角度值(原始级)
* @return 累积角度值
*/
float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
/**
* @brief 获取角度值(原始级)
* @return 角度值(原始级)
*/
float angle_sensor::getcurrent_sensorangle(void) const {
return current_angle_deg;
}
Robot_V2/device/ang_sensor.h
-40
View File
@@ -1,40 +0,0 @@
#ifndef _angsensor_H
#define _angsensor_H
#ifdef __cplusplus
extern "C" {
#endif
#include "gd32c10x_eval.h"
#include "stdio.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:
u8 init(void);
float read_sensorAngle(void);
float update(void);
void reset(void);
float getAngle(void) const;
float getRawAngle(void) const;
float getcurrent_sensorangle(void) const;
u8 angle_set(float angle);
protected:
float m_cumulativeAngle;
float current_angle_deg;
float current_angle_rad;
bool m_firstUpdate;
float m_lastAngle;
float m_lastcumulativeAngle;
float velocity[VEL_FILTER_SIZE] = {0.0f};
};
#ifdef __cplusplus
}
#endif
#endif
Robot_V2/device/motor.cpp
-38
View File
@@ -1,38 +0,0 @@
#include "motor.h"
#include "Timer.h"
#include "gd32c10x_eval.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);
}
/**
* @brief 电机复位
*/
void motor_reset(void) {
motor_sleep();
printf("motor reseting...\r\n");
motor_resume();
}
/**
* @brief 电机休眠
*/
void motor_sleep(void) { gpio_bit_reset(GPIOB, GPIO_PIN_12); }
/**
* @brief 电机启动
*/
void motor_resume(void) { gpio_bit_set(GPIOB, GPIO_PIN_12); }
Robot_V2/device/motor.h
-11
View File
@@ -1,11 +0,0 @@
#ifndef _MOTOR_H
#define _MOTOR_H
void motor_init(void);
void motor_reset(void);
void motor_resume(void);
void motor_sleep(void);
#endif
Robot_V2/device/test.cpp
-25
View File
@@ -1,25 +0,0 @@
#include "adc.h"
#include <cstdio>
constexpr int status_print = 0;
extern sampler sampler;
void test_1111() {
// 初始化CAN回环测试
// can_loopback_init();
// can_config();
if (status_print) {
/* value print */
printf(" the temperature data is %2.2f degrees Celsius\r\n",
sampler.get_mcu_temp());
printf(" the drv_temperature data is %2.2f degrees Celsius\r\n",
sampler.get_drv_temp());
printf(" the U current data is %2.2f mA\r\n", sampler.get_current('U'));
printf(" the V current data is %2.2f mA\r\n", sampler.get_current('V'));
printf(" the W current data is %2.2f mA\r\n", sampler.get_current('W'));
printf(" the VBUS is %2.2f V\r\n", sampler.get_vbus_voltage());
// printf(" the angle is %2.2f\r\n", tle5012b_read());
printf("\n----------------------------------------------------\n");
}
// can_loopback_test();
}
Robot_V2/main.cpp
+23 -31
View File
@@ -1,40 +1,32 @@
#include "main.h"
#include "Can.h"
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "sys/OTA.h"
#include "sys/config.h"
#include "initial.h"
#include "sys/systick.h"
#include <cstdio>
extern CAN_transmiter CAN0_transmiter; // CAN初始化
class BootloaderSystem {
private:
OTAHandler ota;
public:
void initHardware() {
extern CAN_transmiter CAN0_transmiter; // CAN初始化
systick_config(); // 系统时钟初始化
SystemCoreClockUpdate(); // 更新系统时钟变量
CAN0_transmiter.init();
}
void run() {
// 检查是否触发OTA模式
if (1) {
// OTA模式
while (1) {
ota.process();
for (volatile int i = 0; i < 500000; i++)
; // 简单延时
}
} else {
// 正常模式：跳转到APP
ota.jumpToApplication();
BootloaderSystem sys;
extern OTAHandler ota;
void BootloaderSystem::initHardware() {
systick_config(); // 系统时钟初始化
SystemCoreClockUpdate(); // 更新系统时钟变量
USART0_init(); // 调试串口初始化
CAN0_transmiter.init();
}
void BootloaderSystem::run() {
// 检查是否触发OTA模式
if (1) {
// OTA模式
while (1) {
CAN0_transmiter.communication_check();
ota.process();
}
} else {
// 正常模式：跳转到APP
ota.jumpToApplication();
}
};
}
int main(void) {
BootloaderSystem sys;
sys.initHardware();
sys.run();
Robot_V2/main.h
+8 -1
View File
@@ -35,7 +35,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef MAIN_H
#define MAIN_H
#include "stdint.h"
#include "sys/OTA.h"
void do_task(u32 currentTasks);
class BootloaderSystem {
private:
public:
void initHardware();
void run();
};
#endif /* MAIN_H */
Robot_V2/sys/OTA.cpp
+82 -46
View File
@@ -1,31 +1,47 @@
#include "OTA.h"
#include "gd32c10x_fmc.h"
#include "systick.h"
#include <cstdio>
#include <cstring>
#include <flash.h>
#include <stdint.h>
extern CAN_transmiter CAN0_transmiter; // CAN初始化
extern FlashEEPROM flashEEP;
OTAHandler ota;
extern "C" {
void canfd_rx_irq_handler(void);
}
#define DEBUG_OTA 1
OTAHandler::OTAHandler() : ota_active(false), received_size(0) {}
void OTAHandler::init() {
// 初始化CANFD接收中断
nvic_irq_enable(CAN0_RX0_IRQn, 1, 1);
}
void OTAHandler::init() {}
void OTAHandler::process() {
if (!ota_active)
if (!ota_active) {
ota_time = 0; // 重置计时器
static u8 zero[1] = {ota_active};
CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_WAIT_ID, 0, zero);
#ifdef DEBUG_OTA
printf("wait OTA%d\r\n", ota_active);
#endif
delay_1ms(10);
return;
} else {
delay_1ms(1);
ota_time++;
}
// 这里可以添加超时处理逻辑
// 例如：10秒无数据则退出OTA模式
// 检查是否超时（例如：10秒无数据则退出OTA模式）
if (ota_time > 10000) { // 10秒超时（10000 x 1ms）
ota_time = 0;
ota_active = false;
}
}
bool OTAHandler::validateApplication() {
uint32_t *app_vector = (uint32_t *)APP_START_ADDR;
uint32_t stack_ptr = app_vector[0];
uint32_t reset_handler = app_vector[1];
u32 *app_vector = (u32 *)APP_START_ADDR;
u32 stack_ptr = app_vector[0];
u32 reset_handler = app_vector[1];
// 简单验证：检查栈指针是否在SRAM范围内
return (stack_ptr >= 0x20000000 && stack_ptr < 0x20008000) &&
@@ -34,45 +50,58 @@ bool OTAHandler::validateApplication() {
}
void OTAHandler::jumpToApplication() {
if (!validateApplication()) {
// 无效APP，停留在bootloader
return;
}
// 关闭所有中断
__disable_irq();
// 设置主栈指针
uint32_t msp_value = *(__IO uint32_t *)APP_START_ADDR;
__set_MSP(msp_value);
// 获取复位地址
uint32_t reset_handler = *(__IO uint32_t *)(APP_START_ADDR + 4);
typedef void (*AppEntryFunc)(void);
AppEntryFunc app_entry = (AppEntryFunc)reset_handler;
// 跳转
app_entry();
if (!validateApplication()) return;
// 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. 设置MSP并跳转
uint32_t msp_value = *(__IO uint32_t*)APP_START_ADDR;
__set_MSP(msp_value);
uint32_t reset_handler = *(__IO uint32_t*)(APP_START_ADDR + 4);
typedef void (*AppEntryFunc)(void);
AppEntryFunc app_entry = (AppEntryFunc)reset_handler;
// 6. 跳转
app_entry();
}
void OTAHandler::eraseApplicationArea() {
// 擦除APP区域
fmc_unlock();
for (uint32_t addr = APP_START_ADDR; addr < (APP_START_ADDR + APP_MAX_SIZE);
for (u32 addr = APP_START_ADDR; addr < (APP_START_ADDR + APP_MAX_SIZE);
addr += FMC_PAGE_SIZE) {
fmc_page_erase(addr);
}
fmc_lock();
}
void OTAHandler::writeApplicationData(uint32_t offset, uint8_t *data,
uint32_t size) {
void OTAHandler::writeApplicationData(u32 offset, u8 *data, u32 size) {
fmc_unlock();
uint32_t write_addr = APP_START_ADDR + offset;
u32 write_addr = APP_START_ADDR + offset;
// 32位对齐写入
for (uint32_t i = 0; i < size; i += 4) {
uint32_t word = 0;
for (u32 i = 0; i < size; i += 4) {
u32 word = 0;
memcpy(&word, &data[i], 4);
fmc_word_program(write_addr + i, word);
}
@@ -80,27 +109,34 @@ void OTAHandler::writeApplicationData(uint32_t offset, uint8_t *data,
fmc_lock();
}
void OTAHandler::handleOTAMessage(uint32_t id, uint8_t *data, uint8_t len) {
void OTAHandler::handleOTAMessage(u32 id, u8 *data, u8 len) {
if (id == CANFD_OTA_REQ_ID) {
#ifdef DEBUG_OTA
printf("req OTA\r\n");
#endif
// OTA请求包
uint32_t new_size = *(uint32_t *)data;
u32 new_size = *(u32 *)data;
if (new_size <= APP_MAX_SIZE) {
expected_size = new_size;
received_size = 0;
eraseApplicationArea();
// eraseApplicationArea();
ota_active = true;
sendACK(0xFFFFFFFF); // 确认开始
sendACK(new_size); // 确认开始
#ifdef DEBUG_OTA
printf("start OTA%d\r\n", ota_active);
#endif
}
} else if (id == CANFD_OTA_DATA_ID && ota_active) {
// 数据包格式：[4字节包序号][实际数据]
if (len >= 4) {
uint32_t packet_num = *(uint32_t *)data;
uint32_t offset = packet_num * (len - 4);
u32 packet_num = *(u32 *)data;
u32 offset = packet_num * (len - 4);
if (offset < expected_size) {
writeApplicationData(offset, &data[4], len - 4);
writeApplicationData(offset, &data[4], len - 4); // 写入数据
received_size += (len - 4);
sendACK(packet_num);
sendACK(packet_num); // 确认ACK
ota_time = 0; // 重置计时器
// 检查是否完成
if (received_size >= expected_size) {
@@ -115,6 +151,6 @@ void OTAHandler::handleOTAMessage(uint32_t id, uint8_t *data, uint8_t len) {
}
}
void OTAHandler::sendACK(uint32_t packet_num) {
void OTAHandler::sendACK(u32 packet_num) {
CAN0_transmiter.sendACK(CANFD_OTA_ACK_ID, packet_num);
}
Robot_V2/sys/OTA.h
+1 -1
View File
@@ -8,10 +8,10 @@
class OTAHandler {
public:
bool ota_active;
u32 ota_time;
uint32_t received_size;
uint32_t expected_size;
OTAHandler();
void init();
void process();
bool isActive() const { return ota_active; }
Robot_V2/sys/config.h
+19 -14
View File
@@ -1,21 +1,26 @@
#pragma once
#ifndef __CONFIG_H__
#define __CONFIG_H__
#include "stdint.h"
// Flash分区配置
constexpr uint32_t BOOTLOADER_SIZE = 0x4000; // 16KB
constexpr uint32_t APP_START_ADDR = 0x08004000; // APP起始地址
constexpr uint32_t APP_MAX_SIZE = 0x0001C000; // 112KB
constexpr uint32_t FMC_PAGE_SIZE = 0x400;
constexpr u32 BOOTLOADER_SIZE = 0x4000; // 16KB
constexpr u32 APP_START_ADDR = 0x08004000; // APP起始地址
constexpr u32 APP2_START_ADDR = 0x08010000; // APP2起始地址
constexpr u32 APP_MAX_SIZE = 0xC000; // 48KB
constexpr u32 FMC_PAGE_SIZE = 0x400; // 1KB
// CANFD配置
constexpr uint32_t CANFD_OTA_REQ_ID = 0x00000100;
constexpr uint32_t CANFD_OTA_DATA_ID = 0x00000101;
constexpr uint32_t CANFD_OTA_ACK_ID = 0x00000102;
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;
// CANFD数据段最大64字节，我们用32字节数据 + 4字节头部
constexpr uint32_t OTA_PACKET_SIZE = 32;
// CANFD数据段最大64字节，我们用1字节序号 + 48字节数据 + 1字节CRC校验
constexpr u32 OTA_PACKET_SIZE = 48;
typedef struct {
uint32_t packet_num; // 包序号
uint8_t data[OTA_PACKET_SIZE]; // 32字节有效数据
uint32_t crc32; // 简单校验
} canfd_ota_packet_t;
u32 packet_num; // 包序号
u8 data[OTA_PACKET_SIZE]; // 48字节有效数据
u32 crc32; // 简单校验
} canfd_ota_packet_t;
#endif
Robot_V2/sys/flash.cpp
+2 -346
View File
@@ -10,11 +10,8 @@
#include <stdint.h>
FlashEEPROM flashEEP = FlashEEPROM();
extern CAN_transmiter CAN0_transmiter;
constexpr u8 SIZE_U32 = sizeof(uint32_t);
constexpr u8 SIZE_FLOAT = sizeof(float);
constexpr u8 DEFAULT_VALUE = 0;
constexpr u8 NUM_SAVED = 6;
#define EEPDEBUG 0
// FlashEEPROM类实现
@@ -31,322 +28,7 @@ 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[NUM_SAVED + 1] = {FLASH_EEPROM_OK};
float data[NUM_SAVED] = {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 < NUM_SAVED + 1; i++) {
if (status_read[i] ==
FLASH_EEPROM_DATA_NOT_FOUND) { // 首次读取，不保存报错
data[i - 1] = DEFAULT_VALUE;
} else if (status_read[i] != FLASH_EEPROM_OK) { // 其余错误，保存报错
data[i - 1] = DEFAULT_VALUE;
status_read[0] = status_read[i];
}
}
// 读取到全部数据
if (status_read[0] == FLASH_EEPROM_OK) {
u32 next_address = EEPROM_END_ADDRESS;
// 遍历Flash页面寻找下一个可写入位置
for (u32 address = EEPROM_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, NUM_SAVED);
#if EEPDEBUG
printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
#endif
if (CRC_NUM != *((u32 *)next_address)) { // CRC错误
for (u8 i = 0; i < NUM_SAVED; i++) {
data[i] = DEFAULT_VALUE;
}
EEPROMStatus = FLASH_EEPROM_CRC_ERROR;
} else { // CRC正确
EEPROMStatus = FLASH_EEPROM_OK;
}
} else {
// 读取失败，报错
EEPROMStatus = status_read[0];
}
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 EEPDEBUG
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 EEPDEBUG
data_read = data_read & DATA_U16_MASK;
u16 float_data_read = *((u16 *)&data_read);
printf("Found data in:0x%x=%u\r\n", address, data_read);
#endif
return FLASH_EEPROM_OK;
} else {
break;
}
}
}
// 遍历Flash页面寻找下一个可写入位置
for (u32 address = EEPROM_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;
// 使用GD32库函数进行安全编程
fmc_unlock();
fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
fmc_state_enum fmc_status =
fmc_word_program(next_address, data_word); // 写入数据
fmc_lock();
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_WRITE_ERROR;
}
return FLASH_EEPROM_OK;
} else { // 空间不足，需要格式化页面
#if EEPDEBUG
printf("no enough space\r\n");
#endif
FlashEepromStatus status = format();
if (status == FLASH_EEPROM_OK) {
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 EEPDEBUG
data_word_read = data_word_read & DATA_FLOAT_MASK;
float float_data_read = *((float *)&data_word_read);
printf("Found data in:0x%x=%f\r\n", address, float_data_read);
#endif
return FLASH_EEPROM_OK;
} else {
break; // 找到相同地址但数据不同，需要更新
}
}
}
// 遍历Flash页面寻找下一个可写入位置
for (u32 address = EEPROM_DATA_START_ADDRESS; address < EEPROM_END_ADDRESS;
address += SIZE_U32) {
// 安全读取Flash数据
volatile u32 *flash_ptr = (volatile u32 *)address;
u32 data_word_read = *flash_ptr;
// 找到空位置
if (data_word_read == 0xFFFFFFFF) {
next_address = address;
break;
}
// 更新下一个可能的写入位置（如果需要格式化）
next_address = address + SIZE_U32;
}
// 如果有足够空间，直接写入
if (next_address < EEPROM_END_ADDRESS) {
// 使用GD32库函数进行安全编程
fmc_unlock();
fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
// 确保使用对齐地址
fmc_state_enum fmc_status = fmc_word_program(next_address, data_word);
fmc_lock();
#if EEPDEBUG
u32 data_word_c = *((u32 *)next_address);
data_word_c = data_word_c & DATA_FLOAT_MASK;
float float_data = *((float *)&data_word_c);
printf("save:0x%x=%f\r\n", next_address, float_data);
#endif
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_WRITE_ERROR;
}
return FLASH_EEPROM_OK;
} else {
// 空间不足，需要格式化页面
#if EEPDEBUG
printf("no enough space\r\n");
#endif
FlashEepromStatus status = format();
if (status == FLASH_EEPROM_OK) {
return status;
} else {
return FLASH_EEPROM_ERASE_ERROR;
}
}
return 0; // 正常不会返回这个，未预料的异常情况返回
}
/**
@@ -375,24 +57,11 @@ FlashEepromStatus FlashEEPROM::format() {
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) {
FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM) {
static u32 next_address;
// 遍历Flash页面寻找下一个可写入crc的位置
for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32;
@@ -426,24 +95,11 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
EEPROMStatus = fmc_status;
return FLASH_EEPROM_WRITE_ERROR;
}
} else {
// 空间不足，需要格式化页面
#if EEPDEBUG
printf("no enough space\r\n");
#endif
FlashEepromStatus status = format();
if (status == FLASH_EEPROM_OK) {
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 {
Robot_V2/sys/flash.h
+2 -30
View File
@@ -2,8 +2,6 @@
#define __FLASH_H
#include <stdint.h>
// 修改后 - 强制4字节对齐
#define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐
// EEPROM操作状态
typedef enum {
@@ -26,21 +24,8 @@ class FlashEEPROM {
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; // 虚拟地址掩码
0x0801F900; // DATA段开始地址
// 定义用户自定义的虚拟地址
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();
@@ -48,24 +33,11 @@ class 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();
u8 read_saved_system_config(void);
float read_angle();
FlashEepromStatus writeCRC(u32 CRC_NUM);
private:
FlashEepromStatus writeCRC(u32 CRC_NUM, bool saved);
};
void flash_eeprom_example_main(void);
#endif /* __FLASH_H */
Robot_V2/sys/health.cpp
-103
View File
@@ -1,103 +0,0 @@
#include "health.h"
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"
#include "flash.h"
#include <stdint.h>
extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
extern FlashEEPROM flashEEP;
ThermalController thermalController; // 热量控制器初始化
#define max(a, b, c) \
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
/**
** @brief 热能计算
** @param None
** @retval 热量值
*/
u32 ThermalController::update() {
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) {
// 过热停机状态
// 检查是否可以恢复
q16_16_t recovery_threshold =
Q16_16_MUL(thermal_capacity, recoveryRate);
if (thermal_value < recovery_threshold) {
remainingTime10Ms = 1; // 可以恢复工作
}
if (remainingTime10Ms == 1) { // 过热恢复
FOC.resume();
CAN0_transmiter.ThermalStatus = 0;
return --remainingTime10Ms;
}
return --remainingTime10Ms;
} 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; // 过热停机
CAN0_transmiter.ThermalStatus = 1;
return remainingTime10Ms;
}
return thermal_value;
}
/**
** @brief 温度检测
** @param None
*/
void angsens_MG_check(void) {
bool MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10);
bool MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
CAN0_transmiter.MagStatus = MGH or MGL;
}
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
-40
View File
@@ -1,40 +0,0 @@
#ifndef _HEALTH_H
#define _HEALTH_H
#include "stdint.h"
typedef int32_t q16_16_t; // Q16.16格式
// 定点数运算宏
#define Q16_16_ONE 65536 // 1.0 in Q16.16
#define FLOAT_TO_Q16_16(x) ((q16_16_t)((x) * Q16_16_ONE))
#define Q16_16_TO_FLOAT(x) ((float)(x) / Q16_16_ONE)
#define Q16_16_MUL(a, b) ((q16_16_t)(((int64_t)(a) * (b)) >> 16))
#define Q16_16_SQUARE(x) Q16_16_MUL((x), (x))
class ThermalController {
private:
u8 temperature;
u16 current_max;
q16_16_t thermalRate; // 热耗散系数（每单位时间散热比例）
q16_16_t recoveryRate; // 自然恢复率（散热到多少以下开始工作）
u32 thermal_value = 0;
u32 thermal_capacity = 3600000; // 热容量
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;
remainingTime10Ms = 0;
}
};
void angsens_MG_check(void);
void power_manage(void);
#endif
Robot_V2/sys/initial.cpp
+1 -58
View File
@@ -12,21 +12,6 @@ u8 USART0_init() {
return 1;
}
/*!
\brief configure the watchdog peripheral
\param[in] none
\param[out] none
\retval none
*/
u8 watchdog_init(u32 time) {
u8 status = 0;
/* configure FWDGT counter clock: 40KHz(IRC40K) / 64 = 0.625 KHz */
/* time = reload_value / clock_frequency */
status = fwdgt_config(time * 0.625, FWDGT_PSC_DIV64);
fwdgt_enable();
return status;
}
int fputc(int ch, FILE *f) throw() {
usart_data_transmit(EVAL_COM0, (uint8_t)ch);
@@ -48,46 +33,4 @@ void unused_pin_init() {
gpio_bit_reset(GPIOA, GPIO_PIN_8 | GPIO_PIN_11 | GPIO_PIN_12);
gpio_bit_reset(GPIOB, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_15);
gpio_bit_reset(GPIOC, GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15);
}
/*!
\brief 检测并打印复位源信息
\param[in] none
\param[out] none
\retval none
*/
u16 get_reset_cause(void) {
u16 cause = 0;
if (RESET != rcu_flag_get(RCU_FLAG_FWDGTRST)) {
// printf("Free Watchdog Timer Reset\r\n");
cause = RCU_FLAG_FWDGTRST;
}
if (RESET != rcu_flag_get(RCU_FLAG_EPRST)) {
// printf("External Pin Reset\r\n");
cause = RCU_FLAG_EPRST;
}
if (RESET != rcu_flag_get(RCU_FLAG_PORRST)) {
// printf("Power Reset\r\n");
cause = RCU_FLAG_PORRST;
}
if (RESET != rcu_flag_get(RCU_FLAG_WWDGTRST)) {
// printf("Window Watchdog Timer Reset\r\n");
cause = RCU_FLAG_WWDGTRST;
}
if (RESET != rcu_flag_get(RCU_FLAG_LPRST)) {
// printf("Low Power Reset\r\n");
cause = RCU_FLAG_LPRST;
}
if (RESET != rcu_flag_get(RCU_FLAG_SWRST)) {
// printf("Software Reset\r\n");
cause = RCU_FLAG_SWRST;
}
// 清除所有复位标志
rcu_all_reset_flag_clear();
// printf("cause:%d\r\n", cause);
return cause;
}
}
Robot_V2/sys/initial.h
+1 -9
View File
@@ -9,17 +9,9 @@ extern "C" {
#include "stdint.h"
#include <stdio.h>
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函数以重载标准库实现
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();
Robot_V2/sys/systick.h
-4
View File
@@ -48,10 +48,6 @@ void delay_1ms(uint32_t count);
void delay_1s(uint32_t count);
/* delay decrement */
void delay_decrement(void);
/* 空闲任务计数 */
void idle_task_count(void);
/* 获取CPU使用率 */
uint8_t get_cpu_usage(void);
/* 系统软复位 */
void system_soft_reset(void);