From 93cbf18f1f3f2940da8c6c104ca0c141cf05613b Mon Sep 17 00:00:00 2001
From: cbb <2625274797@qq.com>
Date: Fri, 5 Dec 2025 00:46:19 +0800
Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E5=AE=9E=E7=8E=B0=E7=94=B5?=
 =?UTF-8?q?=E6=B5=81=E7=8E=AF=E6=8E=A7=E5=88=B6=E5=B9=B6=E4=BC=98=E5=8C=96?=
 =?UTF-8?q?=E7=9B=B8=E5=85=B3=E9=80=BB=E8=BE=91=20=E6=96=B0=E5=A2=9E?=
 =?UTF-8?q?=E4=BA=86=E7=94=B5=E6=B5=81=E7=8E=AF=E6=8E=A7=E5=88=B6=E5=8A=9F?=
 =?UTF-8?q?=E8=83=BD=EF=BC=8C=E5=8C=85=E6=8B=AC=E9=80=9A=E8=BF=87=E4=B8=89?=
 =?UTF-8?q?=E7=9B=B8=E7=94=B5=E6=B5=81=E8=AE=A1=E7=AE=97=20Iq=20=E7=9A=84?=
 =?UTF-8?q?=20`cal=5FIq=5FId`=20=E6=96=B9=E6=B3=95=EF=BC=8C=E5=B9=B6?=
 =?UTF-8?q?=E5=9C=A8=20`current=5Fcontrol`=20=E4=B8=AD=E5=AE=9E=E7=8E=B0?=
 =?UTF-8?q?=E4=BA=86=E5=9F=BA=E4=BA=8E=20PID=20=E7=9A=84=E7=94=B5=E6=B5=81?=
 =?UTF-8?q?=E6=8E=A7=E5=88=B6=E9=80=BB=E8=BE=91=E3=80=82=E5=90=8C=E6=97=B6?=
 =?UTF-8?q?=E5=AF=B9=E7=94=B5=E5=8E=8B=E8=AE=BE=E7=BD=AE=E3=80=81=E5=8D=A0?=
 =?UTF-8?q?=E7=A9=BA=E6=AF=94=E8=AE=A1=E7=AE=97=E7=AD=89=E9=83=A8=E5=88=86?=
 =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E4=BA=86=E8=B0=83=E6=95=B4=E4=B8=8E=E4=BC=98?=
 =?UTF-8?q?=E5=8C=96=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

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

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

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

chore(headers): 清理无用头文件引用和结构调整
移除了多个源文件中不必要的头文件包含（如 `<cstdio>`、`<array>`），并对部分全局变量声明做了 extern 处理以避免重复定义问题。
---
 Robot_V2/Algorithm/DengFOC.cpp | 58 ++++++++++++++++++++++++--------
 Robot_V2/Algorithm/DengFOC.h   | 25 ++++++--------
 Robot_V2/Algorithm/Filter.cpp  | 61 ++++------------------------------
 Robot_V2/Algorithm/Filter.h    | 47 +++++---------------------
 Robot_V2/device/ADC.cpp        | 10 +++++-
 Robot_V2/device/Can.cpp        |  6 ++--
 Robot_V2/device/Timer.cpp      |  2 --
 Robot_V2/device/ang_sensor.cpp |  4 +--
 Robot_V2/main.cpp              | 11 ++++--
 Robot_V2/sys/health.cpp        | 58 ++++++++++++++++++++++++--------
 Robot_V2/sys/health.h          | 20 ++++++++---
 11 files changed, 152 insertions(+), 150 deletions(-)

diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp
index 3580c04..9fa6d01 100644
--- a/Robot_V2/Algorithm/DengFOC.cpp
+++ b/Robot_V2/Algorithm/DengFOC.cpp
@@ -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;
 }
\ No newline at end of file
diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h
index 446c891..b5dbde9 100644
--- a/Robot_V2/Algorithm/DengFOC.h
+++ b/Robot_V2/Algorithm/DengFOC.h
@@ -5,7 +5,7 @@
 #include <stdint.h>
 
 class FOC_Controlroller {
-	
+
   public:
 	FOC_Controlroller();
 
@@ -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);
diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp
index c234abf..ca40507 100644
--- a/Robot_V2/Algorithm/Filter.cpp
+++ b/Robot_V2/Algorithm/Filter.cpp
@@ -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);
diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h
index 9d8dbc5..d34cc47 100644
--- a/Robot_V2/Algorithm/Filter.h
+++ b/Robot_V2/Algorithm/Filter.h
@@ -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
\ No newline at end of file
diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp
index 32017f1..eb11ec8 100644
--- a/Robot_V2/device/ADC.cpp
+++ b/Robot_V2/device/ADC.cpp
@@ -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;
 }
 
 /**
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index 28647eb..cc4a8a0 100644
--- a/Robot_V2/device/Can.cpp
+++ b/Robot_V2/device/Can.cpp
@@ -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) |
diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp
index 431df26..d8a18f1 100644
--- a/Robot_V2/device/Timer.cpp
+++ b/Robot_V2/device/Timer.cpp
@@ -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;
 		}
diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp
index a82d181..4f39520 100644
--- a/Robot_V2/device/ang_sensor.cpp
+++ b/Robot_V2/device/ang_sensor.cpp
@@ -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
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index 0468896..72bb07e 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -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(); // 看门狗复位
 		}
diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp
index e931133..4ef4277 100644
--- a/Robot_V2/sys/health.cpp
+++ b/Robot_V2/sys/health.cpp
@@ -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; // 过热停机
diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h
index 66bbb65..3dfd298 100644
--- a/Robot_V2/sys/health.h
+++ b/Robot_V2/sys/health.h
@@ -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