feat(FOC): 添加速度目标突变检测和积分清零机制

- 在velocity_control()函数中添加目标速度突变检测，当速度变化
  超过5.0f时自动清零积分项
- 在angle_control_with_velocity_limit()函数中添加相同的速度
  突变检测逻辑
- 在MIT_Mode()函数中实现速度环积分清零功能
- 添加reset_integral()方法到PID控制器用于清零积分项

refactor(Filter): 将角度卡尔曼滤波器替换为α-β观测器

- 重构EncoderAngleKalman类，将内部卡尔曼滤波器改为α-β观
  测器算法
- 保留原有的接口名称和参数以维持兼容性
- 添加ANGLE_OBSERVER_ALPHA和ANGLE_OBSERVER_BETA配置参数
- 修改速度获取逻辑，直接从观测器状态获取速度估计值

perf(systick): 实现基于DWT的CPU使用率监测

- 使用DWT周期计数器精确测量任务执行周期
- 添加busy_task_enter()和busy_task_exit()函数追踪忙碌周期
- 实现每秒统计CPU占用率并提供get_cpu_usage()接口
- 在CAN数据包中添加CPU使用率传输字段

fix(Can): 修复CANFD数据包中的整数打包问题

- 将16位整数正确拆分为两个字节进行传输
- 修正了test_u16数据的字节序打包方式

chore(config): 更新软件版本号并优化配置参数

- 将版本号从26.4.26更新到26.5.17
- 调整角度/速度α-β观测器的相关增益参数

Robot_V2/Algorithm/FOC.cpp

