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

feat(FOC): 实现电流环控制并优化相关逻辑

Browse Source 
新增了电流环控制功能，包括通过三相电流计算 Iq 的 `cal_Iq_Id` 方法，并在 `current_control` 中实现了基于 PID 的电流控制逻辑。同时对电压设置、占空比计算等部分进行了调整与优化。

refactor(filter): 移除冗余的电流卡尔曼滤波器实现
删除了 `Filter.cpp` 和 `Filter.h` 中关于 `CurrentKalman` 类的冗余代码，统一外部定义，简化结构。

refactor(health): 改进热量控制器算法和数据类型
将热累积计算改为更精确的分段处理方式，并引入 Q16.16 定点数提升运算精度与性能；增加对异常温度值的判断修正。

fix(CAN): 调整电流发送顺序及标志位设置位置
修改 CAN 发送中 V 相与 W 相电流打包顺序，确保通信正确性；调整发送标志设置逻辑至主循环任务内触发。

chore(headers): 清理无用头文件引用和结构调整
移除了多个源文件中不必要的头文件包含（如 `<cstdio>`、`<array>`），并对部分全局变量声明做了 extern 处理以避免重复定义问题。
This commit is contained in:
cbb
2025-12-05 00:46:19 +08:00
parent 871e33e10f
commit 93cbf18f1f
11 changed files with 152 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+45 -13
View File
@@ -1,6 +1,5 @@
#include "DengFOC.h"
#include "Can.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
@@ -27,9 +26,6 @@ extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
// 初始变量及函数定义
#define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
@@ -47,6 +43,27 @@ inline float _normalizeAngle(float angle) {
// 取余运算可以用于归一化，列出特殊值例子算便知
return a >= 0 ? a : (a + 2 * PI);
}
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
float current_c) {
// 计算电气角度
float angle_el = _normalizeAngle(shaft_angle * PP);
// Clarke变换 (使用三相电流)
float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
(1.0f / 3.0f) * current_c;
float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
// Park变换
float sin_el, cos_el;
cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
float I_q = I_beta * cos_el - I_alpha * sin_el;
return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA
}
/**
* @brief 电机控制类构造函数
*
@@ -77,15 +94,15 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
// 计算占空比并限制范围
float dc_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
float dc_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
float dc_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
duty_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
duty_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
duty_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
// 设置PWM输出
float voltage_exchange = limit_voltage / v_safe;
dc_a = dc_a * voltage_exchange;
dc_b = dc_b * voltage_exchange;
dc_c = dc_c * voltage_exchange;
Set_3Pwm(dc_a, dc_b, dc_c);
duty_a = duty_a * voltage_exchange;
duty_b = duty_b * voltage_exchange;
duty_c = duty_c * voltage_exchange;
Set_3Pwm(duty_a, duty_b, duty_c);
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
}
@@ -94,7 +111,6 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
*
*/
void FOC_Controlroller::shut_down() {
thermal_uncontrol = 0;
is_shut_down = 1;
gpio_bit_reset(GPIOB, GPIO_PIN_12);
}
@@ -119,7 +135,7 @@ void FOC_Controlroller::drv_hold(float duty) {
*
*/
void FOC_Controlroller::resume() {
thermal_uncontrol = false;
over_current = false;
voltage_error = false;
is_shut_down = false;
gpio_bit_set(GPIOB, GPIO_PIN_12);
@@ -155,6 +171,8 @@ float FOC_Controlroller::velocity_open_loop() {
const float Uq = voltage_power_supply / 2.0f;
setPhaseVoltage(Uq, 0.0f);
CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
return limit_voltage;
}
@@ -223,5 +241,19 @@ float FOC_Controlroller::velocity_control(float target_velocity) {
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
return rt_velocity;
}
float FOC_Controlroller::current_control(float target_current) {
// PID 控制计算
float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
float Uq = current_loop(target_current - Iq);
CAN0_transmiter.testfloat_union.f = Iq;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return 1;
}
Robot_V2/Algorithm/DengFOC.h
+10 -13
View File
@@ -16,14 +16,17 @@ class FOC_Controlroller {
float target_velocity = 0.0f;
float rt_angle = 0.0f;
float rt_velocity = 0.0f;
bool thermal_uncontrol = false;
bool over_current = false;
bool voltage_error = false;
bool is_shut_down = false;
float voltage_power_supply = 24.0f; // 电源电压
float v_safe = voltage_power_supply * 1.05f;
float duty_a = 0, duty_b = 0, duty_c = 0;
float Ua = 0, Ub = 0, Uc = 0;
float velocity_open_loop();
float angle_control(float target_angle);
float velocity_control(float target_velocity);
float current_control(float target_current);
float angle_control_NPID(float target_angle);
void update_power(void);
@@ -34,31 +37,25 @@ class FOC_Controlroller {
void drv_hold(float duty = 0.5f);
protected:
std::u64 last_time_us = 0;
std::u64 now_us = 0;
float shaft_angle = 0;
float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
float Ualpha, Ubeta = 0;
float zero_electric_angle = 0;
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
float v_safe = voltage_power_supply * 0.95f;
// PIDController vel_loop =PIDController{1.8f, 1.2f, 0.0002f, 1000000,
// voltage_power_supply};//8V
// PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000,
// voltage_power_supply}; // 8V
PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000,
voltage_power_supply}; // 12V
PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
voltage_power_supply}; // 12V
PIDController current_loop =
PIDController{0.01f, 0.001f, 0, 100000, voltage_power_supply}; // 12V
void setPhaseVoltage(float Uq, float Ud);
float cal_Iq_Id(float current_a, float current_b, float current_c);
};
float _normalizeAngle(float angle);
void DFOC_Vbus(float power_supply);
inline float _electricalAngle();
float DFOC_M0_Angle(void);
constexpr u16 LUT_SIZE = 1024;
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
Robot_V2/Algorithm/Filter.cpp
+6 -55
View File
@@ -1,13 +1,15 @@
#include "Filter.h"
#include "Can.h"
#include "ang_sensor.h"
#include "gd32c10x_can.h"
#include <algorithm>
#include <cstring>
#include <stdint.h>
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f);
constexpr u8 SIZE_FLOAT = sizeof(float);
/*!
@@ -28,56 +30,6 @@ float medium_filter(float values[], u8 size) {
return sorted[size / 2]; // 返回中值
}
// CurrentKalman类的实现
/*!
\brief 构造函数，初始化卡尔曼滤波器参数
\param[in] initial_current 初始电流值
\param[in] L 电机电感(H) 0.0026H
\param[in] R 电机电阻(Ω) 4.8 Ω
\param[in] T 控制周期(s) 1e -4
\param[in] Q 过程噪声协方差(典型值: 1e-6 ~ 1e-4)
\param[in] R_noise 测量噪声协方差(典型值: 电流范围的0.5%~2%)
\retval none
*/
CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T,
float Q, float R_noise)
: kalman(initial_current, 1.0f, Q, R_noise), L(L), R_m(R), T(T) {}
/*!
\brief 更新卡尔曼滤波器状态
\param[in] measured_value 测量值
\param[in] applied_voltage 应用电压值
\retval 估计的电流值
\note 实现卡尔曼滤波的预测和更新步骤
*/
float CurrentKalman::update(float measured_value, float applied_voltage) {
// 1. 预测步骤 - 基于电机模型
float predicted_current = kalman.getState() + T * (applied_voltage - R_m * kalman.getState()) / L;
// 2. 预测误差协方差已经在kalman.predict中处理
// 3. 执行预测
kalman.predict(predicted_current);
// 4. 更新步骤 - 直接使用测量值更新
return kalman.update(measured_value);
}
/*!
\brief 获取当前电流估计值
\retval 当前电流估计值
*/
float CurrentKalman::getCurrent() const { return kalman.getState(); }
/*!
\brief 设置过程噪声协方差
\param[in] q 过程噪声协方差值
\retval none
*/
void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
/*!
\brief 设置测量噪声协方差
\param[in] r 测量噪声协方差值
\retval none
*/
void CurrentKalman::setMeasurementNoise(float r) { kalman.setMeasurementNoise(r); }
// EncoderVelocityKalman类的实现
/*!
@@ -109,7 +61,7 @@ float EncoderVelocityKalman::update(float measured_velocity) {
/*!
\brief 获取当前速度估计值
\retval 当前速度估计值 (rad/s)
\retval 当前速度估计值 (deg/s)
*/
float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); }
@@ -158,9 +110,8 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
float EncoderAngleKalman::update(float measured_angle) {
// 更新last_angle以保持同步
last_angle = kalman.getState();
// 1. 预测步骤 - 假设角度符合随机游走模型（角速度为0）
// 由于角度变化相对缓慢，我们可以简单预测角度保持不变
kalman.predict(kalman.getState());
// 1. 预测步骤 - 速度估计
kalman.predict(last_angle);
// 2. 更新步骤 - 使用处理后的角度值更新
float filtered_angle = kalman.update(measured_angle);
Robot_V2/Algorithm/Filter.h
+9 -38
View File
@@ -1,7 +1,6 @@
#ifndef _FILTER_H
#define _FILTER_H
#include <cstdio>
#ifdef __cplusplus
extern "C" {
#endif
@@ -14,7 +13,6 @@ extern "C" {
#endif
#ifdef __cplusplus
#include <array>
#include <cstring>
#include <stdint.h>
@@ -26,7 +24,7 @@ float medium_filter(float values[], u8 size);
*/
template <typename T> class KalmanFilter {
private:
T x; // 状态估计
T xp; // 状态估计
T P; // 估计误差协方差
T Q; // 过程噪声协方差
T R; // 测量噪声协方差
@@ -41,7 +39,7 @@ template <typename T> class KalmanFilter {
*/
KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
T measurement_noise)
: x(initial_state), P(initial_error_covariance), Q(process_noise),
: xp(initial_state), P(initial_error_covariance), Q(process_noise),
R(measurement_noise) {}
/*!
@@ -50,7 +48,7 @@ template <typename T> class KalmanFilter {
\retval none
*/
void predict(T state_prediction) {
x = state_prediction;
xp = state_prediction;
P += Q;
}
@@ -65,10 +63,10 @@ template <typename T> class KalmanFilter {
T S = P + R; // 残差协方差
T K = P / S; // 卡尔曼增益
x += K * y; // 更新状态估计
xp += K * y; // 更新状态估计
P = (1 - K) * P; // 更新误差协方差
return x;
return xp;
}
/*!
@@ -79,16 +77,16 @@ template <typename T> class KalmanFilter {
T update(T measurement) {
T K = P / (P + R); // 卡尔曼增益
x += K * (measurement - x); // 更新状态估计
xp += K * (measurement - xp); // 更新状态估计
P = (1 - K) * P; // 更新误差协方差
return x;
return xp;
}
/*!
\brief 获取当前状态估计值
\retval 当前状态估计值
*/
T getState() const { return x; }
T getState() const { return xp; }
/*!
\brief 设置过程噪声协方差
@@ -105,34 +103,6 @@ template <typename T> class KalmanFilter {
void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
};
/*!
\brief 电流卡尔曼滤波器类
\note 基于电机模型的卡尔曼滤波器实现，用于电流状态估计
\note
// 创建卡尔曼滤波器实例
CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.02f, 1e-6f, 0.02f);
// 更新滤波器状态并获取估计值
float estimatedCurrent = kalman.update(measuredCurrent, appliedVoltage);
*/
class CurrentKalman {
private:
KalmanFilter<float> kalman;
float L; // 电机电感(H)
float R_m; // 电机电阻(Ω)
float T; // 控制周期(s)
public:
CurrentKalman(float initial_current, float L, float R, float T, float Q,
float R_noise);
float update(float measured_current, float applied_voltage);
float getCurrent() const;
void setProcessNoise(float q);
void setMeasurementNoise(float r);
};
/*!
\brief 编码器速度卡尔曼滤波器类
@@ -182,6 +152,7 @@ public:
void setMeasurementNoise(float r);
};
#endif // __cplusplus
#endif // _FILTER_H
Robot_V2/device/ADC.cpp
+9 -1
View File
@@ -1,5 +1,6 @@
#include "adc.h"
#include "Can.h"
#include "DengFOC.h"
#include "Filter.h"
#include "math.h"
#include <cstring>
@@ -20,8 +21,8 @@ constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
\brief configure the DMA peripheral
@@ -171,14 +172,17 @@ float sampler::get_drv_temp() {
float sampler::get_current(char type) {
switch (type) {
case 'U': {
// return currU_samples[current_sample_index];
tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1);
break;
}
case 'V': {
// return currV_samples[current_sample_index];
tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1);
break;
}
case 'W': {
// return currW_samples[current_sample_index];
tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1);
break;
}
@@ -189,6 +193,7 @@ float sampler::get_current(char type) {
if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) {
return ERROR_ADC_VAL;
}
return tempora_sample;
}
@@ -238,6 +243,7 @@ void sampler::update_drv_temp() {
*/
void sampler::update_current() {
u8 idx = current_sample_index;
extern FOC_Controlroller FOC;
// 优化计算
const float scale = ADC_SCALE * CURRENT_FACTOR;
const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
@@ -251,6 +257,8 @@ void sampler::update_current() {
if (current_sample_index >= FILTER_SIZE - 1) {
current_sample_index = 0;
}
// 卡尔曼滤波更新
float voltage = FOC.voltage_power_supply;
}
/**
Robot_V2/device/Can.cpp
+3 -3
View File
@@ -445,10 +445,10 @@ void CAN_transmiter::update(void) {
Target_Control_Mode = FOC.control_mode;
controller_Mode = FOC.control_mode;
// 使用简单平均滤波的电流值
u_current_union.f = sampler.get_current('U') * 1000;
w_current_union.f = sampler.get_current('W') * 1000;
v_current_union.f = sampler.get_current('V') * 1000;
// testfloat_union.f = KTH7823.getVelocity();
}
/*!
@@ -624,8 +624,8 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_u16_to_bytes(transmit_data, 0, mcu_temp);
pack_u16_to_bytes(transmit_data, 2, motor_temp);
pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 6, u16(w_current_union.f));
pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f));
pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f));
pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f));
transmit_data[10] = (transmit_data[10] & ~0x07) |
((MotorStatus & 0x01) |
((MagStatus & 0x01) << 1) |
Robot_V2/device/Timer.cpp
-2
View File
@@ -8,7 +8,6 @@
#include "gd32c10x_rcu.h"
#include "gd32c10x_timer.h"
#include "initial.h"
#include <cstdio>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
@@ -139,7 +138,6 @@ void TIMER2_IRQHandler(void) {
break;
}
case 9: {
CAN0_transmiter.can0_send_flag = SET;
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
break;
}
Robot_V2/device/ang_sensor.cpp
+2 -2
View File
@@ -27,8 +27,8 @@ volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
angle_sensor KTH7823; // 角度传感器初始化
EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f);
extern EncoderAngleKalman Angle_kalman;
extern EncoderVelocityKalman Vel_kalman;
/**
* @brief SPI DMA initialization
Robot_V2/main.cpp
+8 -3
View File
@@ -12,9 +12,9 @@
#include "sys/health.h"
#include "sys/systick.h"
#include "systick.h"
#include <stdint.h>
TaskScheduler scheduler = {0};
int main(void) {
systick_config(); // 系统时钟初始化
USART0_init(); // 调试串口初始化
@@ -28,6 +28,7 @@ int main(void) {
extern CAN_transmiter CAN0_transmiter; // CAN
extern ThermalController thermalController; // 热量控制器
u32 cnt_test = 0;
delay_1ms(5);
angsens_MG_check(); // 角度传感器磁感应强度测试
watchdog_init(5000); // 看门狗初始化
@@ -38,7 +39,7 @@ int main(void) {
// 一次性清除所有已处理的任务标志
scheduler.taskFlags &= ~currentTasks;
__enable_irq();
cnt_test++;
// 按照优先级和重要性处理任务
if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
sampler.update_current();
@@ -66,7 +67,8 @@ int main(void) {
break;
}
case 3: {
FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制
// FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制
FOC.current_control(FOC.limit_velocity); // 电流闭环控制
break;
}
case 4: {
@@ -93,6 +95,9 @@ int main(void) {
FOC.update_power();
}
if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
cnt_test = 0;
CAN0_transmiter.can0_send_flag = SET;
CAN0_transmiter.communication_check();
fwdgt_counter_reload(); // 看门狗复位
}
Robot_V2/sys/health.cpp
+44 -14
View File
@@ -2,28 +2,38 @@
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"
#include <stdint.h>
extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
ThermalController thermalController; // 热量控制器初始化
#define SQUARE(x) ((x) * (x))
#define max(a, b, c) \
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
constexpr u16 PROTECT_TIME = 3000; // 过热保护时间，单位*10ms
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
/**
** @brief 热能计算
** @param None
** @retval 热量值
*/
u32 ThermalController::update() {
thermal_value = thermal_value * thermalRate;
thermal_value +=
SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp());
thermal_value = Q16_16_MUL(thermal_value, thermalRate);
// 添加温度积累
int drv_temp = (int)sampler.get_drv_temp();
int mcu_temp = (int)sampler.get_mcu_temp();
if (drv_temp == 255)
drv_temp = 0;
if (mcu_temp == 255)
mcu_temp = 0;
thermal_value += drv_temp * drv_temp + mcu_temp * mcu_temp;
if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热
if (thermal_value < thermal_capacity * recoveryRate) {
if (remainingTime10Ms) {
// 过热停机状态
// 检查是否可以恢复
q16_16_t recovery_threshold =
Q16_16_MUL(thermal_capacity, recoveryRate);
if (thermal_value < recovery_threshold) {
remainingTime10Ms = 1; // 可以恢复工作
}
if (remainingTime10Ms == 1) { // 过热恢复
@@ -32,13 +42,33 @@ u32 ThermalController::update() {
return --remainingTime10Ms;
}
return --remainingTime10Ms;
} else { // 当前没有过热停机
current_max =
1000 * max(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W')); // 获取电流最大值
thermal_value +=
SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值
} else {
// 当前没有过热停机
// 添加电流热积累
u32 currents[3] = {(u32)(sampler.get_current('U') * 1000),
(u32)(sampler.get_current('V') * 1000),
(u32)(sampler.get_current('W') * 1000)};
// 找最大电流（原始ADC值）
current_max = max(currents[0], currents[1], currents[2]);
if (current_max < 2000) {
thermal_value += current_max * current_max * gapTimeMs *
0.00007f; // 计算当前电流热积累值
} else if (current_max > 3800) {
thermal_value += current_max * current_max * gapTimeMs *
0.01f; // 添加当前电流热积累值
} else if (current_max > 3400) {
thermal_value += current_max * current_max * gapTimeMs *
0.005f; // 计算当前电流热积累值
} else if (current_max > 3000) {
thermal_value += current_max * current_max * gapTimeMs *
0.0002f; // 计算当前电流热积累值
} else if (current_max > 2000) {
thermal_value += current_max * current_max * gapTimeMs *
0.0001f; // 计算当前电流热积累值
}
}
if (thermal_value > thermal_capacity) {
FOC.shut_down();
remainingTime10Ms = PROTECT_TIME; // 过热停机
Robot_V2/sys/health.h
+15 -5
View File
@@ -2,20 +2,31 @@
#define _HEALTH_H
#include "stdint.h"
typedef int32_t q16_16_t; // Q16.16格式
// 定点数运算宏
#define Q16_16_ONE 65536 // 1.0 in Q16.16
#define FLOAT_TO_Q16_16(x) ((q16_16_t)((x) * Q16_16_ONE))
#define Q16_16_TO_FLOAT(x) ((float)(x) / Q16_16_ONE)
#define Q16_16_MUL(a, b) ((q16_16_t)(((int64_t)(a) * (b)) >> 16))
#define Q16_16_SQUARE(x) Q16_16_MUL((x), (x))
class ThermalController {
private:
u8 temperature;
u16 current_max;
float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例）
float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作）
q16_16_t thermalRate; // 热耗散系数（每单位时间散热比例）
q16_16_t recoveryRate; // 自然恢复率（散热到多少以下开始工作）
u32 thermal_value = 0;
u32 thermal_capacity = 2250000; // 热容量
u32 thermal_capacity = 3600000; // 热容量
u16 gapTimeMs = 10; // 单次计算间隔ms
u16 gapTimeMs = 10; // 单次计算间隔ms
u16 remainingTime10Ms = 0; // 过热冷却剩余ms
public:
ThermalController()
: thermalRate(FLOAT_TO_Q16_16(0.995f)),
recoveryRate(FLOAT_TO_Q16_16(0.33f)) {}
u32 update();
void reset() {
thermal_value = 0;
@@ -25,5 +36,4 @@ class ThermalController {
void angsens_MG_check(void);
#endif