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环增益）

Robot_V2/device/Can.cpp

@@ -901,6 +901,7 @@ void CAN_transmiter::update(void) {
 	EEPROMStatus = flashEEP.EEPROMStatus;
 	Target_Control_Mode = FOC.control_mode;
 	controller_Mode = FOC.control_mode;
+	cpu_usage = get_cpu_usage();
 
 	// 使用简单平均滤波的电流值
 	u_current_union.f = sampler.get_current('U') * 1000;
@@ -938,7 +939,9 @@ u8 CAN_transmiter::CANFD_all_Send() {
 	pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
 	pack_float_to_bytes(transmit_data, 21, rt_current_union);
 	pack_float_to_bytes(transmit_data, 25, testfloat_union);
-	pack_u16_to_bytes(transmit_data, 29, test_u16);
+	transmit_data[29] = test_u16 & 0xFF;
+	transmit_data[30] = (test_u16 >> 8) & 0xFF;
+	transmit_data[31] = cpu_usage;
 	// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
 	success_flag +=
 		CANFD_AsyncSend(build_can_id(CANFD_STATUS_ID), 32, transmit_data);

Robot_V2/device/Can.h

@@ -88,6 +88,7 @@ class CAN_transmiter {
 	u16 Power_Voltage = 0;		// 电源电压
 
 	u16 limit_voltage = 0;					  // 限制电压
+	u8 cpu_usage = 0;						  // CPU 使用率（%）
 
 	float_union_t rt_angle_union = {0};	   // 实时角度
 	float_union_t rt_velocity_union = {0}; // 实时速度

Robot_V2/main.cpp

@@ -63,6 +63,11 @@ int main(void) {
 		scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志
 		__enable_irq();
 
+		if (currentTasks == 0U) {
+			idle_task_count();
+			continue;
+		}
+
 		do_task(currentTasks);
 	}
 	return 0;
@@ -92,6 +97,7 @@ u8 hardware_init(void) {
 }
 
 void do_task(u32 currentTasks) {
+	busy_task_enter(); // DWT: 记录忙碌起始周期
 	cnt_test++;
 	// 按照优先级和重要性处理任务
 	if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
@@ -101,8 +107,8 @@ void do_task(u32 currentTasks) {
 		KTH7823.update();
 		FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
 	}
-	if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
-		FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
+	if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度观测值
+		FOC.rt_velocity = Angle_kalman.getVelocity();
 	}
 	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务 (最高优先级)
 		motor_runtime_50us++;
@@ -163,4 +169,5 @@ void do_task(u32 currentTasks) {
 		CAN0_transmiter.communication_check();
 		fwdgt_counter_reload(); // 看门狗复位
 	}
+	busy_task_exit(); // DWT: 累加忙碌周期
 }

Robot_V2/sys/config.h

@@ -16,8 +16,8 @@
 
 // 版本号定义 - 可根据实际项目进行调整
 constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
-constexpr u16 SOFTWARE_VERSION_MINOR = 4;
-constexpr u16 SOFTWARE_VERSION_PATCH = 26;
+constexpr u16 SOFTWARE_VERSION_MINOR = 5;
+constexpr u16 SOFTWARE_VERSION_PATCH = 17;
 constexpr u16 SOFTWARE_VERSION_BUILD = 2;
 
 constexpr u16 HARDWARE_VERSION_MAJOR = 25;
@@ -93,26 +93,21 @@ constexpr u16 ANGLESENS_OFFSET = 0;
 constexpr float ANGLESENS_CALIBRATION_ANGLE = 83.0f;
 constexpr float KE_FACTOR = 0.0f;
 
-// 角度卡尔曼滤波器参数
-// 参数调节建议：
-// - 增大 Q 值：增加对测量的信任，提高响应速度
-// - 减小 Q 值：增加对预测的信任，提高平滑度
-// - 增大 R 值：增加平滑度，减少噪声，但会降低响应速度
-// - 减小 R 值：提高响应速度，但会增加噪声
+// 角度/速度 α-β 观测器参数
 constexpr float ANGLE_KALMAN_INITIAL_STATE = 0.0f;
 constexpr float ANGLE_KALMAN_INITIAL_COVARIANCE = 0.0f;
-constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q =
-	1e-3f; // 过程噪声协方差 - 调节此参数控制平滑度
-constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R =
-	0.0062f; // 测量噪声协方差 - 调节此参数控制响应速度
+constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q = 0.0f;
+constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R = 0.0f;
+constexpr float ANGLE_OBSERVER_ALPHA = 0.75f; // 角度校正增益，越大越跟测量
+constexpr float ANGLE_OBSERVER_BETA = 0.002f; // 速度校正增益，原0.001微调至0.002
 
-// 速度卡尔曼滤波器参数
+// 旧速度 Kalman 参数保留用于接口兼容；主速度输出现在直接来自 α-β 观测器
 constexpr float VELOCITY_KALMAN_INITIAL_STATE = 0.0f;
-constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
+constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-3f;
 constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
-	1e-6f; // 过程噪声协方差 - 通常保持较小
+	5e-4f; // 略增大，减少拖尾
 constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
-	1.0f; // 测量噪声协方差 - 主要调节参数 1.0
+	0.05f; // 明显小于旧值，避免压得太死
 
 // 电流卡尔曼滤波器参数
 constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;

Robot_V2/sys/systick.cpp

@@ -50,9 +50,43 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 volatile static uint32_t delay;
 volatile static uint32_t systick_ms = 0; // 系统运行时间（毫秒）
+
+// ============================================================================
+// DWT-based CPU usage measurement
+// ============================================================================
+// GD32C103 = Cortex-M4 @ 120MHz, DWT_CYCCNT 可用
+//
+// 原理：
+//   do_task() 入口记录 CYCCNT → task_enter_cyccnt
+//   do_task() 出口记录 CYCCNT → 差值 = 本次忙碌周期
+//   累加到 busy_cycles_accum
+//   SysTick 每 1s 统计：cpu% = busy_cycles_accum * 100 / SystemCoreClock
+//
+//   不需要 idle_task_count()，DWT 直接数周期
+// ============================================================================
+
+volatile static uint32_t task_enter_cyccnt = 0;    // do_task 入口 CYCCNT
+volatile static uint32_t busy_cycles_accum = 0;    // 1s 内累计忙碌周期
+volatile static uint16_t sample_seconds = 0;        // 统计窗口（秒）
+volatile static uint8_t cpu_usage = 0;             // CPU 使用率（0~100%）
+volatile static uint8_t dwt_initialized = 0;       // DWT 初始化标志
 extern CAN_transmiter CAN0_transmiter;
 extern FlashEEPROM flashEEP;
 
+/*!
+	\brief      初始化 DWT 周期计数器
+	\note       使能 DWT 的 CYCCNT，用于精确测量 CPU 周期
+*/
+static void dwt_init(void) {
+	/* 使能 DWT 和 ITM（TRCENA 位在 DEMCR 中） */
+	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+	/* 复位 CYCCNT */
+	DWT->CYCCNT = 0;
+	/* 使能 CYCCNT */
+	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+	dwt_initialized = 1;
+}
+
 /*!
 	\brief      get system tick time in milliseconds
 	\param[in]  none
@@ -70,6 +104,9 @@ uint32_t get_systick_ms(void) {
 	\retval     none
 */
 void systick_config(void) {
+	/* 初始化 DWT 周期计数器 */
+	dwt_init();
+
 	/* setup systick timer for 1000Hz interrupts */
 	if (SysTick_Config(SystemCoreClock / 1000U)) {
 		/* capture error */
@@ -89,6 +126,24 @@ void systick_config(void) {
 extern "C" void SysTick_Handler(void) {
 	systick_ms++;      // 递增系统时间
 	delay_decrement(); // 递减延时计数器
+
+	/* 每 1 秒统计一次 CPU 占用率 */
+	sample_seconds++;
+	if (sample_seconds >= 1000U) {
+		if (dwt_initialized) {
+			/* 1 秒总周期 = SystemCoreClock (120,000,000) */
+			/* CPU% = busy_cycles_accum * 100 / SystemCoreClock */
+			uint64_t busy_64 = (uint64_t)busy_cycles_accum * 100U;
+			uint32_t busy_percent = (uint32_t)(busy_64 / SystemCoreClock);
+			if (busy_percent > 100U) {
+				busy_percent = 100U;
+			}
+			cpu_usage = (uint8_t)busy_percent;
+		}
+
+		busy_cycles_accum = 0;
+		sample_seconds = 0;
+	}
 }
 /*!
 	\brief      delay a time in seconds
@@ -128,6 +183,33 @@ void delay_decrement(void) {
 	}
 }
 
+/*!
+	\brief      空闲任务计数（兼容接口，DWT 版本无需操作）
+*/
+void idle_task_count(void) {
+	/* DWT 版本：空闲周期 = 总周期 - 忙碌周期，无需手动计数 */
+}
+
+/*!
+	\brief      do_task 入口调用，记录忙碌起始周期
+*/
+void busy_task_enter(void) {
+	if (dwt_initialized) {
+		task_enter_cyccnt = DWT->CYCCNT;
+	}
+}
+
+/*!
+	\brief      do_task 出口调用，累加忙碌周期
+*/
+void busy_task_exit(void) {
+	if (dwt_initialized) {
+		busy_cycles_accum += (DWT->CYCCNT - task_enter_cyccnt);
+	}
+}
+
+uint8_t get_cpu_usage(void) { return cpu_usage; }
+
 /*!
 	\brief      system soft reset
 	\param[in]  none
@@ -207,4 +289,4 @@ void app_prepare_deep_sleep(bool save_data) {
 		}
 	}
 	jumpToBOOT(); // 跳转到 bootloader 正式休眠，进入低功耗待机模式
-}
+}

Robot_V2/sys/systick.h

@@ -18,7 +18,7 @@ this list of conditions and the following disclaimer.
 	   and/or other materials provided with the distribution.
 	3. Neither the name of the copyright holder nor the names of its
 contributors may be used to endorse or promote products derived from this
-software without specific prior written permission.
+software without without specific prior written permission.
 
 	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
@@ -50,8 +50,12 @@ void delay_1ms(uint32_t count);
 void delay_1s(uint32_t count);
 /* delay decrement */
 void delay_decrement(void);
-/* 空闲任务计数 */
+/* 空闲任务计数（兼容接口，DWT 版本无需操作） */
 void idle_task_count(void);
+/* do_task 入口调用，记录忙碌起始周期 */
+void busy_task_enter(void);
+/* do_task 出口调用，累加忙碌周期 */
+void busy_task_exit(void);
 /* 获取CPU使用率 */
 uint8_t get_cpu_usage(void);
 /* 系统软复位 */
@@ -65,4 +69,4 @@ void EXTI5_9_IRQHandler(void);
 }
 #endif
 
-#endif /* SYSTICK_H */
+#endif /* SYSTICK_H */