@@ -448,10 +448,19 @@ float FOC_Controller::angle_control() {
  * @return float: 当前速度
  */
 float FOC_Controller::velocity_control() {
-	// PID 控制计算，使用 DIR 参数调整速度误差的符号
-	// DIR = -1.0f 时：速度误差 = -(target_velocity - rt_velocity) = rt_velocity - target_velocity
-	// DIR = 1.0f 时：速度误差 = target_velocity - rt_velocity
-	float Uq = vel_loop((target_velocity - rt_velocity) * DIR * REDUCTION_RATIO);
+	// 速度误差计算
+	float velocity_error = (target_velocity - rt_velocity) * DIR * REDUCTION_RATIO;
+
+	// 目标速度突变检测：target 大幅变化时清积分
+	static float last_target_vel = 0.0f;
+	const float target_delta = target_velocity - last_target_vel;
+	if (fabsf(target_delta) > 5.0f) {
+		vel_loop.reset_integral();
+	}
+	last_target_vel = target_velocity;
+
+	// PID 控制计算
+	float Uq = vel_loop(velocity_error);
 	CAN0_transmiter.testfloat_union.f = Uq;
 
 	// 获取电机轴角度
@@ -507,7 +516,19 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	} else {
 		vel_inVelAngloop.set_P(vel_angloop_p);
 	}
-	float Uq = vel_inVelAngloop(target_velocity - rt_velocity)* DIR;
+	float velocity_error = target_velocity - rt_velocity;
+
+	// 目标速度突变检测：target 大幅变化时清积分
+	static float last_vang_target_vel = 0.0f;
+	if (fabsf(target_velocity - last_vang_target_vel) > 5.0f) {
+		vel_inVelAngloop.reset_integral();
+	}
+	last_vang_target_vel = target_velocity;
+
+	static float last_velocity_error = 0.0f;
+	last_velocity_error = velocity_error;
+
+	float Uq = vel_inVelAngloop(velocity_error)* DIR;
 	CAN0_transmiter.testfloat_union.f = Uq;
 
 	// // 实现力度控制：如果电流超过目标电流，则限制输出
@@ -549,6 +570,14 @@ float FOC_Controller::MIT_Mode() {
 	// 第二层：速度环（中环）
 	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
 	float velocity_error = (target_velocity - rt_velocity)* DIR;
+
+	// 目标速度变化检测：target 大幅变化时清积分
+	static float last_mit_target_vel = 0.0f;
+	if (fabsf(target_velocity - last_mit_target_vel) > 5.0f) {
+		vel_inMITloop.reset_integral();
+	}
+	last_mit_target_vel = target_velocity;
+
 	vel_inMITloop.set_P(vel_MIT_p);
 	float target_current = vel_inMITloop(velocity_error);
 

Robot_V2/Algorithm/Filter.cpp

@@ -95,79 +95,82 @@ void EncoderVelocityKalman::setMeasurementNoise(float r) {
 
 // EncoderAngleKalman类的实现
 /*!
-	\brief      构造函数，初始化编码器角度卡尔曼滤波器参数
+	\brief      构造函数，初始化编码器角度 α-β 观测器参数
 	\param[in]  initial_angle 初始角度值 (degree)
-	\param[in]  initial_error_covariance 初始误差协方差
-	\param[in]  process_noise 过程噪声协方差
-	\param[in]  measurement_noise 测量噪声协方差
+	\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),
-	  velocity_last_angle(initial_angle),
-	  velocity_last_us50_counter(motor_runtime_50us) {}
+						   float initial_error_covariance,
+						   float process_noise,
+						   float measurement_noise)
+	: angle_estimate(initial_angle), velocity_estimate(0.0f),
+	  alpha_gain(ANGLE_OBSERVER_ALPHA), beta_gain(ANGLE_OBSERVER_BETA),
+	  last_update_count(0U), initialized(false) {
+	(void)initial_error_covariance;
+	(void)process_noise;
+	(void)measurement_noise;
+}
 
 /*!
-	\brief      更新卡尔曼滤波器状态
+	\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 {
-	float angle_now = kalman.getState();
-	u32 time_counts = motor_runtime_50us - velocity_last_us50_counter;
-
-	float vel = 0.0f;
-	if (time_counts > 0) {
-		vel = (angle_now - velocity_last_angle) * TIMER2_FREQUENCY /
-			  time_counts ;
+	const u32 now_counts = motor_runtime_50us;
+	if (!initialized) {
+		angle_estimate = measured_angle;
+		velocity_estimate = 0.0f;
+		last_update_count = now_counts;
+		initialized = true;
+		return angle_estimate;
 	}
 
-	velocity_last_angle = angle_now;
-	velocity_last_us50_counter = motor_runtime_50us;
+	u32 dt_counts = now_counts - last_update_count;
+	last_update_count = now_counts;
+	if (dt_counts == 0U) {
+		dt_counts = 1U;
+	}
+	const float dt = TIME_SLICE * static_cast<float>(dt_counts);
+	const float angle_pred = angle_estimate + velocity_estimate * dt;
+	const float velocity_pred = velocity_estimate;
+	const float error = measured_angle - angle_pred;
 
-	return vel;
+	angle_estimate = angle_pred + alpha_gain * error;
+	velocity_estimate = velocity_pred + (beta_gain / dt) * error;
+	return angle_estimate;
 }
 
 /*!
-	\brief      设置过程噪声协方差
-	\param[in]  q 过程噪声协方差值
-	\retval     none
+	\brief      获取当前角度估计值
+	\retval     当前角度估计值 (degree)
 */
-void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
+float EncoderAngleKalman::getAngle() const { return angle_estimate; }
+/*!
+	\brief      获取当前速度估计值
+	\retval     当前速度估计值 (degree/s)
+*/
+float EncoderAngleKalman::getVelocity() const { return velocity_estimate; }
 
 /*!
-	\brief      设置测量噪声协方差
-	\param[in]  r 测量噪声协方差值
+	\brief      设置 α 增益（借用原接口名）
+	\param[in]  q α 增益
+	\retval     none
+*/
+void EncoderAngleKalman::setProcessNoise(float q) {
+	alpha_gain = q;
+}
+
+/*!
+	\brief      设置 β 增益（借用原接口名）
+	\param[in]  r β 增益
 	\retval     none
 */
 void EncoderAngleKalman::setMeasurementNoise(float r) {
-	kalman.setMeasurementNoise(r);
+	beta_gain = r;
 }
 
 // CurrentKalman类的实现

Robot_V2/Algorithm/Filter.h

@@ -166,12 +166,12 @@ class EncoderVelocityKalman {
 */
 class EncoderAngleKalman {
   private:
-	KalmanFilter<float> kalman;
+	float angle_estimate;
+	float velocity_estimate;
+	float alpha_gain;
+	float beta_gain;
+	u32 last_update_count;
 	bool initialized;
-	float last_angle;
-	// 添加成员变量来替代静态变量
-	mutable u32 velocity_last_us50_counter;
-	mutable float velocity_last_angle;
 
   public:
 	EncoderAngleKalman(float initial_angle, float initial_error_covariance,
@@ -183,17 +183,15 @@ class EncoderAngleKalman {
 	float getVelocity() const;
 
 	void setProcessNoise(float q);
-	float getstate() const { return kalman.getState(); };
+	float getstate() const { return angle_estimate; };
 
 	void setMeasurementNoise(float r);
 	u8 init(float init_angle, float initial_error_covariance = 1.0f) {
-		kalman.x = init_angle;
-		kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
-		last_angle = init_angle;						  // 同时重置last_angle
-		// 重置速度计算相关的变量
-		velocity_last_angle = init_angle;
-		extern u32 motor_runtime_50us;
-		velocity_last_us50_counter = motor_runtime_50us;
+		(void)initial_error_covariance;
+		angle_estimate = init_angle;
+		velocity_estimate = 0.0f;
+		last_update_count = 0U;
+		initialized = false;
 		return 0;
 	}
 };

Robot_V2/Algorithm/pid.cpp

@@ -87,4 +87,11 @@ void PIDController::reset() {
 	error_prev = 0.0f;
 	output_prev = 0.0f;
 	integral_prev = 0.0f;
-}
+}
+
+/**
+ * @brief 仅清除PID积分项，保留输出斜坡和误差历史
+ *
+ * @return void
+ */
+void PIDController::reset_integral() { integral_prev = 0.0f; }

Robot_V2/Algorithm/pid.h

@@ -11,6 +11,7 @@ class PIDController {
 	void set_P(float pp);
 	void set_limit(float limit);
 	void reset();  // 重置PID控制器状态
+	void reset_integral(); // 仅清除积分项，保留输出斜坡状态
 
 	float P; //!< 比例增益(P环增益)
 	float I; //!< 积分增益（I环增益）