refactor(angle): 引入角度卡尔曼滤波器 EncoderAngleKalman
新增 EncoderAngleKalman 类以处理编码器角度噪声，改善角度反馈质量，并配合速度滤波器提供更准确的速度估算。 perf(CAN): 优化CAN数据打包与状态标识字段 改进 pack_u16_to_bytes 等函数实现方式，使用 reinterpret_cast 提高效率； 扩展 Status_code 为多个独立状态位，增强诊断能力。 feat(FOC): 添加电机锁止和停止功能 新增 drv_stop 和 drv_hold 方法用于控制电机占空比，支持紧急制动与角度保持。优化了FOC控制器的接口设计，提升安全性和可控性。 fix(filter): 修正中值滤波器数组大小动态分配问题 将 medium_filter 函数中的排序数组改为使用传入参数 size 动态定义，避免固定大小带来的资源浪费或越界风险，提高滤波精度和稳定性。 chore(project): 移除冗余文件及代码清理 移除了 thermal.cpp/h 文件及相关引用，调整项目结构，删除未使用的全局变量定义、注释和头文件包含，精简系统初始化流程。
This commit is contained in:
@@ -20,6 +20,7 @@ constexpr float DIR = 1.0;
|
||||
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
|
||||
constexpr float zero_electric_angle = 0.0f;
|
||||
|
||||
FOC_Controlroller FOC; // FOC控制初始化
|
||||
extern sampler sampler;
|
||||
extern angle_sensor KTH7823;
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
@@ -93,6 +94,22 @@ void FOC_Controlroller::shut_down() {
|
||||
is_shut_down = 1;
|
||||
gpio_bit_reset(GPIOB, GPIO_PIN_12);
|
||||
}
|
||||
/**
|
||||
* @brief 占空比置0
|
||||
*
|
||||
*/
|
||||
void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
|
||||
/**
|
||||
* @brief 电机锁止
|
||||
*/
|
||||
void FOC_Controlroller::drv_hold(float duty) {
|
||||
if (duty > 0.95f) {
|
||||
duty = 0.95f;
|
||||
} else if (duty < 0.0f) {
|
||||
duty = 0.0f;
|
||||
}
|
||||
Set_3Pwm(duty, duty, duty);
|
||||
}
|
||||
/**
|
||||
* @brief 电机恢复
|
||||
*
|
||||
@@ -113,8 +130,7 @@ void FOC_Controlroller::update_power(void) {
|
||||
voltage_error = false;
|
||||
v_half = voltage_power_supply * 0.5f;
|
||||
v_safe = voltage_power_supply * 0.95f;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
voltage_error = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -28,6 +28,8 @@ class FOC_Controlroller {
|
||||
void DFOC_alignSensor(int _PP);
|
||||
void shut_down(void);
|
||||
void resume(void);
|
||||
void drv_stop(void);
|
||||
void drv_hold(float duty = 0.5f);
|
||||
|
||||
protected:
|
||||
std::u64 last_time_us = 0;
|
||||
@@ -42,9 +44,9 @@ class FOC_Controlroller {
|
||||
// voltage_power_supply};//8V
|
||||
// PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000,
|
||||
// voltage_power_supply}; // 8V
|
||||
PIDController vel_loop = PIDController{1.0f, 0.85f, 0.0002f, 1000000,
|
||||
PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000,
|
||||
voltage_power_supply}; // 12V
|
||||
PIDController angle_loop = PIDController{70.0f, 75.0f, 0.0001f, 10000000,
|
||||
PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
|
||||
voltage_power_supply}; // 12V
|
||||
|
||||
void setPhaseVoltage(float Uq, float Ud);
|
||||
|
||||
@@ -1,53 +1,14 @@
|
||||
#include "Filter.h"
|
||||
#include "Can.h"
|
||||
#include "ang_sensor.h"
|
||||
#include "gd32c10x_can.h"
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
extern angle_sensor KTH7823;
|
||||
|
||||
EncoderVelocityKalman Vel_kalman(KTH7823.getVelocity(), 1e-5f, 1e-6f, 0.05f);
|
||||
|
||||
// 队列实现
|
||||
void CircularBuffer::enqueue(const float value) {
|
||||
buffer[head] = value;
|
||||
head = (head + 1) % HISTORY_SIZE;
|
||||
if (count < HISTORY_SIZE)
|
||||
count++;
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 获取操作 - O(1)
|
||||
\param[out] float 最新的元素
|
||||
\retval 最新的元素值
|
||||
\note 注意：此操作不删除元素，只是返回最新的元素
|
||||
*/
|
||||
float CircularBuffer::get(size_t index) const {
|
||||
if (index >= count)
|
||||
return buffer[head]; // 返回最新元素
|
||||
size_t pos = (head + HISTORY_SIZE - 1 - index) % HISTORY_SIZE;
|
||||
return buffer[pos];
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 获取最新的5个数值
|
||||
\param[out] latest_values: 指向存储最新5个数值的数组的指针
|
||||
\retval none
|
||||
\note 数组必须至少能容纳5个float元素
|
||||
*/
|
||||
void CircularBuffer::getLatestFive(float *latest_values) const {
|
||||
for (size_t i = 0; i < 5 && i < count; ++i) {
|
||||
latest_values[i] = get(i);
|
||||
}
|
||||
|
||||
// 如果历史数据不足5个，用同样的数字填充剩余位置
|
||||
for (size_t i = count; i < 5; ++i) {
|
||||
latest_values[i] = latest_values[count - 1];
|
||||
}
|
||||
}
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
constexpr u8 SIZE_FLOAT = sizeof(float);
|
||||
|
||||
/*!
|
||||
\brief 中值滤波器函数
|
||||
@@ -58,8 +19,8 @@ void CircularBuffer::getLatestFive(float *latest_values) const {
|
||||
*/
|
||||
float medium_filter(float values[], u8 size) {
|
||||
// 使用局部数组避免动态内存分配
|
||||
float sorted[HISTORY_SIZE];
|
||||
memcpy(sorted, values, size * sizeof(float));
|
||||
float sorted[size];
|
||||
memcpy(sorted, values, size * SIZE_FLOAT);
|
||||
|
||||
// 这比完整排序更高效，时间复杂度从O(n log n)降低到O(n)
|
||||
std::nth_element(sorted, sorted + size / 2, sorted + size);
|
||||
@@ -168,4 +129,76 @@ void EncoderVelocityKalman::setProcessNoise(float q) {
|
||||
*/
|
||||
void EncoderVelocityKalman::setMeasurementNoise(float r) {
|
||||
kalman.setMeasurementNoise(r);
|
||||
}
|
||||
|
||||
// EncoderAngleKalman类的实现
|
||||
/*!
|
||||
\brief 构造函数，初始化编码器角度卡尔曼滤波器参数
|
||||
\param[in] initial_angle 初始角度值 (degree)
|
||||
\param[in] initial_error_covariance 初始误差协方差
|
||||
\param[in] process_noise 过程噪声协方差
|
||||
\param[in] measurement_noise 测量噪声协方差
|
||||
\retval none
|
||||
*/
|
||||
EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
|
||||
float initial_error_covariance,
|
||||
float process_noise,
|
||||
float measurement_noise)
|
||||
: kalman(initial_angle, initial_error_covariance, process_noise,
|
||||
measurement_noise),
|
||||
last_angle(initial_angle), initialized(true) {}
|
||||
|
||||
|
||||
/*!
|
||||
\brief 更新卡尔曼滤波器状态
|
||||
\param[in] measured_angle 测量的角度值 (degree)
|
||||
\retval 估计的角度值 (degree)
|
||||
\note 实现卡尔曼滤波的预测和更新步骤，处理角度环绕问题
|
||||
*/
|
||||
float EncoderAngleKalman::update(float measured_angle) {
|
||||
// 更新last_angle以保持同步
|
||||
last_angle = kalman.getState();
|
||||
// 1. 预测步骤 - 假设角度符合随机游走模型（角速度为0）
|
||||
// 由于角度变化相对缓慢，我们可以简单预测角度保持不变
|
||||
kalman.predict(kalman.getState());
|
||||
|
||||
// 2. 更新步骤 - 使用处理后的角度值更新
|
||||
float filtered_angle = kalman.update(measured_angle);
|
||||
|
||||
|
||||
return filtered_angle;
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 获取当前角度估计值
|
||||
\retval 当前角度估计值 (degree)
|
||||
*/
|
||||
float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
|
||||
/*!
|
||||
\brief 获取当前角度估计值
|
||||
\retval 当前角度估计值 (degree)
|
||||
*/
|
||||
float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*1e4; }
|
||||
/*!
|
||||
\brief 获取前一个角度估计值
|
||||
\retval 前一个角度估计值 (degree)
|
||||
*/
|
||||
float EncoderAngleKalman::getlastAngle() const { return last_angle; }
|
||||
|
||||
/*!
|
||||
\brief 设置过程噪声协方差
|
||||
\param[in] q 过程噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void EncoderAngleKalman::setProcessNoise(float q) {
|
||||
kalman.setProcessNoise(q);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 设置测量噪声协方差
|
||||
\param[in] r 测量噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void EncoderAngleKalman::setMeasurementNoise(float r) {
|
||||
kalman.setMeasurementNoise(r);
|
||||
}
|
||||
+27
-16
@@ -18,21 +18,6 @@ extern "C" {
|
||||
#include <cstring>
|
||||
#include <stdint.h>
|
||||
|
||||
constexpr u8 HISTORY_SIZE = 10; // 历史数据大小
|
||||
|
||||
// 环形缓冲区类，用于存储历史数据
|
||||
class CircularBuffer {
|
||||
private:
|
||||
std::array<float, HISTORY_SIZE> buffer;
|
||||
size_t head = 0;
|
||||
size_t count = 0;
|
||||
|
||||
public:
|
||||
void enqueue(float value);
|
||||
float get(size_t index) const;
|
||||
void getLatestFive(float *latest_values) const;
|
||||
};
|
||||
|
||||
float medium_filter(float values[], u8 size);
|
||||
|
||||
/*!
|
||||
@@ -125,7 +110,7 @@ template <typename T> class KalmanFilter {
|
||||
\note 基于电机模型的卡尔曼滤波器实现，用于电流状态估计
|
||||
\note
|
||||
// 创建卡尔曼滤波器实例
|
||||
CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
|
||||
CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.02f, 1e-6f, 0.02f);
|
||||
// 更新滤波器状态并获取估计值
|
||||
float estimatedCurrent = kalman.update(measuredCurrent, appliedVoltage);
|
||||
*/
|
||||
@@ -171,6 +156,32 @@ class EncoderVelocityKalman {
|
||||
void setMeasurementNoise(float r);
|
||||
};
|
||||
|
||||
/*!
|
||||
\brief 编码器角度卡尔曼滤波器类
|
||||
\note 用于滤除编码器角度测量噪声的卡尔曼滤波器实现
|
||||
*/
|
||||
class EncoderAngleKalman {
|
||||
private:
|
||||
KalmanFilter<float> kalman;
|
||||
float last_angle;
|
||||
bool initialized;
|
||||
|
||||
public:
|
||||
EncoderAngleKalman(float initial_angle, float initial_error_covariance,
|
||||
float process_noise, float measurement_noise);
|
||||
|
||||
float update(float measured_angle);
|
||||
|
||||
float getAngle() const;
|
||||
float getlastAngle() const;
|
||||
float getVelocity() const;
|
||||
|
||||
|
||||
void setProcessNoise(float q);
|
||||
|
||||
void setMeasurementNoise(float r);
|
||||
};
|
||||
|
||||
#endif // __cplusplus
|
||||
|
||||
#endif // _FILTER_H
|
||||
@@ -1,41 +0,0 @@
|
||||
#include "thermal.h"
|
||||
#include "Can.h"
|
||||
#include "DengFOC.h"
|
||||
#include "adc.h"
|
||||
|
||||
extern sampler sampler;
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
extern FOC_Controlroller FOC;
|
||||
#define SQUARE(x) ((x) * (x))
|
||||
#define max(a, b, c) \
|
||||
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
|
||||
|
||||
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
|
||||
u32 ThermalController::update() {
|
||||
thermal_value = thermal_value * thermalRate;
|
||||
thermal_value +=
|
||||
SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp());
|
||||
|
||||
if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热
|
||||
if (thermal_value < thermal_capacity * recoveryRate) {
|
||||
remainingTime10Ms = 1; // 可以恢复工作
|
||||
}
|
||||
if (remainingTime10Ms == 1) { // 过热恢复
|
||||
FOC.resume();
|
||||
return --remainingTime10Ms;
|
||||
}
|
||||
return --remainingTime10Ms;
|
||||
} else { // 当前没有过热停机
|
||||
current_max =
|
||||
1000 * max(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W')); // 获取电流最大值
|
||||
thermal_value +=
|
||||
SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值
|
||||
}
|
||||
if (thermal_value > thermal_capacity) {
|
||||
FOC.shut_down();
|
||||
remainingTime10Ms = PROTECT_TIME; // 过热停机10秒
|
||||
return remainingTime10Ms;
|
||||
}
|
||||
return thermal_value;
|
||||
}
|
||||
@@ -1,27 +1,7 @@
|
||||
#ifndef THERMAL_H
|
||||
#define THERMAL_H
|
||||
#include "stdint.h"
|
||||
|
||||
|
||||
class ThermalController {
|
||||
private:
|
||||
u8 temperature;
|
||||
u16 current_max;
|
||||
|
||||
float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例）
|
||||
float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作）
|
||||
|
||||
u32 thermal_value = 0;
|
||||
u32 thermal_capacity = 2250000; // 热容量
|
||||
|
||||
u16 gapTimeMs = 10; // 单次计算间隔ms
|
||||
u16 remainingTime10Ms = 0; // 过热冷却剩余ms
|
||||
public:
|
||||
u32 update();
|
||||
void reset() {
|
||||
thermal_value = 0;
|
||||
remainingTime10Ms = 0;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user