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/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 */