feat: 更新CMSIS和GD32C10x_DFP包版本并优化代码

- 升级ARM::CMSIS从4.5.0到6.1.0版本
- 添加ARM::CMSIS-DSP@1.16.2依赖包
- 升级GigaDevice::GD32C10x_DFP从1.0.1到1.5.0版本
- 移除FOC.cpp中未使用的systick.h头文件
- 移除pid.cpp中未使用的<algorithm>头文件
- 更新GD32C10x固件库至V1.7.0版本(2026-01-30)
- 修复ADC驱动中的无符号整数运算问题
- 优化BKP、CAN、CTC等外设驱动中的FlagStatus返回逻辑
- 改进CAN驱动的消息发送和传输停止函数实现
- 修复CRC、DMA、FMC等驱动中的数据类型和变量声明问题
- 清理和修正各驱动文件中的文档注释

Robot_V2.cbuild-pack.yml

@@ -1,8 +1,11 @@
 cbuild-pack:
   resolved-packs:
-    - resolved-pack: ARM::CMSIS@4.5.0
+    - resolved-pack: ARM::CMSIS@6.1.0
       selected-by-pack:
         - ARM::CMSIS
-    - resolved-pack: GigaDevice::GD32C10x_DFP@1.0.1
+    - resolved-pack: ARM::CMSIS-DSP@1.16.2
+      selected-by-pack:
+        - ARM::CMSIS-DSP
+    - resolved-pack: GigaDevice::GD32C10x_DFP@1.5.0
       selected-by-pack:
         - GigaDevice::GD32C10x_DFP

Robot_V2/Algorithm/FOC.cpp

@@ -11,12 +11,13 @@
 #include <cmath>
 #include <gd32c10x.h>
 
-
-FOC_Controlroller FOC; // FOC控制初始化
+FOC_Controller FOC; // FOC控制初始化
 extern sampler sampler;
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
 extern EncoderVelocityKalman Vel_kalman; // 速度滤波
+extern CurrentKalman Current_kalman;
+extern u16 adc_value[6]; // ADC DMA缓冲区，用于快速电压检测
 
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
@@ -24,27 +25,38 @@ extern EncoderVelocityKalman Vel_kalman; // 速度滤波
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
-/**@
- * @brief 归一化角度
- *
- * @param angle
- * @return float
- */
-inline float _normalizeAngle(float angle) {
-	float a = fmod(angle, 360.0f);
-	// 取余运算可以用于归一化，列出特殊值例子算便知
-	return a >= 0 ? a : (a + 360.0f);
+// 校准步骤（代码模板）
+void calibrate_zero_position(void) {
+	// Set_3Pwm(.25, 0, 0); // 起劲
+	// delay_1ms(1000);
+	// KTH7823.update();
+
+	// // 改进校准算法：获取当前传感器角度并计算补偿值
+	// float current_sensor_angle = KTH7823.getcurrent_sensorangle();
+	// float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f); //
+	// 计算当前电角度
+
+	// // 计算需要的补偿值，使得当前位置对应0°电角度
+	// KTH7823.calibration_angle = -electrical_angle;
+
+	// // 对补偿角进行归一化处理，确保在[-180, 180]范围内
+	// KTH7823.calibration_angle = fmodf(KTH7823.calibration_angle + 180.0f,
+	// 360.0f); if (KTH7823.calibration_angle < 0) { KTH7823.calibration_angle
+	// += 360.0f;
+	// }
+	// KTH7823.calibration_angle -= 180.0f;
+
+	// Set_3Pwm(0, 0, 0); // 停止
 }
 
 // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
-inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
-										  float current_c) {
+inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
+									   float current_c) {
 	// 计算电气角度
-	float angle_el = _normalizeAngle(shaft_angle * PP);
+	float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
 
-	float I_alpha, I_beta;
 	// Clarke变换 (使用三相电流)
-	// arm_clarke_f32(current_a, current_b, &I_alpha, &I_beta);
+	float I_alpha, I_beta;
 	I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
 			  (1.0f / 3.0f) * current_c;
 	I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
@@ -54,10 +66,11 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
 	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
 	float I_q, I_d;
 	I_q = I_beta * cos_el - I_alpha * sin_el;
-	// arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el);
+	I_d = I_beta * sin_el + I_alpha * cos_el;
 
-	return I_q * 1000.0f -
-		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以100以转换为mA
+	// 返回Iq值，乘以1000以转换为mA
+	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
+	// return I_q * 1000 - KE_FACTOR * rt_velocity;
 }
 
 /**
@@ -65,20 +78,17 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
  *
  * @return u8 0表示初始化成功，1表示初始化失败
  */
-u8 FOC_Controlroller::init() {
-	motor_init();									 // 电机驱动初始化
-	vel_loop.set_PID(vel_loop_p * 100 / limit_voltage, 0.20f, 5e-5); // 设置电机速度控制参数
-	angle_loop.set_PID(angle_loop_p * 100 / limit_voltage, 100.0f,
-					   0.2f); // 设置电机角度控制参数
-	current_loop.set_PID(current_loop_p * 100 / limit_voltage, 5e-7f,
-						 7e-8); // 设置电机电流控制参数
-
-	vel_inVelAngloop.set_PID(vel_angloop_p * 100 / limit_voltage, 3.0f,
-							 5e-4); // 设置电机速度控制参数
-	ang_inVelAngloop.set_PID(ang_angloop_p * 100 / limit_voltage, 0.0f,
-							 5e-4); // 设置电机角度控制参数
-
+u8 FOC_Controller::init() {
+	motor_init(); // 电机驱动初始化
 
+	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
+	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
+	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
+	FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
+	FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
+	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
+	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
+	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
 	return 0;
 }
 
@@ -86,73 +96,220 @@ u8 FOC_Controlroller::init() {
  * @brief 电机控制类构造函数
  *
  */
-FOC_Controlroller::FOC_Controlroller() { ; }
+FOC_Controller::FOC_Controller() { ; }
+
+// /**
+//  * @brief 设置电机输出
+//  *
+//  * @param Uq: q轴电压
+//  * @param Ud: d轴电压
+//  * @param angle_el: 电机轴角度
+//  */
+// inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
+// 	// 计算电气角度
+// 	float angle_el = fmod(shaft_angle * PP, 360.0f);
+
+// 	// 帕克逆变换 (直接计算，避免函数调用开销)
+// 	float sin_el, cos_el;
+// 	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
+// 	float Ualpha = Ud * cos_el - Uq * sin_el;
+// 	float Ubeta = Ud * sin_el + Uq * cos_el;
+
+// 	// 克拉克逆变换 (使用预计算常量优化)
+// 	Ua = Ualpha + v_half;
+// 	Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half;
+// 	Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half;
+
+// 	if (Ua > v_safe)
+// 		Ua = v_safe;
+// 	else if (Ua < -v_safe)
+// 		Ua = -v_safe;
+// 	if (Ub > v_safe)
+// 		Ub = v_safe;
+// 	else if (Ub < -v_safe)
+// 		Ub = -v_safe;
+// 	if (Uc > v_safe)
+// 		Uc = v_safe;
+// 	else if (Uc < -v_safe)
+// 		Uc = -v_safe;
+
+// 	// 设置PWM输出
+// 	float voltage_exchange = limit_voltage / v_safe;
+// 	duty_a = Ua / voltage_power_supply * voltage_exchange;
+// 	duty_b = Ub / voltage_power_supply * voltage_exchange;
+// 	duty_c = Uc / voltage_power_supply * voltage_exchange;
+
+// 	Set_3Pwm(duty_a, duty_b, duty_c);
+// }
 
 /**
- * @brief 设置电机输出，使用优化的SVPWM算法
+ * @brief 使用SVPWM算法设置电机输出
  *
  * @param Uq: q轴电压
  * @param Ud: d轴电压
- * @param angle_el: 电机轴角度
  */
-inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
+inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 	// 计算电气角度
-	float angle_el = _normalizeAngle(shaft_angle * PP);
+	float angle_el = shaft_angle * PP + KTH7823.calibration_angle;
 
-	// 帕克逆变换 (直接计算，避免函数调用开销)
+	// 对电角度进行归一化，确保在[0, 360)范围内
+	angle_el = fmodf(angle_el, 360.0f);
+
+	// 帕克逆变换
 	float sin_el, cos_el;
 	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
 	float Ualpha = Ud * cos_el - Uq * sin_el;
 	float Ubeta = Ud * sin_el + Uq * cos_el;
 
-	// 克拉克逆变换 (使用预计算常量优化)
-	Ua = Ualpha + v_half;
-	Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half;
-	Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
+	// 计算角度并归一化到 [0, 2π)
+	float angle = atan2f(Ubeta, Ualpha);
+	if (angle < 0.0f)
+		angle += 2.0f * PI;
+
+	// 扇区与局部角度
+	int sector = (int)(angle * INV_PI_3) + 1;
+	if (sector < 1)
+		sector = 1;
+	else if (sector > 6)
+		sector = 6;
+	float local_angle = angle - (sector - 1) * PI_3;
+
+	// 模长计算
+	float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta);
+
+	// 计算电压调制比，同时限制最大输出电压
+	float modulation_index =
+		(magnitude / voltage_power_supply); // 90%作为安全裕量
+
+	// 电压限制
+	if (limit_voltage > voltage_power_supply) {
+		limit_voltage = voltage_power_supply;
+	}
+	float limit_percent = limit_voltage / v_safe;
+	if (modulation_index > limit_percent) {
+		// 限制电压
+		modulation_index = limit_percent;
+		// 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude
+		magnitude = voltage_power_supply * limit_percent;
+	}
+
+	// 使用arm_sin_cos_f32计算local_angle的sin和cos
+	float sin_l, cos_l;
+	arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
+	float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
+	float T2 = modulation_index * sin_l;
+	float T0 = 1.0f - T1 - T2;
+
+	// 计算占空比（按规范化时间映射）
+	switch (sector) {
+	case 1:
+		duty_a = 0.5f * T0 + T1 + T2;
+		duty_b = 0.5f * T0 + T2;
+		duty_c = 0.5f * T0;
+		break;
+	case 2:
+		duty_a = 0.5f * T0 + T1;
+		duty_b = 0.5f * T0 + T1 + T2;
+		duty_c = 0.5f * T0;
+		break;
+	case 3:
+		duty_a = 0.5f * T0;
+		duty_b = 0.5f * T0 + T1 + T2;
+		duty_c = 0.5f * T0 + T2;
+		break;
+	case 4:
+		duty_a = 0.5f * T0;
+		duty_b = 0.5f * T0 + T1;
+		duty_c = 0.5f * T0 + T1 + T2;
+		break;
+	case 5:
+		duty_a = 0.5f * T0 + T2;
+		duty_b = 0.5f * T0;
+		duty_c = 0.5f * T0 + T1 + T2;
+		break;
+	case 6:
+		duty_a = 0.5f * T0 + T1 + T2;
+		duty_b = 0.5f * T0;
+		duty_c = 0.5f * T0 + T1;
+		break;
+	default:
+		duty_a = 0.5f;
+		duty_b = 0.5f;
+		duty_c = 0.5f;
+		break;
+	}
 
-	// 计算占空比并限制范围
-	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;
-	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);
 }
 
 /**
- * @brief 电机关机
+ * @brief 电机关机（通用关机，不带原因标记）
  *
  */
-void FOC_Controlroller::shut_down() {
-	is_shut_down = 1;
+void FOC_Controller::shut_down() {
+	drv_stop();
 	motor_sleep();
+	is_shut_down = true;
+}
+
+/**
+ * @brief 过热关机
+ *
+ */
+void FOC_Controller::shut_down_thermal() {
+	drv_stop();
+	motor_sleep();
+	is_shut_down = true;
+	thermal_shutdown = true;
+}
+
+/**
+ * @brief 过热恢复
+ *
+ */
+void FOC_Controller::resume_from_thermal() {
+	thermal_shutdown = false;
+	if (!voltage_error && !over_current) {
+		is_shut_down = false;
+		motor_resume();
+	}
 }
 /**
  * @brief 占空比置0
  *
  */
-void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
+void FOC_Controller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
 /**
  * @brief 电机锁止
  */
-float FOC_Controlroller::drv_hold() {
+float FOC_Controller::drv_hold() {
 	float hold_pwm_duty = HOLD_PWM_DUTY;
 	if (HOLD_PWM_DUTY > 0.95f) {
 		hold_pwm_duty = 0.95f;
 	} else if (HOLD_PWM_DUTY < 0.0f) {
 		hold_pwm_duty = 0.0f;
 	}
+	Set_3Pwm(hold_pwm_duty, 0, 0);
+	return hold_pwm_duty;
+}
+
+
+/**
+ * @brief PWM模式（调试）
+ */
+float FOC_Controller::PWM_out() {
+	float hold_pwm_duty = target_velocity/100;
 	Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
 	return hold_pwm_duty;
 }
+
+
 /**
  * @brief 电机恢复
  *
  */
-void FOC_Controlroller::resume() {
+void FOC_Controller::resume() {
 	over_current = false;
 	voltage_error = false;
 	is_shut_down = false;
@@ -162,99 +319,117 @@ void FOC_Controlroller::resume() {
  * @brief 更新电源电压
  *
  */
-void FOC_Controlroller::update_power(void) {
-	voltage_power_supply = sampler.get_vbus_voltage();
-	if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) {
+void FOC_Controller::update_power(float voltage) {
+	if (voltage < 36.0f and voltage > 8.0f) {
 		voltage_error = false;
-		v_half = voltage_power_supply * 0.5f;
-		v_safe = voltage_power_supply * 0.95f;
+		voltage_power_supply = voltage;
+		v_half = voltage * 0.5f;
+		v_safe = voltage * 1.1f;
 	} else {
 		voltage_error = true;
 	}
 }
 
+/**
+ * @brief 快速电压检测（在电机控制循环中每100us调用一次）
+ *
+ * 直接从DMA缓冲区读取最新ADC值，检测电压跌幅
+ * 响应时间 < 200us
+ *
+ * 算法：
+ * 1. 使用峰值检测算法跟踪当前额定电压（vbus_reference）
+ *    - 电压上升时，基准电压跟随
+ *    - 电压下跌时，基准电压保持（不跟随下跌）
+ * 2. 计算电压跌幅 = 基准电压 - 当前电压
+ * 3. 关机条件（满足任一即可）：
+ *    - 电压跌幅超过 VBUS_DROP_THRESHOLD
+ *    - 电压低于绝对下限 VBUS_ABSOLUTE_MIN
+ *
+ * 参数定义在 config.h 中:
+ * - VBUS_DROP_THRESHOLD: 电压跌幅阈值 (默认5V)
+ * - VBUS_ABSOLUTE_MIN: 绝对最低电压 (默认8V)
+ * - VBUS_DEBOUNCE_COUNT: 防抖计数 (默认5次)
+ */
+void FOC_Controller::fast_voltage_check(void) {
+	// 直接从DMA缓冲区读取最新ADC值（通道5是Vbus）
+	float current_vbus = adc_value[5] * (3.3f / 4096.0f) * 11.0f;
+
+	// 更新基准电压（峰值检测）
+	// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
+	if (current_vbus > vbus_reference) {
+		vbus_reference = current_vbus;
+	}
+	// 电压下跌时不更新基准电压，保持之前的峰值
+
+	// 更新上次电压值
+	last_vbus_fast = current_vbus;
+
+	// 计算电压跌幅（相对于基准电压）
+	float voltage_drop = vbus_reference - current_vbus;
+
+	// 关机条件（满足任一即可）：
+	// 1. 电压跌幅超过阈值
+	// 2. 电压低于绝对下限
+	bool drop_exceeded = (voltage_drop > VBUS_DROP_THRESHOLD);
+	bool below_absolute = (current_vbus < VBUS_ABSOLUTE_MIN);
+
+	if ((drop_exceeded || below_absolute) && (!is_shut_down)) {
+		// 电压跌落计数（防抖）
+		vbus_drop_count++;
+
+		if (vbus_drop_count >= VBUS_DEBOUNCE_COUNT) {
+			// 确认需要关机，立即停机
+			drv_stop();
+			motor_sleep();
+			is_shut_down = true;
+			emergency_shutdown = true; // 标记紧急关机，低频任务会执行保存
+			vbus_drop_count = 0;	 // 重置计数器
+		}
+	} else {
+		// 电压正常，重置计数器
+		vbus_drop_count = 0;
+	}
+}
+
 /**
  * @brief 速度开环控制
  *
  * @param target_velocity: 目标速度
  * @return float: q轴电压
  */
-float FOC_Controlroller::velocity_open_loop() {
+float FOC_Controller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
-	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR *
-													REDUCTION_RATIO * 1.0 /
-													TIMER2_FREQUENCY);
+	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
+										 MOTOR_FREQUENCY / 2,
+					   360.0f);
+	// 获取电机轴角度
+	float shaft_angle = KTH7823.getcurrent_sensorangle();
+	CAN0_transmiter.testfloat_union.f = shaft_angle;
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
-
-	setPhaseVoltage(Uq, 0.0f);
-	CAN0_transmiter.testfloat_union.f = shaft_angle;
-	// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-	// 		  sampler.get_current('W'));
-
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return limit_voltage;
 }
 
-/**
- * @brief 电机初始化
- *
- * @param _PP: 电机极队数
- */
-// void FOC_Controlroller::DFOC_alignSensor(int _PP) {
-// 	u8 PP = _PP;
-// 	setPhaseVoltage(3, 0, _3PI_2);
-// 	delay_1ms(3000);
-// 	zero_electric_angle = _electricalAngle();
-// 	setPhaseVoltage(0, 0, _3PI_2);
-// 	std::printf("0电角度:%f", zero_electric_angle);
-// }
-
 /**
  * @brief 角度闭环控制
  *
  * @param target_angle: 目标角度
  * @return float: 当前角度
  */
-float FOC_Controlroller::angle_control() {
+float FOC_Controller::angle_control() {
 	// PID 控制计算
-	float Uq = angle_loop(target_angle - rt_angle);
+	float Uq = angle_loop(rt_angle - target_angle);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
-	return shaft_angle;
-}
-
-/**
- * @brief 带速度限制的角度闭环控制
- *
- * @param target_angle: 目标角度
- * @param max_velocity: 最大速度限制 (单位: rad/s)
- * @return float: 当前角度
- */
-float FOC_Controlroller::angle_control_with_velocity_limit() {
-	// 计算角度误差
-	float angle_error = target_angle - rt_angle;
-
-	// 使用角度PID控制器计算目标速度
-	float target_velocity = ang_inVelAngloop(angle_error);
-
-	// 对目标速度进行限幅
-	target_velocity =
-		_constrain(target_velocity, -limit_velocity, limit_velocity);
-
-	// 使用速度PID控制器计算最终输出电压
-	float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
-	CAN0_transmiter.testfloat_union.f = shaft_angle;
-
-	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle();
-
-	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
-
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
 }
 
@@ -264,30 +439,126 @@ float FOC_Controlroller::angle_control_with_velocity_limit() {
  * @param target_velocity: 目标速度
  * @return float: 当前速度
  */
-float FOC_Controlroller::velocity_control() {
-	// PID 控制计算
-	float Uq = vel_loop((rt_velocity - target_velocity) * 25);
+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);
+	CAN0_transmiter.testfloat_union.f = Uq;
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
-	CAN0_transmiter.testfloat_union.f =
-		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-				  sampler.get_current('W'));
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
 
 	return rt_velocity;
 }
 
-float FOC_Controlroller::current_control() {
+float FOC_Controller::current_control() {
 	// PID 控制计算
-	float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						 sampler.get_current('W'));
+	float Iq = rt_current =
+		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;
+	if (Uq > limit_voltage) {
+		Uq = limit_voltage;
+	} else if (Uq < -limit_voltage) {
+		Uq = -limit_voltage;
+	}
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
+	CAN0_transmiter.testfloat_union.f = shaft_angle;
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return 1;
 }
+
+/**
+ * @brief 带速度限制的角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @param max_velocity: 最大速度限制 (单位: rad/s)
+ * @return float: 当前角度
+ */
+float FOC_Controller::angle_control_with_velocity_limit() {
+	// 计算角度误差
+	float angle_error = target_angle - rt_angle;
+
+	ang_inVelAngloop.set_P(ang_angloop_p);
+	// 使用角度PID控制器计算目标速度
+	float target_velocity = ang_inVelAngloop(angle_error);
+
+	// 使用速度PID控制器计算最终输出电压
+	// 根据目标速度大小，选择不同的PID参数
+	if (target_velocity < -120.0f) {
+		vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
+	} else if (target_velocity < -30.0f) {
+		vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
+	} else {
+		vel_inVelAngloop.set_P(vel_angloop_p);
+	}
+	float Uq = vel_inVelAngloop(target_velocity - rt_velocity)* DIR;
+	CAN0_transmiter.testfloat_union.f = Uq;
+
+	// // 实现力度控制：如果电流超过目标电流，则限制输出
+	// if (abs(rt_current) > abs(target_current)) {
+	// 	// 当实际电流超过目标电流时，减小输出以限制电流
+	// 	// 可以采用比例限制或直接限制输出电压的方式
+	// 	float current_ratio = abs(target_current) / abs(rt_current);
+	// 	if (current_ratio < 1.0f) {
+	// 		Uq *= current_ratio;  // 按比例缩小输出电压
+	// 	}
+	// }
+
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+	// 获取当前实际电流值
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+
+	return shaft_angle;
+}
+
+/**
+ * @brief MIT控制
+ *
+ * @param target_angle: 目标角度
+ * @param max_velocity: 最大速度限制 (单位: rad/s)
+ * @return float: 当前角度
+ */
+float FOC_Controller::MIT_Mode() {
+	// 第一层：角度环（外环）
+	// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
+	float angle_error = target_angle - rt_angle;
+	ang_inMITloop.set_P(ang_MIT_p);
+	float target_velocity =
+		ang_inMITloop(angle_error); // 使用angle_loop作为角度到速度的映射
+
+	// 第二层：速度环（中环）
+	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
+	float velocity_error = (target_velocity - rt_velocity)* DIR;
+	vel_inMITloop.set_P(vel_MIT_p);
+	float target_current = vel_inMITloop(velocity_error);
+
+	// 第三层：电流环（内环）
+	// 计算当前实际电流
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+
+	// 通过电流环PID控制器计算输出电压
+	float current_error = target_current - rt_current;
+	current_inMITloop.set_P(cur_MIT_p);
+	float Uq = current_inMITloop(current_error);
+
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+
+	return shaft_angle;
+}

Robot_V2/Algorithm/FOC.h

@@ -5,52 +5,97 @@
 #include "pid.h"
 #include <cstdint>
 
-class FOC_Controlroller {
+class FOC_Controller {
 
   public:
-	FOC_Controlroller();
+	FOC_Controller();
 
 	// 初始化函数
 	u8 init();
 
 	u8 control_mode = 0;
 	float limit_voltage = 0.0f;
-	float limit_velocity = 0.0f;
+
+
 	float target_angle = 0.0f;
 	float target_velocity = 0.0f;
 	float target_current = 0.0f;
+
+
 	float rt_angle = 0.0f;
 	float rt_velocity = 0.0f;
+	float rt_current = 0.0f;
+
 	bool over_current = false;
 	bool voltage_error = false;
 	bool is_shut_down = false;
+	bool thermal_shutdown = false; // 过热关机标志
+	bool emergency_shutdown = false; // 紧急电压跌落关机标志
+
 	float voltage_power_supply = 24.0f; // 电源电压
-	float v_safe = voltage_power_supply * 0.95f;
+	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 vbus_reference = 0.0f;	// 基准电压（峰值检测，跟踪当前额定电压）
+	float last_vbus_fast = 0.0f;	// 上次电压值（快速检测用）
+	u32 vbus_drop_count = 0;		 // 电压跌落计数器（防抖）
+
+	float cur_loop_p = DEFAULT_current_loop_p;
+	float vel_loop_p = DEFAULT_vel_loop_p;
+	float ang_loop_p = DEFAULT_angle_loop_p;
+
+	float vel_angloop_p = DEFAULT_vel_angloop_p;
+	float ang_angloop_p = DEFAULT_angle_angloop_p;
+
+	float ang_MIT_p = DEFAULT_angle_MIT_p;
+	float vel_MIT_p = DEFAULT_vel_MIT_p;
+	float cur_MIT_p = DEFAULT_current_MIT_p;
+
 	float velocity_open_loop(void);
 	float angle_control(void);
 	float angle_control_with_velocity_limit(void);
+	float MIT_Mode(void);
 	float velocity_control(void);
 	float current_control(void);
 
-	void update_power(void);
+	void update_power(float voltage);
 	void DFOC_alignSensor(int _PP);
 	void shut_down(void);
 	void resume(void);
 	void drv_stop(void);
 	float drv_hold(void);
-	PIDController vel_loop = PIDController{
-		vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
+	float PWM_out(void);
+	void shut_down_thermal(void);	// 过热关机
+	void resume_from_thermal(void); // 过热恢复
+	void fast_voltage_check(void);	// 快速电压检测（100us周期调用）
+	
+	// 此处默认值不作数
+	PIDController vel_loop =
+		PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
+					  vel_loop_Kd, 10000, voltage_power_supply}; // 12V
 	PIDController angle_loop =
-		PIDController{angle_loop_p * 100, 100.0f, 0.2f, 10000000,
-					  voltage_power_supply}; // 12V
+		PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
+					  10000000, voltage_power_supply}; // 12V
 	PIDController current_loop = PIDController{
-		current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V
-	PIDController vel_inVelAngloop = PIDController{
-		vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply};
+		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply}; // 12V
+	PIDController vel_inVelAngloop =
+		PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
+					  vel_angloop_Kd, 1e6, v_safe};
 	PIDController ang_inVelAngloop =
-		PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999};
+		PIDController{DEFAULT_angle_angloop_p * 100 / voltage_power_supply,
+					  ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
+
+	PIDController ang_inMITloop =
+		PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
+					  angle_MIT_Kd, 1e5, 100};
+	PIDController vel_inMITloop = PIDController{
+		DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
+	PIDController current_inMITloop = PIDController{
+		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply}; // 12V
 
 	void update_pid_limits() {
 		vel_loop.limit = voltage_power_supply;
@@ -64,13 +109,16 @@ class FOC_Controlroller {
 	float Ualpha, Ubeta = 0;
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	void setPhaseVoltage(float Uq, float Ud);
+	float startup_ramp_val = 0.0f;      // 软启动系数
+	float startup_ramp_step = 0.01f;    // 启动斜坡步长，可调节以控制启动速度
+	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);
 };
 
 // 定义函数指针类型
-typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);
+typedef float (FOC_Controller::*FOCFunctionPtr)(void);
 
+void calibrate_zero_position(void);
 inline float _normalizeAngle(float angle);
 inline float _electricalAngle();
 

Robot_V2/Algorithm/Filter.cpp

@@ -8,8 +8,20 @@
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
 extern u32 motor_runtime_50us;
-EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
-EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
+
+// 使用配置文件中的参数
+EncoderAngleKalman Angle_kalman(ANGLE_KALMAN_INITIAL_STATE,
+								ANGLE_KALMAN_INITIAL_COVARIANCE,
+								ANGLE_KALMAN_PROCESS_NOISE_Q,
+								ANGLE_KALMAN_MEASUREMENT_NOISE_R);
+
+EncoderVelocityKalman Vel_kalman(VELOCITY_KALMAN_INITIAL_STATE,
+								 VELOCITY_KALMAN_INITIAL_COVARIANCE,
+								 VELOCITY_KALMAN_PROCESS_NOISE_Q,
+								 VELOCITY_KALMAN_MEASUREMENT_NOISE_R);
+CurrentKalman Current_kalman(	CURRENT_KALMAN_INITIAL_STATE, CURRENT_KALMAN_INITIAL_COVARIANCE,
+							CURRENT_KALMAN_PROCESS_NOISE_Q,
+							CURRENT_KALMAN_MEASUREMENT_NOISE_R);
 
 /*!
  \brief 	中值滤波器函数
@@ -96,7 +108,9 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
 									   float measurement_noise)
 	: kalman(initial_angle, initial_error_covariance, process_noise,
 			 measurement_noise),
-	  last_angle(initial_angle), initialized(true) {}
+	  last_angle(initial_angle), initialized(true),
+	  velocity_last_angle(initial_angle),
+	  velocity_last_us50_counter(motor_runtime_50us) {}
 
 /*!
 	\brief      更新卡尔曼滤波器状态
@@ -125,10 +139,18 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
 	\retval     当前角度估计值 (degree)
 */
 float EncoderAngleKalman::getVelocity() const {
-	static u32 last_us50_counter = motor_runtime_50us;
-	float vel = (kalman.getState() - last_angle) * TIMER2_FREQUENCY *
-				(motor_runtime_50us - last_us50_counter);
-	last_us50_counter = motor_runtime_50us;
+	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 ;
+	}
+
+	velocity_last_angle = angle_now;
+	velocity_last_us50_counter = motor_runtime_50us;
+
 	return vel;
 }
 
@@ -147,3 +169,49 @@ void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
 void EncoderAngleKalman::setMeasurementNoise(float r) {
 	kalman.setMeasurementNoise(r);
 }
+
+// CurrentKalman类的实现
+/*!
+	\brief      构造函数，初始化电流卡尔曼滤波器参数
+	\param[in]  initial_current 初始电流值 (A)
+	\param[in]  initial_error_covariance 初始误差协方差
+	\param[in]  process_noise 过程噪声协方差
+	\param[in]  measurement_noise 测量噪声协方差
+	\retval     none
+*/
+CurrentKalman::CurrentKalman(float initial_current,
+							 float initial_error_covariance,
+							 float process_noise, float measurement_noise)
+	: kalman(initial_current, initial_error_covariance, process_noise,
+			 measurement_noise) {}
+
+/*!
+	\brief      更新卡尔曼滤波器状态
+	\param[in]  measured_current 测量的电流值 (A)
+	\retval     估计的电流值 (A)
+*/
+float CurrentKalman::update(float measured_current) {
+	return kalman.update(measured_current);
+}
+
+/*!
+	\brief      获取当前电流估计值
+	\retval     当前电流估计值 (A)
+*/
+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);
+}

Robot_V2/Algorithm/Filter.h

@@ -5,16 +5,17 @@
 extern "C" {
 #endif
 
-// C风格函数声明可以放在这里
-// 例如: void some_c_function(float* data, int size);
-
 #ifdef __cplusplus
 } // extern "C"
 #endif
 
 #ifdef __cplusplus
-#include <cstring>
+#include "Can.h"
 #include <cstdint>
+#include <cstring>
+
+
+extern CAN_transmiter CAN0_transmiter;
 
 float medium_filter(float values[], u8 size);
 
@@ -75,10 +76,19 @@ template <typename T> class KalmanFilter {
 		\retval 估计的状态值
 	*/
 	T update(T measurement) {
+
 		T K = P / (P + R); // 卡尔曼增益
 		
 		x += K * (measurement - x); // 更新状态估计
-		P = (1 - K) * P;			// 更新误差协方差
+
+		P = (1 - K) * P; // 更新误差协方差
+		
+		// 添加协方差下限保护，防止数值不稳定
+		const T MIN_COVARIANCE = static_cast<T>(1e-6);
+		if (P < MIN_COVARIANCE) {
+			P = MIN_COVARIANCE;
+		}
+		
 		return x;
 	}
 
@@ -101,6 +111,25 @@ template <typename T> class KalmanFilter {
 		\retval none
 	*/
 	void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
+
+	/*!
+		\brief 重置估计误差协方差
+		\param[in] error_covariance 误差协方差值
+		\retval none
+	*/
+	void resetCovariance(T error_covariance) { P = error_covariance; }
+
+	/*!
+		\brief 获取当前误差协方差
+		\retval 当前误差协方差
+	*/
+	T getCovariance() const { return P; }
+
+	/*!
+		\brief 获取当前过程噪声协方差
+		\retval 当前过程噪声协方差
+	*/
+	T getProcessNoise() const { return Q; }
 };
 
 /*!
@@ -124,8 +153,9 @@ class EncoderVelocityKalman {
 
 	void setMeasurementNoise(float r);
 
-	u8 init(float init_velocity) {
+	u8 init(float init_velocity, float initial_error_covariance = 1.0f) {
 		kalman.x = init_velocity;
+		kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
 		return 0;
 	}
 };
@@ -139,6 +169,9 @@ class EncoderAngleKalman {
 	KalmanFilter<float> kalman;
 	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,
@@ -153,12 +186,44 @@ class EncoderAngleKalman {
 	float getstate() const { return kalman.getState(); };
 
 	void setMeasurementNoise(float r);
-	u8 init(float init_angle) {
+	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;
 		return 0;
 	}
 };
 
+/*!
+	\brief 电流卡尔曼滤波器类
+	\note  用于滤除电流测量噪声的卡尔曼滤波器实现
+*/
+class CurrentKalman {
+  private:
+	KalmanFilter<float> kalman;
+
+  public:
+	CurrentKalman(float initial_current, float initial_error_covariance,
+				  float process_noise, float measurement_noise);
+
+	float update(float measured_current);
+
+	float getCurrent() const;
+
+	void setProcessNoise(float q);
+
+	void setMeasurementNoise(float r);
+
+	u8 init(float init_current, float initial_error_covariance = 1.0f) {
+		kalman.x = init_current;
+		kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
+		return 0;
+	}
+};
 #endif // __cplusplus
 
 #endif // _FILTER_H

Robot_V2/Algorithm/pid.cpp

@@ -2,12 +2,10 @@
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
 #include "Can.h"
-#include <algorithm>
+#include "config.h"
 #include <cmath>
 
 extern CAN_transmiter CAN0_transmiter;
-constexpr float TIME_SLICE = 1e-4;
-
 PIDController::PIDController(float P, float I, float D, float ramp, float limit)
 	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
 	  ,
@@ -68,4 +66,25 @@ void PIDController::set_PID(float P, float I, float D) {
  * @param P: P参数
  * @return void
  */
-void PIDController::set_P_percent(float pp) { this->P = pp; }
+void PIDController::set_P(float pp) {
+	this->P = pp;
+}
+
+/**
+ * @brief 设置输出限幅
+ *
+ * @param limit: 输出限幅
+ * @return void
+ */
+void PIDController::set_limit(float limit) { this->limit = limit; }
+
+/**
+ * @brief 重置PID控制器状态
+ *
+ * @return void
+ */
+void PIDController::reset() {
+	error_prev = 0.0f;
+	output_prev = 0.0f;
+	integral_prev = 0.0f;
+}

Robot_V2/Algorithm/pid.h

@@ -8,7 +8,9 @@ class PIDController {
 
 	float operator()(float error);
 	void set_PID(float P, float I, float D);
-	void set_P_percent(float pp);
+	void set_P(float pp);
+	void set_limit(float limit);
+	void reset();  // 重置PID控制器状态
 
 	float P; //!< 比例增益(P环增益)
 	float I; //!< 积分增益（I环增益）
@@ -21,5 +23,7 @@ class PIDController {
 	float output_prev;			  //!< 最后一个 pid 输出值
 	float integral_prev;		  //!< 最后一个积分分量值
 	unsigned long timestamp_prev; //!< 上次执行时间戳
+	float integral_separation_threshold;  //!< 积分分离阈值，用于抗积分饱和
 };
+
 #endif

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_adc.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_adc.c
     \brief   ADC driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -508,8 +508,8 @@ void adc_inserted_channel_config(uint32_t adc_periph , uint8_t rank , uint8_t ad
     inserted_length = (uint8_t)GET_BITS(ADC_ISQ(adc_periph) , 20U , 21U);
     /* the channel number is written to these bits to select a channel as the nth conversion in the inserted channel group */
     isq = ADC_ISQ(adc_periph);
-    isq &= ~((uint32_t)(ADC_ISQ_ISQN << (5U * ((3 + rank) - inserted_length))));
-    isq |= ((uint32_t)adc_channel << (5U * ((3 + rank) - inserted_length)));
+    isq &= ~((uint32_t)(ADC_ISQ_ISQN << (5U * ((3U + rank) - inserted_length))));
+    isq |= ((uint32_t)adc_channel << (5U * ((3U + rank) - inserted_length)));
     ADC_ISQ(adc_periph) = isq;
 
     /* ADC sampling time config */  
@@ -669,7 +669,6 @@ void adc_software_trigger_enable(uint32_t adc_periph , uint8_t adc_channel_group
 /*!
     \brief      read ADC regular group data register 
     \param[in]  adc_periph: ADCx,x=0,1
-    \param[in]  none
     \param[out] none
     \retval     the conversion value
 */
@@ -848,7 +847,6 @@ void adc_flag_clear(uint32_t adc_periph , uint32_t adc_flag)
 /*!
     \brief      get the bit state of ADCx software start conversion
     \param[in]  adc_periph: ADCx, x=0,1, only one among these parameters can be selected
-    \param[in]  none
     \param[out] none
     \retval     FlagStatus: SET or RESET
 */
@@ -864,7 +862,6 @@ FlagStatus adc_regular_software_startconv_flag_get(uint32_t adc_periph)
 /*!
     \brief      get the bit state of ADCx software inserted channel start conversion
     \param[in]  adc_periph: ADCx, x=0,1 only one among these parameters can be selected
-    \param[in]  none
     \param[out] none
     \retval     FlagStatus: SET or RESET
 */

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_bkp.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_bkp.c
     \brief   BKP driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -287,11 +287,14 @@ void bkp_interrupt_disable(void)
 */
 FlagStatus bkp_flag_get(void)
 {
+    FlagStatus ret;
+
     if(RESET != (BKP_TPCS & BKP_FLAG_TAMPER)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!
@@ -313,11 +316,14 @@ void bkp_flag_clear(void)
 */
 FlagStatus bkp_interrupt_flag_get(void)
 {
+    FlagStatus ret;
+
     if(RESET != (BKP_TPCS & BKP_INT_FLAG_TAMPER)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_can.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_can.c
     \brief   CAN driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:
@@ -33,7 +33,6 @@ OF SUCH DAMAGE.
 */
 
 #include "gd32c10x_can.h"
-#include "stdio.h"
 #include <stdlib.h>
 
 #define CAN_ERROR_HANDLE(s)         do{}while(1)
@@ -301,10 +300,11 @@ ErrStatus can_fd_init(uint32_t can_periph, can_fdframe_struct *can_fdframe_init)
 {
     uint32_t timeout = CAN_TIMEOUT;
     uint32_t tempreg = 0U;
+    ErrStatus reval = SUCCESS;
 
     /* check null pointer */
     if(0 == can_fdframe_init) {
-        return ERROR;
+        reval = ERROR;
     }
     /* disable sleep mode */
     CAN_CTL(can_periph) &= ~CAN_CTL_SLPWMOD;
@@ -316,7 +316,7 @@ ErrStatus can_fd_init(uint32_t can_periph, can_fdframe_struct *can_fdframe_init)
     }
     /* check initialize working success */
     if(CAN_STAT_IWS != (CAN_STAT(can_periph) & CAN_STAT_IWS)) {
-        return ERROR;
+        reval = ERROR;
     } else {
         /* set the data bit timing register */
         CAN_DBT(can_periph) = (BT_DSJW((uint32_t)can_fdframe_init->data_resync_jump_width) | \
@@ -342,7 +342,7 @@ ErrStatus can_fd_init(uint32_t can_periph, can_fdframe_struct *can_fdframe_init)
                 CAN_FDTDC(can_periph) = (FDTDC_TDCF(can_fdframe_init->p_delay_compensation->tdc_filter) | FDTDC_TDCO(
                                              can_fdframe_init->p_delay_compensation->tdc_offset));
             } else {
-                return ERROR;
+                reval = ERROR;
             }
         } else {
             /* Transmitter delay compensation mode is disabled */
@@ -366,11 +366,11 @@ ErrStatus can_fd_init(uint32_t can_periph, can_fdframe_struct *can_fdframe_init)
         }
         /* check exit initialize mode */
         if(0U == timeout) {
-            return ERROR;
+            reval = ERROR;
         }
     }
 
-    return SUCCESS;
+    return reval;
 }
 
 /*!
@@ -747,90 +747,89 @@ uint8_t can_message_transmit(uint32_t can_periph, can_trasnmit_message_struct *t
     } else {
         mailbox_number = CAN_NOMAILBOX;
     }
-    /* return no mailbox empty */
-    if(CAN_NOMAILBOX == mailbox_number) {
-        return CAN_NOMAILBOX;
-    }
     
-    CAN_TMI(can_periph, mailbox_number) &= CAN_TMI_TEN;
-    if(CAN_FF_STANDARD == transmit_message->tx_ff) {
-        /* set transmit mailbox standard identifier */
-        CAN_TMI(can_periph, mailbox_number) |= (uint32_t)(TMI_SFID(transmit_message->tx_sfid) | \
-                                               transmit_message->tx_ft);
-    } else {
-        /* set transmit mailbox extended identifier */
-        CAN_TMI(can_periph, mailbox_number) |= (uint32_t)(TMI_EFID(transmit_message->tx_efid) | \
-                                               transmit_message->tx_ff | \
-                                               transmit_message->tx_ft);
-    }
+    /* got one empty mailbox */
+    if(CAN_NOMAILBOX != mailbox_number) {
 
-    if(CAN_FDF_CLASSIC == transmit_message->fd_flag) {
-        /* set the data length */
-        CAN_TMP(can_periph, mailbox_number) &= ~(CAN_TMP_DLENC | CAN_TMP_ESI | CAN_TMP_BRS | CAN_TMP_FDF);
-        /* Classic CAN frame data length does not exceed 8 */
-        if (transmit_message->tx_dlen > 8U) {
-            transmit_message->tx_dlen = 8U;
+        CAN_TMI(can_periph, mailbox_number) &= CAN_TMI_TEN;
+        if(CAN_FF_STANDARD == transmit_message->tx_ff) {
+            /* set transmit mailbox standard identifier */
+            CAN_TMI(can_periph, mailbox_number) |= (uint32_t)(TMI_SFID(transmit_message->tx_sfid) | \
+                                                   transmit_message->tx_ft);
+        } else {
+            /* set transmit mailbox extended identifier */
+            CAN_TMI(can_periph, mailbox_number) |= (uint32_t)(TMI_EFID(transmit_message->tx_efid) | \
+                                                   transmit_message->tx_ff | \
+                                                   transmit_message->tx_ft);
         }
-        CAN_TMP(can_periph, mailbox_number) |= transmit_message->tx_dlen;
-        /* set the data */
-        CAN_TMDATA0(can_periph, mailbox_number) = TMDATA0_DB3(transmit_message->tx_data[3]) | \
-                TMDATA0_DB2(transmit_message->tx_data[2]) | \
-                TMDATA0_DB1(transmit_message->tx_data[1]) | \
-                TMDATA0_DB0(transmit_message->tx_data[0]);
-        CAN_TMDATA1(can_periph, mailbox_number) = TMDATA1_DB7(transmit_message->tx_data[7]) | \
-                TMDATA1_DB6(transmit_message->tx_data[6]) | \
-                TMDATA1_DB5(transmit_message->tx_data[5]) | \
-                TMDATA1_DB4(transmit_message->tx_data[4]);
-    } else {
-        canfd_en = CAN_FDCTL(can_periph) & CAN_FDCTL_FDEN;
-        /* check FD function has been enabled */
-        if(canfd_en) {
-            if(transmit_message->tx_dlen <= 8U) {
-                /* set the data length */
-                reg_temp |= transmit_message->tx_dlen;
-            } else {
-                /* data length greater than 8 */
-                for(i = 0U; i < 7U; i++) {
-                    if(transmit_message->tx_dlen == g_can_fdlength_table[i]) {
-                        hit = 1U;
-                        break;
+        
+        if(CAN_FDF_CLASSIC == transmit_message->fd_flag) {
+            /* set the data length */
+            CAN_TMP(can_periph, mailbox_number) &= ~(CAN_TMP_DLENC | CAN_TMP_ESI | CAN_TMP_BRS | CAN_TMP_FDF);
+            /* Classic CAN frame data length does not exceed 8 */
+            if (transmit_message->tx_dlen > 8U) {
+                transmit_message->tx_dlen = 8U;
+            }
+            CAN_TMP(can_periph, mailbox_number) |= transmit_message->tx_dlen;
+            /* set the data */
+            CAN_TMDATA0(can_periph, mailbox_number) = TMDATA0_DB3(transmit_message->tx_data[3]) | \
+                    TMDATA0_DB2(transmit_message->tx_data[2]) | \
+                    TMDATA0_DB1(transmit_message->tx_data[1]) | \
+                    TMDATA0_DB0(transmit_message->tx_data[0]);
+            CAN_TMDATA1(can_periph, mailbox_number) = TMDATA1_DB7(transmit_message->tx_data[7]) | \
+                    TMDATA1_DB6(transmit_message->tx_data[6]) | \
+                    TMDATA1_DB5(transmit_message->tx_data[5]) | \
+                    TMDATA1_DB4(transmit_message->tx_data[4]);
+        } else {
+            canfd_en = CAN_FDCTL(can_periph) & CAN_FDCTL_FDEN;
+            /* check FD function has been enabled */
+            if(canfd_en) {
+                if(transmit_message->tx_dlen <= 8U) {
+                    /* set the data length */
+                    reg_temp |= transmit_message->tx_dlen;
+                } else {
+                    /* data length greater than 8 */
+                    for(i = 0U; i < 7U; i++) {
+                        if(transmit_message->tx_dlen == g_can_fdlength_table[i]) {
+                            hit = 1U;
+                            break;
+                        }
+                    }
+                    /* data length is valid */
+                    if(1U == hit) {
+                        reg_temp |= 9U + i;
+                    } else {
+                        CAN_ERROR_HANDLE("dlen is invalid \r\n");
                     }
                 }
-                /* data length is valid */
-                if(1U == hit) {
-                    reg_temp |= 9U + i;
-                } else {
-                    CAN_ERROR_HANDLE("dlen is invalid \r\n");
+                reg_temp |= (((uint32_t)transmit_message->fd_brs << 5U) | ((uint32_t)transmit_message->fd_esi << 4U) | ((uint32_t)transmit_message->fd_flag << 7U));
+                CAN_TMP(can_periph, mailbox_number) = reg_temp;
+        
+                /* set the data */
+                i = transmit_message->tx_dlen / 4U;
+        
+                /* data length is 5-7 need send 2 word */
+                if((1U == i) && (4U != transmit_message->tx_dlen)) {
+                    i++;
                 }
-            }
-            reg_temp |= (((uint32_t)transmit_message->fd_brs << 5U) | ((uint32_t)transmit_message->fd_esi << 4U) | ((uint32_t)transmit_message->fd_flag << 7U));
-            CAN_TMP(can_periph, mailbox_number) = reg_temp;
-
-            /* set the data */
-            i = transmit_message->tx_dlen / 4U;
-
-            /* data length is 5-7 need send 2 word */
-            if((1U == i) && (4U != transmit_message->tx_dlen)) {
-                i++;
-            }
-            p_temp = (uint32_t)transmit_message->tx_data;
-            if((0U == i)) {
-                CAN_TMDATA0(can_periph, mailbox_number) = *(uint32_t *)p_temp;
-            } else {
-                for(; i > 0U; i--) {
+                p_temp = (uint32_t)transmit_message->tx_data;
+                if((0U == i)) {
                     CAN_TMDATA0(can_periph, mailbox_number) = *(uint32_t *)p_temp;
-                    p_temp = ((uint32_t)((uint32_t)p_temp + 4U));
+                } else {
+                    for(; i > 0U; i--) {
+                        CAN_TMDATA0(can_periph, mailbox_number) = *(uint32_t *)p_temp;
+                        p_temp = ((uint32_t)((uint32_t)p_temp + 4U));
+                    }
                 }
+        
+            } else {
+                CAN_ERROR_HANDLE("CAN FD function disabled \r\n");
             }
-
-        } else {
-            CAN_ERROR_HANDLE("CAN FD function disabled \r\n");
         }
+        
+        /* enable transmission */
+        CAN_TMI(can_periph, mailbox_number) |= CAN_TMI_TEN;
     }
-
-    /* enable transmission */
-    CAN_TMI(can_periph, mailbox_number) |= CAN_TMI_TEN;
-
     return mailbox_number;
 }
 
@@ -901,25 +900,63 @@ can_transmit_state_enum can_transmit_states(uint32_t can_periph, uint8_t mailbox
                 only one parameter can be selected which is shown as below:
       \arg        CAN_MAILBOXx(x=0,1,2)
     \param[out] none
-    \retval     none
+    \retval     ErrStatus: ERROR or SUCCESS
 */
-void can_transmission_stop(uint32_t can_periph, uint8_t mailbox_number)
+ErrStatus can_transmission_stop(uint32_t can_periph, uint8_t mailbox_number)
 {
+    ErrStatus reval = SUCCESS;
+    /* timeout for CAN_TSTAT_MSTx bits */
+    uint32_t timeout = CAN_TIMEOUT;
+    uint32_t reg_value0 = 0U;
+    uint32_t reg_value1 = 0U;
+
+    /* get the status of transmit FIFO order */
+    reg_value0 = CAN_CTL(can_periph) & CAN_CTL_TFO;
+
     if(CAN_MAILBOX0 == mailbox_number) {
-        CAN_TSTAT(can_periph) |= CAN_TSTAT_MST0;
-        while(CAN_TSTAT_MST0 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST0)) {
+        reg_value1 = CAN_TSTAT(can_periph) & (CAN_TSTAT_TMLS0 | CAN_ALL_MAILBOX_EMPTY);
+        if((CAN_CTL_TFO == reg_value0) && (CAN_TSTAT_TMLS0 == reg_value1)){
+            reval = ERROR;
+        } else {
+            CAN_TSTAT(can_periph) |= CAN_TSTAT_MST0;
+            while((CAN_TSTAT_MST0 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST0)) && (0U != timeout)) {
+                timeout--;
+            }
+            if(CAN_TSTAT_MST0 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST0)){
+                reval = ERROR;
+            }
         }
     } else if(CAN_MAILBOX1 == mailbox_number) {
-        CAN_TSTAT(can_periph) |= CAN_TSTAT_MST1;
-        while(CAN_TSTAT_MST1 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST1)) {
+        reg_value1 = CAN_TSTAT(can_periph) & (CAN_TSTAT_TMLS1 | CAN_ALL_MAILBOX_EMPTY);
+        if((CAN_CTL_TFO == reg_value0) && (CAN_TSTAT_TMLS1 == reg_value1)){
+            reval = ERROR;
+        }else{
+            CAN_TSTAT(can_periph) |= CAN_TSTAT_MST1;
+            while((CAN_TSTAT_MST1 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST1)) && (0U != timeout)) {
+                timeout--;
+            }
+            if(CAN_TSTAT_MST1 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST1)){
+                reval = ERROR;
+            }
         }
+        
     } else if(CAN_MAILBOX2 == mailbox_number) {
-        CAN_TSTAT(can_periph) |= CAN_TSTAT_MST2;
-        while(CAN_TSTAT_MST2 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST2)) {
+        reg_value1 = CAN_TSTAT(can_periph) & (CAN_TSTAT_TMLS2 | CAN_ALL_MAILBOX_EMPTY);
+        if((CAN_CTL_TFO == reg_value0) && (CAN_TSTAT_TMLS2 == reg_value1)){
+            reval = ERROR;
+        }else{
+            CAN_TSTAT(can_periph) |= CAN_TSTAT_MST2;
+            while((CAN_TSTAT_MST2 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST2)) && (0U != timeout)) {
+                timeout--;
+            }
+            if(CAN_TSTAT_MST2 == (CAN_TSTAT(can_periph) & CAN_TSTAT_MST2)){
+                reval = ERROR;
+            }
         }
     } else {
         /* illegal parameters */
     }
+    return reval;
 }
 
 /*!
@@ -976,13 +1013,10 @@ void can_message_receive(uint32_t can_periph, uint8_t fifo_number, can_receive_m
             receive_message->rx_data[5] = (uint8_t)(GET_RFIFOMDATA1_DB5(CAN_RFIFOMDATA1(can_periph, fifo_number)));
             receive_message->rx_data[6] = (uint8_t)(GET_RFIFOMDATA1_DB6(CAN_RFIFOMDATA1(can_periph, fifo_number)));
             receive_message->rx_data[7] = (uint8_t)(GET_RFIFOMDATA1_DB7(CAN_RFIFOMDATA1(can_periph, fifo_number)));
-
         } else {
-			printf("CAN FD function disabled but FD frame received\r\n");
-            // CAN_ERROR_HANDLE("CAN FD function disabled \r\n");
+            CAN_ERROR_HANDLE("CAN FD function disabled \r\n");
         }
     } else {
-		
         /* check FD function has been enabled */
         /* get receive data length */
         canfd_recv_cnt = (uint8_t)(GET_RFIFOMP_DLENC(CAN_RFIFOMP(can_periph, fifo_number)));
@@ -1322,12 +1356,14 @@ void can_interrupt_disable(uint32_t can_periph, uint32_t interrupt)
 */
 FlagStatus can_flag_get(uint32_t can_periph, can_flag_enum flag)
 {
+    FlagStatus retval;
     /* get flag and interrupt enable state */
     if(RESET != (CAN_REG_VAL(can_periph, flag) & BIT(CAN_BIT_POS(flag)))) {
-        return SET;
+        retval = SET;
     } else {
-        return RESET;
+        retval = RESET;
     }
+    return retval;
 }
 
 /*!
@@ -1390,6 +1426,7 @@ void can_flag_clear(uint32_t can_periph, can_flag_enum flag)
 */
 FlagStatus can_interrupt_flag_get(uint32_t can_periph, can_interrupt_flag_enum flag)
 {
+    FlagStatus retval;
     uint32_t ret1 = RESET;
     uint32_t ret2 = RESET;
 
@@ -1406,10 +1443,11 @@ FlagStatus can_interrupt_flag_get(uint32_t can_periph, can_interrupt_flag_enum f
     /* get the status of interrupt enable bit */
     ret2 = CAN_INTEN(can_periph) & BIT(CAN_BIT_POS1(flag));
     if(ret1 && ret2) {
-        return SET;
+        retval = SET;
     } else {
-        return RESET;
+        retval = RESET;
     }
+    return retval;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_crc.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_crc.c
     \brief   CRC driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -117,11 +117,11 @@ uint32_t crc_single_data_calculate(uint32_t sdata)
     \param[out] none
     \retval     32-bit value calculated by CRC
 */
-uint32_t crc_block_data_calculate(const uint32_t *array, uint32_t size)
+uint32_t crc_block_data_calculate(uint32_t array[], uint32_t size)
 {
     uint32_t index;
     for(index = 0U; index < size; index++){
-        CRC_DATA = *(array+index);
+        CRC_DATA = array[index];
     }
     return (CRC_DATA);
 }

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_ctc.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_ctc.c
     \brief   CTC driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -79,7 +79,7 @@ void ctc_counter_disable(void)
 
 /*!
     \brief      configure the IRC48M trim value
-    \param[in]  ctc_trim_value: 8-bit IRC48M trim value
+    \param[in]  trim_value: 8-bit IRC48M trim value
       \arg        0x00 - 0x3F
     \param[out] none
     \retval     none
@@ -218,11 +218,14 @@ uint16_t ctc_counter_capture_value_read(void)
 */
 FlagStatus ctc_counter_direction_read(void)
 {
+    FlagStatus ret;
+
     if(RESET != (CTC_STAT & CTC_STAT_REFDIR)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!
@@ -300,6 +303,7 @@ void ctc_interrupt_disable(uint32_t interrupt)
 FlagStatus ctc_interrupt_flag_get(uint32_t interrupt)
 {
     uint32_t intflag = 0U, intenable = 0U;
+    FlagStatus ret;
 
     /* check whether the interrupt is enabled */
     if(RESET != (interrupt & CTC_FLAG_MASK)){
@@ -312,10 +316,11 @@ FlagStatus ctc_interrupt_flag_get(uint32_t interrupt)
     intflag = CTC_STAT & interrupt;
 
     if(intflag && intenable){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!
@@ -357,11 +362,14 @@ void ctc_interrupt_flag_clear(uint32_t interrupt)
 */
 FlagStatus ctc_flag_get(uint32_t flag)
 {
+    FlagStatus ret;
+
     if(RESET != (CTC_STAT & flag)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_dac.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_dac.c
     \brief   DAC driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -318,6 +318,7 @@ void dac_trigger_source_config(uint32_t dac_periph, uint8_t dac_out, uint32_t tr
     \brief      enable DAC software trigger
     \param[in]  dac_periph: DACx(x=0)
     \param[in]  dac_out: DAC_OUTx(x=0,1)
+    \param[out] none
     \retval     none
 */
 void dac_software_trigger_enable(uint32_t dac_periph, uint8_t dac_out)

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_dbg.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_dbg.c
     \brief   DBG driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_dma.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_dma.c
     \brief   DMA driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -614,6 +614,7 @@ void dma_flag_clear(uint32_t dma_periph, dma_channel_enum channelx, uint32_t fla
 FlagStatus dma_interrupt_flag_get(uint32_t dma_periph, dma_channel_enum channelx, uint32_t flag)
 {
     uint32_t interrupt_enable = 0U, interrupt_flag = 0U;
+    FlagStatus status = RESET;
     
     switch(flag){
         case DMA_INT_FLAG_FTF:
@@ -637,10 +638,11 @@ FlagStatus dma_interrupt_flag_get(uint32_t dma_periph, dma_channel_enum channelx
     
     /* when the interrupt flag is set and enabled, return SET */
     if((0U != interrupt_flag) && (0U != interrupt_enable)){
-        return SET;
+        status = SET;
     }else{
-        return RESET;
+        status = RESET;
     }
+    return status;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_exmc.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_exmc.c
     \brief   EXMC driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_exti.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_exti.c
     \brief   EXTI driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:
@@ -196,11 +196,14 @@ void exti_software_interrupt_disable(exti_line_enum linex)
 */
 FlagStatus exti_flag_get(exti_line_enum linex)
 {
+    FlagStatus ret;
+
     if(RESET != (EXTI_PD & (uint32_t)linex)) {
-        return SET;
+        ret = SET;
     } else {
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!
@@ -226,11 +229,14 @@ void exti_flag_clear(exti_line_enum linex)
 */
 FlagStatus exti_interrupt_flag_get(exti_line_enum linex)
 {
+    FlagStatus ret;
+
     if(RESET != (EXTI_PD & (uint32_t)linex)) {
-        return SET;
+        ret = SET;
     } else {
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_fmc.c
     \brief   FMC driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -33,7 +33,6 @@ OF SUCH DAMAGE.
 */
 
 #include "gd32c10x_fmc.h"
-#include "stdio.h"
 
 /* FMC mask */
 #define LOW_8BITS_MASK             ((uint32_t)0x000000FFU)
@@ -508,7 +507,7 @@ fmc_state_enum ob_write_protection_enable(uint32_t ob_wp)
 */
 fmc_state_enum ob_security_protection_config(uint8_t ob_spc)
 {
-    uint8_t i;
+    uint32_t i;
     uint32_t op_byte[4];
     fmc_state_enum fmc_state = fmc_ready_wait(FMC_TIMEOUT_COUNT);
     for(i = 0U; i < 4U; i++){
@@ -574,7 +573,7 @@ fmc_state_enum ob_security_protection_config(uint8_t ob_spc)
 */
 fmc_state_enum ob_user_write(uint8_t ob_fwdgt, uint8_t ob_deepsleep, uint8_t ob_stdby)
 {
-    uint8_t i;
+    uint32_t i;
     uint32_t temp;
     uint32_t op_byte[4];
     fmc_state_enum fmc_state = fmc_ready_wait(FMC_TIMEOUT_COUNT);
@@ -582,7 +581,7 @@ fmc_state_enum ob_user_write(uint8_t ob_fwdgt, uint8_t ob_deepsleep, uint8_t ob_
     for(i = 0U; i < 4U; i++){
         op_byte[i] = OP_BYTE(i);
     }
-    temp = ((uint8_t)((uint8_t)((uint8_t)(ob_fwdgt) | ob_deepsleep) | ob_stdby) | (OB_USER_MASK));
+    temp = (uint32_t)(((uint32_t)((uint32_t)(ob_fwdgt) | ob_deepsleep) | ob_stdby) | OB_USER_MASK);
     op_byte[0] = ((uint32_t)(temp) << 16U) | ((op_byte[0] & LOW_16BITS_MASK));
 
     if(FMC_READY == fmc_state){
@@ -632,7 +631,7 @@ fmc_state_enum ob_user_write(uint8_t ob_fwdgt, uint8_t ob_deepsleep, uint8_t ob_
 
 fmc_state_enum ob_data_program(uint16_t ob_data)
 {
-    uint8_t i;
+    uint32_t i;
     uint32_t op_byte[4];
     fmc_state_enum fmc_state = fmc_ready_wait(FMC_TIMEOUT_COUNT);
     for(i = 0U; i < 4U; i++){
@@ -774,17 +773,21 @@ void fmc_ecc_interrupt_disable(uint32_t interrupt)
 FlagStatus fmc_ecc_interrupt_flag_get(uint32_t int_flag)
 {
     uint32_t intenable = 0U, flagstatus = 0U;
+
+    FlagStatus status = RESET;
     /* get the interrupt enable bit status */
     intenable = (FMC_REG_VAL(int_flag) & BIT(FMC_BIT_POS(int_flag)));
     /* get the corresponding flag bit status */
     flagstatus = (FMC_REG_VAL2(int_flag) & BIT(FMC_BIT_POS2(int_flag)));
 
     if(flagstatus && intenable) {
-        return SET;
+        status = SET;
     } else {
-        return RESET;
+        status = RESET;
     }
 
+    /* return the state of corresponding FMC interrupt flag */
+    return status;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fwdgt.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_fwdgt.c
     \brief   FWDGT driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:
@@ -84,24 +84,25 @@ void fwdgt_enable(void)
 ErrStatus fwdgt_prescaler_value_config(uint16_t prescaler_value)
 {
     uint32_t timeout = FWDGT_PSC_TIMEOUT;
-    uint32_t flag_status = RESET;
+    uint32_t flag_status;
+    ErrStatus status = SUCCESS;
 
     /* enable write access to FWDGT_PSC */
     FWDGT_CTL = FWDGT_WRITEACCESS_ENABLE;
 
+    /* configure FWDGT_PSC */
+    FWDGT_PSC = (uint32_t)prescaler_value;
+
     /* wait until the PUD flag to be reset */
     do {
         flag_status = FWDGT_STAT & FWDGT_STAT_PUD;
-    } while((--timeout > 0U) && ((uint32_t)RESET != flag_status));
+    } while((--timeout > 0U) && (0U != flag_status));
 
-    if((uint32_t)RESET != flag_status) {
-        return ERROR;
+    if(0U != flag_status) {
+        status = ERROR;
     }
 
-    /* configure FWDGT */
-    FWDGT_PSC = (uint32_t)prescaler_value;
-
-    return SUCCESS;
+    return status;
 }
 
 /*!
@@ -113,23 +114,25 @@ ErrStatus fwdgt_prescaler_value_config(uint16_t prescaler_value)
 ErrStatus fwdgt_reload_value_config(uint16_t reload_value)
 {
     uint32_t timeout = FWDGT_RLD_TIMEOUT;
-    uint32_t flag_status = RESET;
+    uint32_t flag_status;
+    ErrStatus status = SUCCESS;
 
     /* enable write access to FWDGT_RLD */
     FWDGT_CTL = FWDGT_WRITEACCESS_ENABLE;
 
+    /* configure FWDGT_RLD */
+    FWDGT_RLD = RLD_RLD(reload_value);
+
     /* wait until the RUD flag to be reset */
     do {
         flag_status = FWDGT_STAT & FWDGT_STAT_RUD;
-    } while((--timeout > 0U) && ((uint32_t)RESET != flag_status));
+    } while((--timeout > 0U) && (0U != flag_status));
 
-    if((uint32_t)RESET != flag_status) {
-        return ERROR;
+    if(0U != flag_status) {
+        status = ERROR;
     }
 
-    FWDGT_RLD = RLD_RLD(reload_value);
-
-    return SUCCESS;
+    return status;
 }
 
 /*!
@@ -146,7 +149,7 @@ void fwdgt_counter_reload(void)
 /*!
     \brief      configure counter reload value, and prescaler divider value
     \param[in]  reload_value: specify reload value(0x0000 - 0x0FFF)
-    \param[in]  prescaler_div: FWDGT prescaler value
+    \param[in]  prescaler_value: FWDGT prescaler value
                 only one parameter can be selected which is shown as below:
       \arg        FWDGT_PSC_DIV4: FWDGT prescaler set to 4
       \arg        FWDGT_PSC_DIV8: FWDGT prescaler set to 8
@@ -158,42 +161,51 @@ void fwdgt_counter_reload(void)
     \param[out] none
     \retval     ErrStatus: ERROR or SUCCESS
 */
-ErrStatus fwdgt_config(uint16_t reload_value, uint8_t prescaler_div)
+ErrStatus fwdgt_config(uint16_t reload_value, uint8_t prescaler_value)
 {
     uint32_t timeout = FWDGT_PSC_TIMEOUT;
-    uint32_t flag_status = RESET;
+    uint32_t flag_status;
+    ErrStatus status = SUCCESS;
+
+    /* start the free watchdog timer counter */
+    FWDGT_CTL = FWDGT_KEY_ENABLE;
 
     /* enable write access to FWDGT_PSC,and FWDGT_RLD */
     FWDGT_CTL = FWDGT_WRITEACCESS_ENABLE;
 
+    /* configure FWDGT_PSC */
+    FWDGT_PSC = (uint32_t)prescaler_value;
+
     /* wait until the PUD flag to be reset */
     do {
         flag_status = FWDGT_STAT & FWDGT_STAT_PUD;
-    } while((--timeout > 0U) && ((uint32_t)RESET != flag_status));
+    } while((--timeout > 0U) && (0U != flag_status));
 
-    if((uint32_t)RESET != flag_status) {
-        return ERROR;
+    if(0U != flag_status) {
+        status = ERROR;
     }
 
-    /* configure FWDGT */
-    FWDGT_PSC = (uint32_t)prescaler_div;
+    if(SUCCESS == status) {
+        /* configure FWDGT_RLD */
+        FWDGT_RLD = RLD_RLD(reload_value);
 
-    timeout = FWDGT_RLD_TIMEOUT;
-    /* wait until the RUD flag to be reset */
-    do {
-        flag_status = FWDGT_STAT & FWDGT_STAT_RUD;
-    } while((--timeout > 0U) && ((uint32_t)RESET != flag_status));
+        /* wait until the RUD flag to be reset */
+        timeout = FWDGT_RLD_TIMEOUT;
+        do {
+            flag_status = FWDGT_STAT & FWDGT_STAT_RUD;
+        } while((--timeout > 0U) && (0U != flag_status));
 
-    if((uint32_t)RESET != flag_status) {
-        return ERROR;
+        if(0U != flag_status) {
+            status = ERROR;
+        }
     }
 
-    FWDGT_RLD = RLD_RLD(reload_value);
+    if(SUCCESS == status) {
+        /* reload the counter */
+        FWDGT_CTL = FWDGT_KEY_RELOAD;
+    }
 
-    /* reload the counter */
-    FWDGT_CTL = FWDGT_KEY_RELOAD;
-
-    return SUCCESS;
+    return status;
 }
 
 /*!
@@ -207,9 +219,10 @@ ErrStatus fwdgt_config(uint16_t reload_value, uint8_t prescaler_div)
 */
 FlagStatus fwdgt_flag_get(uint16_t flag)
 {
-    if(FWDGT_STAT & flag) {
-        return SET;
-    }
+    FlagStatus flag_status = RESET;
 
-    return RESET;
+    if(RESET != (FWDGT_STAT & flag)) {
+        flag_status = SET;
+    }
+    return flag_status;
 }

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_gpio.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_gpio.c
     \brief   GPIO driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -264,11 +264,13 @@ void gpio_port_write(uint32_t gpio_periph,uint16_t data)
 */
 FlagStatus gpio_input_bit_get(uint32_t gpio_periph,uint32_t pin)
 {
+    FlagStatus status = RESET;
     if((uint32_t)RESET != (GPIO_ISTAT(gpio_periph)&(pin))){
-        return SET; 
+        status = SET;
     }else{
-        return RESET;
+        status = RESET;
     }
+    return status;
 }
 
 /*!
@@ -293,11 +295,13 @@ uint16_t gpio_input_port_get(uint32_t gpio_periph)
 */
 FlagStatus gpio_output_bit_get(uint32_t gpio_periph, uint32_t pin)
 {
+    FlagStatus status = RESET;
     if((uint32_t)RESET !=(GPIO_OCTL(gpio_periph)&(pin))){
-        return SET;
+        status = SET;
     }else{
-        return RESET;
+        status = RESET;
     }
+    return status;
 }
 
 /*!
@@ -530,9 +534,11 @@ void gpio_compensation_config(uint32_t compensation)
   */
 FlagStatus gpio_compensation_flag_get(void)
 {
+    FlagStatus status = RESET;
     if(((uint32_t)RESET) != (AFIO_CPSCTL & AFIO_CPSCTL_CPS_RDY)){
-        return SET;
+        status = SET;
     }else{
-        return RESET;
+        status = RESET;
     }
+    return status;
 }

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_i2c.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_i2c.c
     \brief   I2C driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -635,11 +635,13 @@ void i2c_sam_timeout_disable(uint32_t i2c_periph)
 */
 FlagStatus i2c_flag_get(uint32_t i2c_periph, i2c_flag_enum flag)
 {
+    FlagStatus status = RESET;
     if(RESET != (I2C_REG_VAL(i2c_periph, flag) & BIT(I2C_BIT_POS(flag)))){
-        return SET;
+        status = SET;
     }else{
-        return RESET;
+        status = RESET;
     }
+    return status;
 }
 
 /*!
@@ -742,6 +744,7 @@ void i2c_interrupt_disable(uint32_t i2c_periph, i2c_interrupt_enum interrupt)
 FlagStatus i2c_interrupt_flag_get(uint32_t i2c_periph, i2c_interrupt_flag_enum int_flag)
 {
     uint32_t intenable = 0U, flagstatus = 0U, bufie;
+    FlagStatus status = RESET;
 
     /* check BUFIE */
     bufie = I2C_CTL1(i2c_periph)&I2C_CTL1_BUFIE;
@@ -759,10 +762,11 @@ FlagStatus i2c_interrupt_flag_get(uint32_t i2c_periph, i2c_interrupt_flag_enum i
         }
     }
     if((0U != flagstatus) && (0U != intenable)){
-        return SET;
+        status = SET;
     }else{
-        return RESET; 
+        status = RESET; 
     }
+    return status;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_misc.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_misc.c
     \brief   MISC driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:
@@ -63,45 +63,31 @@ void nvic_irq_enable(IRQn_Type nvic_irq,
                      uint8_t nvic_irq_pre_priority,
                      uint8_t nvic_irq_sub_priority)
 {
-    uint32_t temp_priority = 0x00U, temp_pre = 0x00U, temp_sub = 0x00U;
+    uint32_t nvic_prigroup, nvic_priority;
 
-    /* use the priority group value to get the temp_pre and the temp_sub */
-    switch((SCB->AIRCR) & (uint32_t)0x700U) {
+    /* check current priority group */
+    switch(SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) {
     case NVIC_PRIGROUP_PRE0_SUB4:
-        temp_pre = 0U;
-        temp_sub = 0x4U;
-        break;
     case NVIC_PRIGROUP_PRE1_SUB3:
-        temp_pre = 1U;
-        temp_sub = 0x3U;
-        break;
     case NVIC_PRIGROUP_PRE2_SUB2:
-        temp_pre = 2U;
-        temp_sub = 0x2U;
-        break;
     case NVIC_PRIGROUP_PRE3_SUB1:
-        temp_pre = 3U;
-        temp_sub = 0x1U;
-        break;
     case NVIC_PRIGROUP_PRE4_SUB0:
-        temp_pre = 4U;
-        temp_sub = 0x0U;
         break;
     default:
         nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
-        temp_pre = 2U;
-        temp_sub = 0x2U;
         break;
     }
 
-    /* get the temp_priority to fill the NVIC->IP register */
-    temp_priority = (uint32_t)nvic_irq_pre_priority << (0x4U - temp_pre);
-    temp_priority |= nvic_irq_sub_priority & (0x0FU >> (0x4U - temp_sub));
-    temp_priority = temp_priority << 0x04U;
-    NVIC->IP[nvic_irq] = (uint8_t)temp_priority;
+    /* get the priority group value */
+    nvic_prigroup = NVIC_GetPriorityGrouping();
+
+    /* encoding the pre-emption, subpriority priority */
+    nvic_priority = NVIC_EncodePriority(nvic_prigroup, (uint32_t)nvic_irq_pre_priority, (uint32_t)nvic_irq_sub_priority);
+    /* set priority */
+    NVIC_SetPriority(nvic_irq, nvic_priority);
 
     /* enable the selected IRQ */
-    NVIC->ISER[nvic_irq >> 0x05U] = (uint32_t)0x01U << (nvic_irq & (uint8_t)0x1FU);
+    NVIC_EnableIRQ(nvic_irq);
 }
 
 /*!
@@ -113,7 +99,7 @@ void nvic_irq_enable(IRQn_Type nvic_irq,
 void nvic_irq_disable(IRQn_Type nvic_irq)
 {
     /* disable the selected IRQ.*/
-    NVIC->ICER[nvic_irq >> 0x05U] = (uint32_t)0x01U << (nvic_irq & (uint8_t)0x1FU);
+    NVIC->ICER[(uint32_t)nvic_irq >> 0x05U] = (uint32_t)0x01U << ((uint32_t)nvic_irq & (uint8_t)0x1FU);
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_pmu.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_pmu.c
     \brief   PMU driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -261,11 +261,14 @@ void pmu_backup_write_disable(void)
 */
 FlagStatus pmu_flag_get(uint32_t flag)
 {
+    FlagStatus ret;
+
     if(RESET != (PMU_CS & flag)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_rcu.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_rcu.c
     \brief   RCU driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -565,7 +565,7 @@ void rcu_adc_clock_config(uint32_t adc_psc)
 
 /*!
     \brief      configure the USBFS prescaler factor
-    \param[in]  adc_div: USB prescaler factor
+    \param[in]  usb_psc: USB prescaler factor
                 only one parameter can be selected which is shown as below:
       \arg        RCU_CKUSB_CKPLL_DIV1_5: USBFS prescaler select CK_PLL/1.5
       \arg        RCU_CKUSB_CKPLL_DIV1: USBFS prescaler select CK_PLL/1
@@ -690,12 +690,16 @@ void rcu_ck48m_clock_config(uint32_t ck48m_clock_source)
 */
 FlagStatus rcu_flag_get(rcu_flag_enum flag)
 {
+    FlagStatus rcu_flag = RESET;
+
     /* get the rcu flag */
     if(RESET != (RCU_REG_VAL(flag) & BIT(RCU_BIT_POS(flag)))){
-        return SET;
+        rcu_flag = SET;
     }else{
-        return RESET;
+        rcu_flag = RESET;
     }
+
+    return rcu_flag;
 }
 
 /*!
@@ -727,12 +731,16 @@ void rcu_all_reset_flag_clear(void)
 */
 FlagStatus rcu_interrupt_flag_get(rcu_int_flag_enum int_flag)
 {
+    FlagStatus rcu_int_flag = RESET;
+
     /* get the rcu interrupt flag */
     if(RESET != (RCU_REG_VAL(int_flag) & BIT(RCU_BIT_POS(int_flag)))){
-        return SET;
+        rcu_int_flag = SET;
     }else{
-        return RESET;
+        rcu_int_flag = RESET;
     }
+
+    return rcu_int_flag;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_rtc.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_rtc.c
     \brief   RTC driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -191,11 +191,14 @@ uint32_t rtc_divider_get(void)
 */
 FlagStatus rtc_flag_get(uint32_t flag)
 {
+    FlagStatus ret;
+
     if(RESET != (RTC_CTL & flag)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!
@@ -227,11 +230,14 @@ void rtc_flag_clear(uint32_t flag)
 */
 FlagStatus rtc_interrupt_flag_get(uint32_t flag)
 {
+    FlagStatus ret;
+
     if(RESET != (RTC_CTL & flag)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_spi.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_spi.c
     \brief   SPI driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -564,11 +564,14 @@ void spi_crc_next(uint32_t spi_periph)
 */
 uint16_t spi_crc_get(uint32_t spi_periph,uint8_t crc)
 {
+    uint16_t crc_value;
     if(SPI_CRC_TX == crc){
-        return ((uint16_t)(SPI_TCRC(spi_periph)));
+        crc_value = ((uint16_t)(SPI_TCRC(spi_periph)));
     }else{
-        return ((uint16_t)(SPI_RCRC(spi_periph)));
+        crc_value = ((uint16_t)(SPI_RCRC(spi_periph)));
     }
+
+    return crc_value;
 }
 
 /*!
@@ -670,28 +673,6 @@ void spi_quad_read_enable(uint32_t spi_periph)
     SPI_QCTL(spi_periph) |= (uint32_t)SPI_QCTL_QRD;
 }
 
-/*!
-    \brief      enable SPI quad wire mode SPI_IO2 and SPI_IO3 pin output
-    \param[in]  spi_periph: SPIx(only x=0)
-    \param[out] none
-    \retval     none
-*/
-void spi_quad_io23_output_enable(uint32_t spi_periph)
-{
-    SPI_QCTL(spi_periph) |= (uint32_t)SPI_QCTL_IO23_DRV;
-}
-
- /*!
-    \brief      disable SPI quad wire mode SPI_IO2 and SPI_IO3 pin output
-    \param[in]  spi_periph: SPIx(only x=0)
-    \param[out] none
-    \retval     none
-*/
-void spi_quad_io23_output_disable(uint32_t spi_periph)
-{
-    SPI_QCTL(spi_periph) &= (uint32_t)(~SPI_QCTL_IO23_DRV);
-}
-
 /*!
     \brief      enable SPI and I2S interrupt 
     \param[in]  spi_periph: SPIx(x=0,1,2)
@@ -773,6 +754,7 @@ void spi_i2s_interrupt_disable(uint32_t spi_periph, uint8_t interrupt)
 */
 FlagStatus spi_i2s_interrupt_flag_get(uint32_t spi_periph, uint8_t interrupt)
 {
+    FlagStatus status = RESET;
     uint32_t reg1 = SPI_STAT(spi_periph);
     uint32_t reg2 = SPI_CTL1(spi_periph);
 
@@ -817,10 +799,12 @@ FlagStatus spi_i2s_interrupt_flag_get(uint32_t spi_periph, uint8_t interrupt)
     }
     /* get SPI/I2S interrupt flag status */
     if((0U != reg1) && (0U != reg2)){
-        return SET;
+        status = SET;
     }else{
-        return RESET;
+        status = RESET;
     }
+
+    return status;
 }
 
 /*!
@@ -847,9 +831,12 @@ FlagStatus spi_i2s_interrupt_flag_get(uint32_t spi_periph, uint8_t interrupt)
 */
 FlagStatus spi_i2s_flag_get(uint32_t spi_periph, uint32_t flag)
 {
+    FlagStatus status = RESET;
     if(RESET != (SPI_STAT(spi_periph) & flag)){
-        return SET;
+        status = SET;
     }else{
-        return RESET;
+        status = RESET;
     }
+
+    return status;
 }

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_timer.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_timer.c
     \brief   TIMER driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -1955,12 +1955,14 @@ void timer_interrupt_disable(uint32_t timer_periph, uint32_t interrupt)
 FlagStatus timer_interrupt_flag_get(uint32_t timer_periph, uint32_t interrupt)
 {
     uint32_t val;
+    FlagStatus ret;
     val = (TIMER_DMAINTEN(timer_periph) & interrupt);
     if((RESET != (TIMER_INTF(timer_periph) & interrupt)) && (RESET != val)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!
@@ -2006,11 +2008,13 @@ void timer_interrupt_flag_clear(uint32_t timer_periph, uint32_t interrupt)
 */
 FlagStatus timer_flag_get(uint32_t timer_periph, uint32_t flag)
 {
+    FlagStatus ret;
     if(RESET != (TIMER_INTF(timer_periph) & flag)){
-        return SET;
+        ret = SET;
     }else{
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_usart.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_usart.c
     \brief   USART driver
     
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
@@ -757,11 +757,14 @@ void usart_hardware_flow_coherence_config(uint32_t usart_periph, uint32_t hcm)
 */
 FlagStatus usart_flag_get(uint32_t usart_periph, usart_flag_enum flag)
 {
+    FlagStatus ret;
+
     if(RESET != (USART_REG_VAL(usart_periph, flag) & BIT(USART_BIT_POS(flag)))) {
-        return SET;
+        ret = SET;
     } else {
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!
@@ -858,16 +861,18 @@ void usart_interrupt_disable(uint32_t usart_periph, usart_interrupt_enum interru
 FlagStatus usart_interrupt_flag_get(uint32_t usart_periph, usart_interrupt_flag_enum int_flag)
 {
     uint32_t intenable = 0U, flagstatus = 0U;
+    FlagStatus ret;
     /* get the interrupt enable bit status */
     intenable = (USART_REG_VAL(usart_periph, int_flag) & BIT(USART_BIT_POS(int_flag)));
     /* get the corresponding flag bit status */
     flagstatus = (USART_REG_VAL2(usart_periph, int_flag) & BIT(USART_BIT_POS2(int_flag)));
 
     if((0U != flagstatus) && (0U != intenable)) {
-        return SET;
+        ret = SET;
     } else {
-        return RESET;
+        ret = RESET;
     }
+    return ret;
 }
 
 /*!

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_wwdgt.c

@@ -2,11 +2,11 @@
     \file    gd32c10x_wwdgt.c
     \brief   WWDGT driver
 
-    \version 2025-08-07, V1.6.0, firmware for GD32C10x
+    \version 2026-01-30, V1.7.0, firmware for GD32C10x
 */
 
 /*
-    Copyright (c) 2025, GigaDevice Semiconductor Inc.
+    Copyright (c) 2026, GigaDevice Semiconductor Inc.
 
     Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:
@@ -95,11 +95,13 @@ void wwdgt_config(uint16_t counter, uint16_t window, uint32_t prescaler)
 */
 FlagStatus wwdgt_flag_get(void)
 {
+    FlagStatus flag_status = RESET;
+
     if(RESET != (WWDGT_STAT & WWDGT_STAT_EWIF)) {
-        return SET;
+        flag_status = SET;
     }
 
-    return RESET;
+    return flag_status;
 }
 
 /*!

Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h

@@ -1,6 +1,6 @@
 /*
  * CSOLUTION generated file: DO NOT EDIT!
- * Generated by: csolution version 2.12.0+p41-gfeadfc5e
+ * Generated by: csolution version 2.13.0+p3-g40b6564e
  *
  * Project: 'Robot_V2.Debug+GD32C103CB' 
  * Target:  'Debug+GD32C103CB' 

Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h

@@ -1,6 +1,6 @@
 /*
  * CSOLUTION generated file: DO NOT EDIT!
- * Generated by: csolution version 2.12.0+p41-gfeadfc5e
+ * Generated by: csolution version 2.13.0+p3-g40b6564e
  *
  * Project: 'Robot_V2.Release+GD32C103CB' 
  * Target:  'Release+GD32C103CB' 
@@ -15,41 +15,41 @@
  */
 #define CMSIS_device_header "gd32c10x.h"
 
-/* GigaDevice::Device:GD32C10x_StdPeripherals:ADC@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:ADC@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_ADC
-/* GigaDevice::Device:GD32C10x_StdPeripherals:BKP@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:BKP@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_BKP
-/* GigaDevice::Device:GD32C10x_StdPeripherals:CAN@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:CAN@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_CAN
-/* GigaDevice::Device:GD32C10x_StdPeripherals:CRC@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:CRC@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_CRC
-/* GigaDevice::Device:GD32C10x_StdPeripherals:DBG@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:DBG@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_DBG
-/* GigaDevice::Device:GD32C10x_StdPeripherals:DMA@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:DMA@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_DMA
-/* GigaDevice::Device:GD32C10x_StdPeripherals:EXTI@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:EXTI@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_EXTI
-/* GigaDevice::Device:GD32C10x_StdPeripherals:FMC@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:FMC@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_FMC
-/* GigaDevice::Device:GD32C10x_StdPeripherals:FWDGT@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:FWDGT@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_FWDGT
-/* GigaDevice::Device:GD32C10x_StdPeripherals:GPIO@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:GPIO@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_GPIO
-/* GigaDevice::Device:GD32C10x_StdPeripherals:MISC@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:MISC@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_MISC
-/* GigaDevice::Device:GD32C10x_StdPeripherals:PMU@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:PMU@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_PMU
-/* GigaDevice::Device:GD32C10x_StdPeripherals:RCU@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:RCU@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_RCU
-/* GigaDevice::Device:GD32C10x_StdPeripherals:RTC@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:RTC@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_RTC
-/* GigaDevice::Device:GD32C10x_StdPeripherals:SPI_I2S@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:SPI_I2S@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_SPI
-/* GigaDevice::Device:GD32C10x_StdPeripherals:TIMER@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:TIMER@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_TIMER
-/* GigaDevice::Device:GD32C10x_StdPeripherals:USART@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:USART@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_USART
-/* GigaDevice::Device:GD32C10x_StdPeripherals:WWDGT@1.0.0 */
+/* GigaDevice::Device:GD32C10x_StdPeripherals:WWDGT@1.3.0 */
 #define RTE_DEVICE_STDPERIPHERALS_WWDGT
 
 

Robot_V2/Robot_V2.cproject.yml

@@ -59,7 +59,7 @@ project:
     - component: Device:GD32C10x_StdPeripherals:BKP
     - component: Device:GD32C10x_StdPeripherals:CRC
     - component: Device:GD32C10x_StdPeripherals:RTC
-    - component: CMSIS:DSP
+    - component: CMSIS:DSP&Source
   output:
     type:
       - elf
@@ -69,6 +69,7 @@ project:
   packs:
     - pack: ARM::CMSIS
     - pack: GigaDevice::GD32C10x_DFP
+    - pack: ARM::CMSIS-DSP
   misc:
     - for-compiler: AC6
       C-CPP:

Robot_V2/device/ADC.cpp

@@ -5,9 +5,9 @@
 #include "config.h"
 #include "math.h"
 #include "systick.h"
+#include <cstdint>
 #include <cstring>
 #include <gd32c10x_dma.h>
-#include <cstdint>
 
 constexpr float ADC_SCALE = 3.3f / 4096.0f;
 constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
@@ -15,7 +15,7 @@ constexpr float VCC_FACTOR = 11.0f;
 constexpr float CURRENT_FACTOR =
 	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
 extern CAN_transmiter CAN0_transmiter;
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 sampler sampler;		// 采样器初始化
 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 /*!
@@ -80,7 +80,7 @@ void adc_config(void) {
 	adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6);
 	/* 配置ADC规则通道 */
 	adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16,
-							   ADC_SAMPLETIME_239POINT5);
+							   ADC_SAMPLETIME_41POINT5);
 	adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5);
 	adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5);
 	adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5);
@@ -167,20 +167,25 @@ float sampler::get_drv_temp() {
  * 通过ADC采样计算各相电流值
  */
 float sampler::get_current(char type) {
+
+	u8 idx = current_sample_index;
 	switch (type) {
 	case 'U': {
 		// return currU_samples[current_sample_index];
-		tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currU_samples[idx];
 		break;
 	}
 	case 'V': {
 		// return currV_samples[current_sample_index];
-		tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currV_samples[idx];
 		break;
 	}
 	case 'W': {
 		// return currW_samples[current_sample_index];
-		tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currW_samples[idx];
 		break;
 	}
 	default: {
@@ -240,7 +245,7 @@ void sampler::update_drv_temp() {
  */
 void sampler::update_current() {
 	u8 idx = current_sample_index;
-	extern FOC_Controlroller FOC;
+	extern FOC_Controller FOC;
 	// 优化计算
 	const float scale = ADC_SCALE * CURRENT_FACTOR;
 	const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
@@ -254,8 +259,6 @@ void sampler::update_current() {
 	if (current_sample_index >= ADC_FILTER_SIZE - 1) {
 		current_sample_index = 0;
 	}
-	// 卡尔曼滤波更新
-	float voltage = FOC.voltage_power_supply;
 }
 
 /**

Robot_V2/device/Can.h

@@ -6,21 +6,23 @@
 extern "C" {
 #endif
 
+#include <cstdint>
 #include "gd32c10x_can.h"
+#include "config.h"
 
-// 定义一个联合体类型用于float和byte数组之间的转换
+// 定义一个联合体类型用于 float 和 byte 数组之间的转换
 typedef union {
 	float f;
 	u8 b[4];
 } float_union_t;
 
-// 添加辅助函数用于将u16类型数据打包到u8数组中
+// 添加辅助函数用于将 u16 类型数据打包到 u8 数组中
 static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
 	buffer[offset] = (u8)(value & 0xFF);
 	buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
 }
 
-// 添加辅助函数用于将float类型数据打包到u8数组中
+// 添加辅助函数用于将 float 类型数据打包到 u8 数组中
 static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
 									   float_union_t &float_union) {
 	buffer[offset] = float_union.b[0];
@@ -29,19 +31,19 @@ static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
 	buffer[offset + 3] = float_union.b[3];
 }
 
-// 添加辅助函数用于从u8数组中解包short类型数据
+// 添加辅助函数用于从 u8 数组中解包 short 类型数据
 static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
 	short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
 	return value;
 }
 
-// 添加辅助函数用于从u8数组中解包u16类型数据
+// 添加辅助函数用于从 u8 数组中解包 u16 类型数据
 static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
 	u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
 	return value;
 }
 
-// 添加辅助函数用于从u8数组中解包float类型数据
+// 添加辅助函数用于从 u8 数组中解包 float 类型数据
 static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
 	float_union_t float_union;
 	float_union.b[0] = buffer[offset];
@@ -51,71 +53,113 @@ static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
 	return float_union;
 }
 
-u8 can_gpio_init(void);				 // CAN引脚初始化
-void can_config(void);				 // CAN初始化
-void canfd_config(void);			 // CANFD初始化
-void can_dma_init(void);			 // CAN DMA初始化
-void CAN0_RX0_IRQHandler(void);		 // CAN接收中断处理函数
-void DMA0_Channel4_IRQHandler(void); // DMA通道4中断处理函数(CAN接收)
-void DMA0_Channel5_IRQHandler(void); // DMA通道5中断处理函数(CAN发送)
+u8 can_gpio_init(void);				 // CAN 引脚初始化
+void can_config(void);				 // CAN 初始化
+void canfd_config(void);			 // CANFD 初始化
+void can_dma_init(void);			 // CAN DMA 初始化
+void CAN0_RX0_IRQHandler(void);		 // CAN 接收中断处理函数
+void DMA0_Channel4_IRQHandler(void); // DMA 通道 4 中断处理函数 (CAN 接收)
+void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发送)
 void jumpToBOOT();
 
 void can_loopback_init(void);
 void can_loopback_test(void);
 
 /*!
-	\brief CAN通信类
-	\note  封装CAN发送和接收功能
+	\brief CAN 通信类
+	\note  封装 CAN 发送和接收功能，支持多电机节点 ID 隔离
 */
 class CAN_transmiter {
 
   private:
+	static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN 接收缓冲区大小
+	static const u32 CAN_SILENT_DURATION_MS = 10000; // CAN 静默时长 10 秒
+	
 	bool is_sleep = false; // 是否休眠
+	bool is_silent = false; // 是否静默（收到 base_id=0x0000 后进入静默）
+	u32 silent_start_time = 0; // 静默开始时间（ms）
+
+	u16 node_id; // 本节点 ID(0-254)，使用 CAN ID 高 8 位存储 (在 init() 中初始化为 DEFAULT_NODE_ID)
+	u8 Target_Control_Mode = 0; // 目标控制模式
+	u8 controller_Mode = 0;		// 当前模式
+	short mcu_temp = 0;			// MCU 温度
+	short motor_temp = 0;		// 电机温度
+	u16 Power_Voltage = 0;		// 电源电压
+
+	u16 limit_voltage = 0;					  // 限制电压
+
+	float_union_t rt_angle_union = {0};	   // 实时角度
+	float_union_t rt_velocity_union = {0}; // 实时速度
+	float_union_t rt_current_union = {0};  // 实时电流
 
-	u8 controller_Mode = 0;					   // 电机模式
-	float_union_t limit_velocity_union = {0};  // 限制速度
-	u16 limit_voltage = 0;					   // 限制电压
-	short mcu_temp = 0;						   // MCU温度
-	short motor_temp = 0;					   // 电机温度
-	u16 Power_Voltage = 0;					   // 电源电压
-	float_union_t rt_angle_union = {0};		   // 实时角度
-	float_union_t rt_velocity_union = {0};	   // 实时速度
-	bool MotorStatus = 0;					   // 电机状态码
 	float_union_t target_angle_union = {0};	   // 目标角度
-	u8 Target_Control_Mode = 0;				   // 目标控制模式
-	u8 EEPROMStatus = 0;					   // EEPROM状态码
 	float_union_t target_velocity_union = {0}; // 目标速度
-	float_union_t target_current_union = {0};
-	float_union_t u_current_union = {0}; // U相电流
-	float_union_t w_current_union = {0}; // W相电流
-	float_union_t v_current_union = {0}; // V相电流
+	float_union_t target_current_union = {0};  // 目标电流
+
+	float_union_t u_current_union = {0}; // U 相电流
+	float_union_t w_current_union = {0}; // W 相电流
+	float_union_t v_current_union = {0}; // V 相电流
+
+	bool MotorStatus = 0; // 电机状态码
+	u8 EEPROMStatus = 0;  // EEPROM 状态码
 
 	can_trasnmit_message_struct transmit_message; // 发送消息结构体
-	u8 transmit_mailbox = CAN_NOMAILBOX;		  // 初始化为无mailbox状态
-	u8 transmit_data[64];						  // CANFD最大64字节
+	u8 transmit_mailbox = CAN_NOMAILBOX;		  // 初始化为无 mailbox 状态
+	u8 transmit_data[64];						  // CANFD 最大 64 字节
+
+	// CAN 接收消息队列
+	can_receive_message_struct rx_message_buffer[CAN_RX_BUFFER_SIZE];
+	volatile uint8_t rx_buffer_head = 0;
+	volatile uint8_t rx_buffer_tail = 0;
 	
 	void update();		// 更新数据
-	u8 CAN_Receive();	// CAN接收
-	u8 CANFD_Receive(); // CANFD接收
+	u8 CAN_Receive();	// CAN 接收
+	u8 CANFD_Receive(); // CANFD 接收
+	bool is_rx_buffer_full();
+	bool is_rx_buffer_empty();
+	bool dequeue_rx_message(can_receive_message_struct& message);
+	void check_silent_mode(); // 检查静默模式是否结束
+	
+  public:
+	// 节点 ID 相关辅助函数 (使用高 16 位存储节点 ID)
+	u32 build_can_id(u32 base_id);			// 根据节点 ID 构建 CAN ID，格式：[节点 ID(16 位)][基础 ID(16 位)]
+	bool is_message_for_me(u32 received_id); // 判断消息是否发给本节点
 
   public:
 	// 添加构造函数声明
 	u8 init(void);
 
 	can_receive_message_struct receive_message; // 接收消息结构体
-	FlagStatus can0_receive_flag;				// CAN0接收标志
-	FlagStatus can0_send_flag;					// CAN0发送标志
+	FlagStatus can0_send_flag;					// CAN0 发送标志
 	float_union_t testfloat_union = {0};		// 测试用
+	u16 test_u16 = 0;
 
 	bool MagStatus = 0;								 // 磁状态码
 	bool ThermalStatus = 0;							 // 热能状态码
 	void communication_check();						 // 通信检查
-	u8 CAN_Sleep();									 // CAN进入睡眠
-	u8 CAN_Wakeup();								 // CAN进入睡眠
-	u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data);	 // CAN异步发送(使用DMA)
-	u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA)
-	u8 CAN_all_Send();								 // CAN发送
-	u8 CANFD_all_Send();							 // CANFD发送
+	u8 CAN_Sleep();									 // CAN 进入睡眠
+	u8 CAN_Wakeup();								 // CAN 唤醒
+	u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data);	 // CAN 异步发送 (使用 DMA)
+	u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD 异步发送 (使用 DMA)
+	u8 CAN_all_Send();								 // CAN 发送
+	u8 CANFD_all_Send();							 // CANFD 发送
+	void enter_silent_mode(); // 进入静默模式（停发 10 秒）
+	bool is_in_silent_mode(); // 检查是否在静默模式中
+	
+	// 节点 ID 管理接口
+	u16 get_node_id();						// 获取当前节点 ID
+	void set_node_id(u16 new_id);			// 设置节点 ID
+	u8 load_node_id_from_flash();			// 从 Flash 加载节点 ID
+	u8 save_node_id_to_flash(u16 new_id);	// 保存节点 ID 到 Flash
+	
+	// 中断处理函数需要访问的公共接口
+	void enqueue_rx_message_from_isr(const can_receive_message_struct& message) {
+		if (!is_rx_buffer_full()) {
+			rx_message_buffer[rx_buffer_head] = message;
+			rx_buffer_head = (rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE;
+		}
+		// 如果缓冲区满，丢弃新消息
+	}
 };
 
 #ifdef __cplusplus

Robot_V2/device/Timer.cpp

@@ -9,33 +9,64 @@
 #include "gd32c10x_timer.h"
 #include "initial.h"
 
-volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
+volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1
 extern u16 adc_value[7];
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 extern TaskScheduler scheduler;
 extern CAN_transmiter CAN0_transmiter;
 // 添加系统运行时间计数器
 extern u32 cnt_test;
 
+// 任务调度查找表 - 每 100us 执行的任务 (1ms 周期内)
+static const u32 task_schedule_100us[10] = {
+    (1u << TASK_MCU_TEMP_UPDATE),      // 0: MCU 温度更新
+    (1u << TASK_DRV_TEMP_UPDATE),      // 1: 驱动温度更新
+    (1u << TASK_VBUS_UPDATE),          // 2: 电压更新
+    0,                                  // 3: 空闲
+    0,                                  // 4: 空闲
+    0,                                  // 5: 空闲
+    0,                                  // 6: 空闲
+    0,                                  // 7: 空闲
+    0,                                  // 8: 空闲
+    (1u << TASK_CAN_TRANSMIT)          // 9: CAN 发送
+};
+
+// 任务调度查找表 - 每 1ms 执行的任务 (10ms 周期内)
+static const u32 task_schedule_1ms[10] = {
+    (1u << TASK_THERMAL_CONTROL),      // 0: 热量控制
+    (1u << TASK_MAGNETIC_CHECK),       // 1: 磁感应强度检测
+    0, 0, 0, 0, 0, 0, 0, 0             // 空闲
+};
+
+// 任务调度查找表 - 每 10ms 执行的任务 (100ms 周期内)
+static const u32 task_schedule_10ms[10] = {
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0       // 当前无任务，预留扩展
+};
+
+// 任务调度查找表 - 每 100ms 执行的任务 (1s 周期内)
+static const u32 task_schedule_100ms[10] = {
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0       // 当前无任务，预留扩展
+};
+
 /**
  * @brief 设置引脚中断优先级
  * @return void
  *
- *设置引脚中断优先级
+ * 设置引脚中断优先级
  */
 void nvic_config(void) {
-	/* 设置NVIC优先级分组，1位抢占优先级，3位响应优先级 */
+	/* 设置 NVIC 优先级分组，1 位抢占优先级，3 位响应优先级 */
 	nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
-	/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
+	/* 使能 TIMER2 中断，设置抢占优先级为 1，子优先级为 1 */
 	nvic_irq_enable(TIMER2_IRQn, 0, 1);
 	nvic_irq_enable(TIMER1_IRQn, 0, 2);
 }
 
 /**
- * @brief 配置定时器2-任务管理
+ * @brief 配置定时器 2-任务管理
  * @return void
  *
- *配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务
+ * 配置定时器 2，用于定时执行 FOC 控制、采样、CAN 发送等任务
  */
 void TIMER2_config(void) {
 	/* ----------------------------------------------------------------------------
@@ -46,21 +77,21 @@ void TIMER2_config(void) {
   */
 	timer_parameter_struct timer_struct;
 
-	/* 使能TIMER2时钟 */
+	/* 使能 TIMER2 时钟 */
 	rcu_periph_clock_enable(RCU_TIMER2);
 
-	/* 复位TIMER2外设到默认状态 */
+	/* 复位 TIMER2 外设到默认状态 */
 	timer_deinit(TIMER2);
-	/* 初始化TIMER初始化参数结构体 */
+	/* 初始化 TIMER 初始化参数结构体 */
 	timer_struct_para_init(&timer_struct);
-	/* TIMER2配置参数设置 */
+	/* TIMER2 配置参数设置 */
 	timer_struct.prescaler =
-		TIMER2_PRESCALER; /* 预分频值：120分频(从0开始计数) */
+		TIMER2_PRESCALER; /* 预分频值：120 分频 (从 0 开始计数) */
 	timer_struct.alignedmode = TIMER_COUNTER_EDGE;	  /* 边沿对齐模式 */
 	timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
 	timer_struct.period =
-		TIMER2_PERIOD; /* 自动重装载值：100次计数(从0开始计数) */
-	timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
+		TIMER2_PERIOD; /* 自动重装载值：100 次计数 (从 0 开始计数) */
+	timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1 分频 */
 	timer_init(TIMER2, &timer_struct);
 
 	/* 清除更新中断标志位 */
@@ -68,108 +99,85 @@ void TIMER2_config(void) {
 	/* 使能更新中断 */
 	timer_interrupt_enable(TIMER2, TIMER_INT_UP);
 
-	/* 使能TIMER2 */
+	/* 使能 TIMER2 */
 	timer_enable(TIMER2);
 }
 
 /**
- * @brief   配置TIMER2-任务管理中断
+ * @brief   配置 TIMER2-任务管理中断
  * @return void
  */
 void User_timer_init(void) {
-	/* 配置TIMER2 */
+	/* 配置 TIMER2 */
 	TIMER2_config();
-	/* 配置TIMER2中断 */
+	/* 配置 TIMER2 中断 */
 	nvic_config();
 }
 
 /**
- * @brief   TIMER2-任务管理中断处理函数
+ * @brief   TIMER2-任务管理中断处理函数 (优化版)
  * @return void
+ * @note    使用查找表替代多层 switch-case，减少代码复杂度和执行时间
+ *          中断周期：50us
+ *          - 每 50us: 电机控制任务 (角度、速度更新)
+ *          - 每 100us: 传感器采样、CAN 通信
+ *          - 每 1ms: 热量控制、磁检
+ *          - 每 10ms/100ms/1s: 预留扩展任务
  */
 void TIMER2_IRQHandler(void) {
-	// 检查更新中断标志并处理
-	if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
-		// 清除更新中断标志
-		timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
+    // 快速路径：检查并清除中断标志
+    if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) == RESET) {
+        return;
+    }
+    timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
 
-		// 每50us执行一次的操作 - 电机控制
-		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
-		scheduler.taskFlags |=
-			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
+    // 固定任务：每 50us 执行电机控制
+    scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) | 
+                           (1u << TASK_VELOCITY_UPDATE) | 
+                           (1u << TASK_MOTOR_CONTROL);
 
-		// 使用标志位控制，每100us执行一次其他任务
-		static bool isEvenCall = false;
-		isEvenCall = !isEvenCall;
+    // 每 100us 执行一次的任务（使用位运算替代布尔变量）
+    static u32 call_counter = 0;
+    call_counter++;
     
-		if (isEvenCall) {
-			// 每100us执行一次的操作 - 保持原有频率
+    if ((call_counter & 1u) == 0u) {  // 偶数次调用（每 100us）
+        // 使用查找表设置 1ms 周期内的任务
+        scheduler.taskFlags |= task_schedule_100us[total_us100_counter];
         
-			switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
-			case 0: {
-				scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
-				break;
-			}
-			case 1: {
-				scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
-				break;
-			}
-			case 2: {
-				scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
-				break;
-			}
-			case 3: {
-				break;
-			}
-			case 9: {
-				scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
-				break;
-			}
-			}
+        total_us100_counter++;
+        if (total_us100_counter >= 10u) {
+            total_us100_counter = 0;
             
-			total_us100_counter++;
-			if (total_us100_counter > 9) {
-				total_us100_counter = 0;
-				// 可以添加每1ms执行的代码，无负载均衡
-				static u8 ms1_counter = 0;
+            // 每 1ms 执行的任务
+            static u32 ms1_counter = 0;
+            ms1_counter++;
+            if (ms1_counter >= 10u) {
+                ms1_counter = 0;
                 
-				switch (ms1_counter) { // 每10ms执行的代码，负载均衡
-				case 0: {
-					scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
-					break;
-				}
-				case 1: {
-					scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
-					break;
-				}
-				case 2: {
-					break;
-				}
-				}
-				ms1_counter++;
-				if (ms1_counter > 9) {
-					ms1_counter = 0;
-					// 可以添加每10ms执行的代码，无负载均衡
-					static u8 ms10_counter = 0;
-					ms10_counter++;
-					if (ms10_counter > 9) {
-						ms10_counter = 0;
-						// 可以添加每100ms执行的代码
-						static u8 sec_counter = 0;
-						sec_counter++;
-						if (sec_counter > 9) {
-							sec_counter = 0;
-							// 可以添加每1s执行的代码
-						}
-					}
-				}
-			}
-		}
-	}
+                // 每 10ms 执行的任务
+                static u32 ms10_counter = 0;
+                ms10_counter++;
+                if (ms10_counter >= 10u) {
+                    ms10_counter = 0;
+                    
+                    // 每 100ms 执行的任务
+                    static u32 ms100_counter = 0;
+                    ms100_counter++;
+                    if (ms100_counter >= 10u) {
+                        ms100_counter = 0;
+                        // 每 1s 任务预留位置
+                    }
+                    scheduler.taskFlags |= task_schedule_100ms[ms100_counter];
+                }
+                scheduler.taskFlags |= task_schedule_10ms[ms10_counter];
+            }
+            scheduler.taskFlags |= task_schedule_1ms[ms1_counter];
+        }
+    }
 }
 
 /**
- * @brief  TIMER1-PWM GPIO配置函数
+ * @brief  TIMER1-PWM GPIO 配置函数
  */
 void timer1_gpio_config(void) {
 	// 时钟使能
@@ -178,7 +186,7 @@ void timer1_gpio_config(void) {
 	rcu_periph_clock_enable(RCU_AF);
 	rcu_periph_clock_enable(RCU_TIMER1);
 
-	// TIMER1 GPIO初始化
+	// TIMER1 GPIO 初始化
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
@@ -188,7 +196,7 @@ void timer1_gpio_config(void) {
 	gpio_bit_set(GPIOB, GPIO_PIN_0);
 	gpio_bit_set(GPIOB, GPIO_PIN_12);
 
-	/* 配置TIMER1完全重映射*/
+	/* 配置 TIMER1 完全重映射*/
 	gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE);
 }
 
@@ -199,13 +207,13 @@ void timer1_pwm_config(void) {
 	timer_parameter_struct timer_initpara;
 	timer_oc_parameter_struct timer_ocinitpara;
 
-	// 使能TIMER1时钟
+	// 使能 TIMER1 时钟
 	rcu_periph_clock_enable(RCU_TIMER1);
 
-	// 复位TIMER1
+	// 复位 TIMER1
 	timer_deinit(TIMER1);
 
-	// 配置TIMER1基本参数
+	// 配置 TIMER1 基本参数
 	timer_struct_para_init(&timer_initpara);
 	timer_initpara.prescaler = TIMER1_PRESCALER;		// 预分频器
 	timer_initpara.counterdirection = TIMER_COUNTER_UP; // 向上计数
@@ -218,23 +226,23 @@ void timer1_pwm_config(void) {
 	timer_ocinitpara.outputstate = TIMER_CCX_ENABLE;
 	timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
 
-	// 配置三个通道为PWM模式
+	// 配置三个通道为 PWM 模式
 	timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocinitpara);
 	timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocinitpara);
 	timer_channel_output_config(TIMER1, TIMER_CH_2, &timer_ocinitpara);
 
-	// 配置第四个通道(CH3)用于在PWM周期x%位置触发中断
+	// 配置第四个通道 (CH3) 用于在 PWM 周期 x% 位置触发中断
 	timer_ocinitpara.outputstate =
-		TIMER_CCX_DISABLE; // 不输出PWM波形，仅用于触发中断
+		TIMER_CCX_DISABLE; // 不输出 PWM 波形，仅用于触发中断
 	timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
 	timer_channel_output_config(TIMER1, TIMER_CH_3, &timer_ocinitpara);
 
-	// 设置PWM模式和占空比
+	// 设置 PWM 模式和占空比
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM0);
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_1, TIMER_OC_MODE_PWM0);
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_2, TIMER_OC_MODE_PWM0);
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_3,
-									 TIMER_OC_MODE_PWM0); // CH3也设为PWM模式
+									 TIMER_OC_MODE_PWM0); // CH3 也设为 PWM 模式
 
 	timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
 											TIMER1_DEFAULT_PULSE);
@@ -242,7 +250,7 @@ void timer1_pwm_config(void) {
 											TIMER1_DEFAULT_PULSE);
 	timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
 											TIMER1_DEFAULT_PULSE);
-	// 设置CH3比较值为PWM周期x%位置
+	// 设置 CH3 比较值为 PWM 周期 x% 位置
 	timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_3,
 											TIMER1_CH3_COMPARE_VALUE);
 
@@ -250,26 +258,26 @@ void timer1_pwm_config(void) {
 	timer_primary_output_config(TIMER1, ENABLE);
 	timer_auto_reload_shadow_enable(TIMER1);
 
-	// 使能CH3比较中断
+	// 使能 CH3 比较中断
 	timer_interrupt_enable(TIMER1, TIMER_INT_CH3);
 
-	// 启动TIMER1
+	// 启动 TIMER1
 	timer_enable(TIMER1);
 }
 
-// 添加TIMER1-PWM中断处理函数
+// 添加 TIMER1-PWM 中断处理函数
 /**
- * @brief   TIMER1中断处理函数
+ * @brief   TIMER1 中断处理函数
  * @return void
  */
 void TIMER1_IRQHandler(void) {
-	// 检查CH3比较中断标志并处理
+	// 检查 CH3 比较中断标志并处理
 	if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) {
-		// 清除CH3比较中断标志
+		// 清除 CH3 比较中断标志
 		timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3);
 
 		scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE);
-		// 例如触发ADC采样或其他操作
+		// 例如触发 ADC 采样或其他操作
 	}
 }
 

Robot_V2/device/ang_sensor.cpp

@@ -9,14 +9,20 @@
 #include <cstdio>
 #include <cstring>
 #include <stdint.h>
-#define ANG_SENS_DEBUG 1
+
+// ANG_SENS 调试宏定义
+#if ANG_SENS_DEBUG
+#define ANG_SENS_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define ANG_SENS_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
 
 #define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
 #define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
 #define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
 #define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
 
-// DMA传输完成标志
+// DMA 传输完成标志
 volatile bool spi_dma_tx_complete = false;
 volatile bool spi_dma_rx_complete = false;
 
@@ -24,7 +30,7 @@ angle_sensor KTH7823; // 角度传感器初始化
 extern FlashEEPROM flashEEP;
 extern EncoderAngleKalman Angle_kalman;
 extern EncoderVelocityKalman Vel_kalman;
-extern CAN_transmiter CAN0_transmiter; // CAN初始化
+extern CAN_transmiter CAN0_transmiter; // CAN 初始化
 
 /**
  * @brief SPI DMA initialization
@@ -32,10 +38,10 @@ extern CAN_transmiter CAN0_transmiter; // CAN初始化
 void spi_dma_init(void) {
 	dma_parameter_struct dma_init_struct;
 
-	// 使能DMA时钟
+	// 使能 DMA 时钟
 	rcu_periph_clock_enable(RCU_DMA0);
 
-	// 初始化发送DMA通道
+	// 初始化发送 DMA 通道
 	dma_deinit(DMA0, DMA_CH2);
 	dma_struct_para_init(&dma_init_struct);
 	dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
@@ -49,7 +55,7 @@ void spi_dma_init(void) {
 	dma_init_struct.priority = DMA_PRIORITY_HIGH;
 	dma_init(DMA0, DMA_CH2, &dma_init_struct);
 
-	// 初始化接收DMA通道
+	// 初始化接收 DMA 通道
 	dma_deinit(DMA0, DMA_CH1);
 	dma_struct_para_init(&dma_init_struct);
 	dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
@@ -63,15 +69,15 @@ void spi_dma_init(void) {
 	dma_init_struct.priority = DMA_PRIORITY_HIGH;
 	dma_init(DMA0, DMA_CH1, &dma_init_struct);
 
-	// 使能SPI的DMA发送和接收功能
+	// 使能 SPI 的 DMA 发送和接收功能
 	spi_dma_enable(SPI0, SPI_DMA_TRANSMIT);
 	spi_dma_enable(SPI0, SPI_DMA_RECEIVE);
 
-	// 配置DMA中断
+	// 配置 DMA 中断
 	nvic_irq_enable(DMA0_Channel2_IRQn, 0, 1);
 	nvic_irq_enable(DMA0_Channel1_IRQn, 0, 1);
 
-	// 使能DMA传输完成中断
+	// 使能 DMA 传输完成中断
 	dma_interrupt_enable(DMA0, DMA_CH2, DMA_INT_FTF);
 	dma_interrupt_enable(DMA0, DMA_CH1, DMA_INT_FTF);
 }
@@ -90,30 +96,30 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 	spi_dma_tx_complete = false;
 	spi_dma_rx_complete = false;
 
-	// 配置发送DMA
+	// 配置发送 DMA
 	dma_channel_disable(DMA0, DMA_CH2);
 	dma_memory_address_config(DMA0, DMA_CH2, (uint32_t)&tx_data);
 	dma_transfer_number_config(DMA0, DMA_CH2, 1);
 	dma_channel_enable(DMA0, DMA_CH2);
 
-	// 配置接收DMA
+	// 配置接收 DMA
 	dma_channel_disable(DMA0, DMA_CH1);
 	dma_memory_address_config(DMA0, DMA_CH1, (uint32_t)rx_data);
 	dma_transfer_number_config(DMA0, DMA_CH1, 1);
 	dma_channel_enable(DMA0, DMA_CH1);
 
-	// 等待传输完成，超时时间为100个循环
+	// 等待传输完成，超时时间为 100 个循环
 	uint32_t timeout = 100;
 	while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
-		// 空循环等待DMA传输完成
+		// 空循环等待 DMA 传输完成
 	}
 
 	// 检查是否超时
 	if (timeout == 0) {
-		// 禁用DMA通道
+		// 禁用 DMA 通道
 		dma_channel_disable(DMA0, DMA_CH2);
 		dma_channel_disable(DMA0, DMA_CH1);
-		printf("SPI DMA transfer timeout\r\n");
+		ANG_SENS_DEBUG_PRINT("SPI DMA transfer timeout\r\n");
 		return -1;
 	}
 
@@ -121,7 +127,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 }
 
 /**
- * @brief DMA0_Channel2中断处理函数
+ * @brief DMA0_Channel2 中断处理函数
  * @param 无
  */
 extern "C" void DMA0_Channel2_IRQHandler(void) {
@@ -132,7 +138,7 @@ extern "C" void DMA0_Channel2_IRQHandler(void) {
 }
 
 /**
- * @brief DMA0_Channel1中断处理函数
+ * @brief DMA0_Channel1 中断处理函数
  * @param 无
  */
 extern "C" void DMA0_Channel1_IRQHandler(void) {
@@ -174,7 +180,7 @@ void KTH7823_spi_init(void) {
 	spi_parameter_struct spi_init_str;
 	spi_struct_para_init(&spi_init_str);
 
-	/* SPI参数：16bit, 主机, 全双工, CPOL=1 CPHA=1, MSB first */
+	/* SPI 参数：16bit, 主机，全双工，CPOL=1 CPHA=1, MSB first */
 	spi_init_str.device_mode = SPI_MASTER;
 	spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
 	spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
@@ -194,18 +200,19 @@ void KTH7823_spi_init(void) {
  * @return 0:初始化成功 1:初始化失败
  */
 u8 angle_sensor::init(void) {
-	KTH7823_gpio_init(); // GPIO初始化
-	KTH7823_spi_init();	 // SPI初始化
-	spi_dma_init();		 // DMA初始化
+	KTH7823_gpio_init(); // GPIO 初始化
+	KTH7823_spi_init();	 // SPI 初始化
+	spi_dma_init();		 // DMA 初始化
 	return 0;
 }
+
 /**
  * @ 角度传感器类 设置当前角度
  * @param angle 输出级角度值
- * @return 0成功/1失败
+ * @return 0 成功/1 失败
  */
 u8 angle_sensor::angle_set(float angle) {
-	m_cumulativeAngle = angle * REDUCTION_RATIO; // 从Flash读取上次保存的角度
+	m_cumulativeAngle = angle * REDUCTION_RATIO;
 	m_firstUpdate = true;
 	return 0;
 }
@@ -229,14 +236,14 @@ inline float angle_sensor::read_sensorAngle(void) {
 		return angle_deg;
 	}
 
-	// 仅在调试模式下输出错误，避免发布版本中的I/O开销
+	// 仅在调试模式下输出错误，避免发布版本中的 I/O 开销
 	if (result != 0) {
-		// 使用更高效的串口输出替代printf（如果可用）
-		printf("Error reading angle from sensor.\n");
+		ANG_SENS_DEBUG_PRINT("Error reading angle from sensor.\n");
 	}
 
 	return (float)FAULT_RECEIVE;
 }
+
 /**
  * @brief 更新传感器角度（输出级）
  * @return 累积角度值
@@ -253,24 +260,23 @@ float angle_sensor::update(void) {
 	// 计算角度变化
 	float delta = current_angle_deg - m_lastAngle;
 
-	// 处理角度绕回情况（0度和360度之间的跳变）
-	// 如果变化超过300度，说明发生了绕回
+	// 处理角度绕回情况（0 度和 360 度之间的跳变）
+	// 如果变化超过 300 度，说明发生了绕回
 	if (delta > ANGLE_WRAP_THRESHOLD) {
-		// 从接近360度绕回到接近0度
+		// 从接近 360 度绕回到接近 0 度
 		delta -= ANGLE_WRAP_VALUE;
 	} else if (delta < -ANGLE_WRAP_THRESHOLD) {
-		// 从接近0度绕到接近360度
+		// 从接近 0 度绕到接近 360 度
 		delta += ANGLE_WRAP_VALUE;
 	}
 
 	m_cumulativeAngle += delta;
 	m_lastAngle = current_angle_deg;
-
 	return m_cumulativeAngle;
 }
 
 /**
- * @brief 获取累积角度值(输出级)
+ * @brief 获取累积角度值 (输出级)
  * @return 累积角度值
  */
 float angle_sensor::getAngle(void) const {
@@ -278,15 +284,15 @@ float angle_sensor::getAngle(void) const {
 }
 
 /**
- * @brief 获取累积角度值(原始级)
+ * @brief 获取累积角度值 (原始级)
  * @return 累积角度值
  */
 float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
 
 /**
- * @brief 获取角度值(原始级)
- * @return 角度值(原始级)
+ * @brief 获取角度值 (原始级)
+ * @return 角度值 (原始级)
  */
-float angle_sensor::getcurrent_sensorangle(void) const {
+float angle_sensor::getcurrent_sensorangle(void) const{
 	return current_angle_deg;
 }

Robot_V2/device/ang_sensor.h

@@ -14,6 +14,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 
 class angle_sensor {
   public:
+	float calibration_angle = 195.0f;
 	u8 init(void);
 	float read_sensorAngle(void);
 	float update(void);
@@ -22,6 +23,7 @@ class angle_sensor {
 	float getRawAngle(void) const;
 	float getcurrent_sensorangle(void) const;
 	u8 angle_set(float angle);
+	u8 set_calibration(float angle);
 
   protected:
 	float m_cumulativeAngle;

Robot_V2/device/test.cpp

@@ -1,25 +1,33 @@
 #include "adc.h"
+#include "config.h"
 #include <cstdio>
 
+// TEST 调试宏定义
+#if TEST_DEBUG
+#define TEST_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define TEST_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
+
 constexpr int status_print = 0;
 extern sampler sampler;
 void test_1111() {
-	// 初始化CAN回环测试
+	// 初始化 CAN 回环测试
 	// can_loopback_init();
 	// can_config();
 	if (status_print) {
 		/* value print */
-		printf(" the temperature data is %2.2f degrees Celsius\r\n",
+		TEST_DEBUG_PRINT(" the temperature data is %2.2f degrees Celsius\r\n",
 			   sampler.get_mcu_temp());
-		printf(" the drv_temperature data is %2.2f degrees Celsius\r\n",
+		TEST_DEBUG_PRINT(" the drv_temperature data is %2.2f degrees Celsius\r\n",
 			   sampler.get_drv_temp());
-		printf(" the U current data is %2.2f mA\r\n", sampler.get_current('U'));
-		printf(" the V current data is %2.2f mA\r\n", sampler.get_current('V'));
-		printf(" the W current data is %2.2f mA\r\n", sampler.get_current('W'));
-		printf(" the VBUS is %2.2f V\r\n", sampler.get_vbus_voltage());
-		// printf(" the angle is %2.2f\r\n", tle5012b_read());
+		TEST_DEBUG_PRINT(" the U current data is %2.2f mA\r\n", sampler.get_current('U'));
+		TEST_DEBUG_PRINT(" the V current data is %2.2f mA\r\n", sampler.get_current('V'));
+		TEST_DEBUG_PRINT(" the W current data is %2.2f mA\r\n", sampler.get_current('W'));
+		TEST_DEBUG_PRINT(" the VBUS is %2.2f V\r\n", sampler.get_vbus_voltage());
+		// TEST_DEBUG_PRINT(" the angle is %2.2f\r\n", tle5012b_read());
 
-		printf("\n----------------------------------------------------\n");
+		TEST_DEBUG_PRINT("\n----------------------------------------------------\n");
 	}
 	// can_loopback_test();
 }

Robot_V2/main.cpp

@@ -8,30 +8,50 @@
 #include "flash.h"
 #include "health.h"
 #include "initial.h"
-#include "motor.h"
 #include "systick.h"
 
-extern CAN_transmiter CAN0_transmiter;		// CAN初始化
+// MAIN 调试宏定义
+#if MAIN_DEBUG
+#define MAIN_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define MAIN_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
+
+extern CAN_transmiter CAN0_transmiter;		// CAN 初始化
 extern sampler sampler;						// 采样器
 extern angle_sensor KTH7823;				// 角度传感器
 extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
 extern EncoderAngleKalman Angle_kalman;		// 角度滤波
 extern ThermalController thermalController; // 热量控制器
-extern FOC_Controlroller FOC;				// FOC控制
-extern FlashEEPROM flashEEP;				// EEPROM类
+extern FOC_Controller FOC;					// FOC 控制
+extern FlashEEPROM flashEEP;				// EEPROM 类
 extern VersionManager Version;
 
 // 全局版本管理器实例
 TaskScheduler scheduler = {0};
-volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次
-volatile u32 cnt_test = 0;			 // 负载检测
+u32 motor_runtime_50us = 0; // 电机驱动计算轮次
+u32 cnt_test = 0;			// 负载检测
+
+// 函数指针数组，包含各种控制模式对应的函数
+static FOCFunctionPtr control_functions[] = {
+	&FOC_Controller::drv_hold,		   // 0: 占空比置
+	&FOC_Controller::angle_control,	   // 1: 位置闭环控制
+	&FOC_Controller::velocity_control, // 2: 速度闭环控制
+	&FOC_Controller::
+		angle_control_with_velocity_limit, // 3:
+										   // 角度闭环控制，带速度限制
+	&FOC_Controller::MIT_Mode,			   // 4: MIT 模式
+	&FOC_Controller::current_control,	   // 5: 电流闭环控制
+	&FOC_Controller::velocity_open_loop,   // 6: 速度开环控制
+	&FOC_Controller::PWM_out,		   // 0: 占空比置
+};
 int main(void) {
 	u8 init_status = hardware_init();
 
 	while (init_status) { // 初始化检测
-		std::printf("初始化失败\r\n");
+		MAIN_DEBUG_PRINT("初始化失败\r\n");
 	}
-	OTA_initial(); //  OTA初始化,A/B分区处理
+	OTA_initial(); //  OTA 初始化，A/B 分区处理
 
 	angsens_MG_check(); // 角度传感器磁感应强度测试
 	watchdog_init(50);	// 看门狗初始化
@@ -53,17 +73,21 @@ u8 hardware_init(void) {
 	systick_config();  // 系统时钟初始化
 	unused_pin_init(); // 未使用引脚初始化
 	USART0_init();	   // 调试串口初始化
-	User_timer_init(); // 任务Timer初始化
+	User_timer_init(); // 任务 Timer 初始化
 
 	u8 init_status = 0;
 	init_status += KTH7823.init(); // 角度传感器初始化
 	init_status += sampler.init(); // 采样器初始化
-	flashEEP.init(); // EEPROM初始化，必须放在角度卡尔曼之前，角度传感器之后
+
+	// EEPROM 初始化 (必须在 CAN 初始化之前，因为 CAN 会从 Flash 读取节点 ID)
+	flashEEP.init(); // EEPROM 初始化
+
+	// CAN 初始化 (会先设置默认节点 ID，然后从 Flash 加载已保存的节点 ID)
+	init_status += CAN0_transmiter.init(); // CAN 初始化
 
 	init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
 	init_status += Vel_kalman.init(0.0f);				  // 速度滤波初始化
-	init_status += CAN0_transmiter.init();				  // CAN初始化
-	init_status += FOC.init(); // FOC初始化
+	init_status += FOC.init();							  // FOC 初始化
 	return init_status;
 }
 
@@ -80,27 +104,44 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
 		FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
 	}
-	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
+	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务 (最高优先级)
 		motor_runtime_50us++;
 
-		// 函数指针数组，包含各种控制模式对应的函数
-		static FOCFunctionPtr control_functions[] = {
-			&FOC_Controlroller::drv_hold, // 0: 占空比置
-			&FOC_Controlroller::
-				angle_control_with_velocity_limit, // 1:
-												   // 角度闭环控制，带速度限制
-			&FOC_Controlroller::velocity_control,  // 2: 速度闭环控制
-			&FOC_Controlroller::current_control,   // 3: 电流闭环控制
-			&FOC_Controlroller::velocity_open_loop, // 4: 速度开环控制
-		};
+		// 快速电压检测（每100us执行一次，响应时间 < 200us）
+		FOC.fast_voltage_check();
+
+		// 静态变量用于跟踪上一次的控制模式
+		static int last_control_mode = -1;
 
 		// 边界检查，防止数组越界
-		if (FOC.control_mode >= 0 && FOC.control_mode < 5) {  // 更新为7个控制模式
-			// 调用对应的控制函数
-			(FOC.*control_functions[FOC.control_mode])();
+		if (FOC.control_mode >= 0 &&
+			FOC.control_mode < 8) { // 更新为 8 个控制模式
+
+			// 检查控制模式是否发生变化
+			if (FOC.control_mode != last_control_mode) {
+				// 重置所有 PID 控制器的状态，避免积分项累积
+				FOC.vel_loop.reset();
+				FOC.angle_loop.reset();
+				FOC.current_loop.reset();
+				FOC.vel_inVelAngloop.reset();
+				FOC.ang_inVelAngloop.reset();
+				FOC.ang_inMITloop.reset();
+				FOC.vel_inMITloop.reset();
+				FOC.current_inMITloop.reset();
+
+				// 更新上次控制模式
+				last_control_mode = FOC.control_mode;
+			}
+			if (!FOC.is_shut_down) {
+				// 调用对应的控制函数
+				(FOC.*control_functions[FOC.control_mode])();
+			} else {
+				// 调用对应的控制函数
+				FOC.drv_stop();
+			}
 		}
 	}
-	if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值
+	if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新 mcu 温度传感器值
 		sampler.update_mcu_temp();
 	}
 	if (currentTasks & (1 << TASK_DRV_TEMP_UPDATE)) { // 更新电机温度传感器值
@@ -112,11 +153,11 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测
 		angsens_MG_check();
 	}
-	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
+	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新 vbus 电压值
 		power_manage();
 	}
-	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {	  // CAN通信
-		// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
+	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {				  // CAN 通信
+		CAN0_transmiter.test_u16 = CAN0_transmiter.get_node_id(); // 负载检测
 		cnt_test = 0;
 		CAN0_transmiter.can0_send_flag = SET;
 		CAN0_transmiter.communication_check();

Robot_V2/main.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MAIN_H
 #include <cstdint>
 
+
 void do_task(u32 currentTasks);
 u8 hardware_init(void);
 

Robot_V2/sys/config.h

@@ -1,19 +1,24 @@
 #ifndef __CONFIG_H
 #define __CONFIG_H
+#include "gd32c10x_eval.h"
 #include <cstdint>
 
-// 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4
+// 如果使用 CMSIS DSP 库，需要定义 ARM_MATH_CM4
 #define ARM_MATH_CM4
 
 #define EEP_DEBUG 1
 #define OTA_DEBUG 1
 #define SLEEP_DEBUG 1
+#define ANG_SENS_DEBUG 1
+#define CAN_DEBUG 1
+#define MAIN_DEBUG 1
+#define TEST_DEBUG 1
 
 // 版本号定义 - 可根据实际项目进行调整
 constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
-constexpr u16 SOFTWARE_VERSION_MINOR = 1;
-constexpr u16 SOFTWARE_VERSION_PATCH = 24;
-constexpr u16 SOFTWARE_VERSION_BUILD = 1;
+constexpr u16 SOFTWARE_VERSION_MINOR = 4;
+constexpr u16 SOFTWARE_VERSION_PATCH = 26;
+constexpr u16 SOFTWARE_VERSION_BUILD = 2;
 
 constexpr u16 HARDWARE_VERSION_MAJOR = 25;
 constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -25,11 +30,11 @@ constexpr u16 BOOT_VERSION_MINOR = 1;
 constexpr u16 BOOT_VERSION_PATCH = 12;
 constexpr u16 BOOT_VERSION_BUILD = 0;
 
-// Flash分区配置
+// Flash 分区配置
 constexpr u32 BOOTLOADER_SIZE = 0x6000;			  // 24KB
-constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT起始地址
-constexpr u32 APPLATEST_START_ADDR = 0x08012000;  // APP起始地址
-constexpr u32 APPBACK_START_ADDR = 0x08006000;	  // APPBACK起始地址
+constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT 起始地址
+constexpr u32 APPLATEST_START_ADDR = 0x08012000;  // APP 起始地址
+constexpr u32 APPBACK_START_ADDR = 0x08006000;	  // APPBACK 起始地址
 constexpr u32 APP_MAX_SIZE = 0xC000;			  // 48KB
 constexpr u32 FMC_PAGE_SIZE = 0x400;			  // 1KB
 constexpr u32 SLEEP_CONFIG_START = 0x0801E400;	  // 睡眠配置起始地址
@@ -38,67 +43,162 @@ constexpr u32 FLASH_EMPTY = 0xFFFFFFFF;			  // 空数据
 constexpr u32 SLEEP_ENABLE = 0x11111111;		  // 睡眠使能
 constexpr u32 SLEEP_DISABLE = 0x00000000;		  // 睡眠禁止
 
+// 节点 ID 配置 (用于多电机 CAN 总线隔离)
+// 使用 CAN ID 高 16 位 (bits 31-16) 存储节点 ID，低 16 位 (bits 15-0) 存储基础 ID
+// 格式：[节点 ID(16 位)][基础 ID(16 位)]，例如：node_id=1, base_id=0x0200 → 0x00010200
+// 
+// 节点 ID 存储方案：
+// - 使用独立 Flash 页 (NODE_ID_PAGE_ADDR) 存储节点 ID，与EEPROM完全隔离
+// - 采用双重备份机制 (主备份 + 备用备份)
+// - 使用魔数 (NODE_ID_MAGIC) 验证数据有效性，防止位翻转导致错读
+// - 使用写入计数器区分最新数据
+constexpr u32 NODE_ID_PAGE_ADDR = 0x0801E800;	 // 节点 ID 存储页 (1KB，独立区域)
+constexpr u32 NODE_ID_PRIMARY_ADDR = 0x0801E800;	 // 主存储地址
+constexpr u32 NODE_ID_BACKUP_ADDR = 0x0801E840;	 // 备份存储地址 (偏移64字节)
+constexpr u32 NODE_ID_MAGIC = 0x4E4F4445;		 // 魔数 'NODE' 用于验证数据有效性
+constexpr u16 DEFAULT_NODE_ID = 1;				// 默认节点 ID
+constexpr u16 BROADCAST_NODE_ID = 0x1FFF;		// 广播 ID (所有节点响应)
+constexpr u16 MAX_NODE_ID = 0x1FFE;				// 最大有效节点 ID (0~0x1FFE)
+
+// 节点 ID 存储结构 (存储在Flash中的格式)
+// 地址偏移 0x00: 魔数 (0x4E4F4445 = 'NODE')
+// 地址偏移 0x04: 节点 ID (u16)
+// 地址偏移 0x06: 保留 (u16，填充为0xFFFF)
+// 地址偏移 0x08: 写入计数器 (u32，用于区分最新数据)
+// 地址偏移 0x0C: CRC32 校验 (u32)
+constexpr u8 NODE_ID_STRUCT_SIZE = 0x10;		 // 结构体大小 16 字节
+constexpr u8 NODE_ID_MAGIC_OFFSET = 0x00;		 // 魔数偏移
+constexpr u8 NODE_ID_VALUE_OFFSET = 0x04;		 // 节点ID值偏移
+constexpr u8 NODE_ID_COUNTER_OFFSET = 0x08;		 // 写入计数器偏移
+constexpr u8 NODE_ID_CRC_OFFSET = 0x0C;			 // CRC偏移
+
 // BOOT 相关
 constexpr u32 OTA_CONFIG_NUM = 6;
-constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801E000;	// 最新APP可用地址
-constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801E004;	// 备份APP可用地址
-constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801E008;	// 最新APP激活
-constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801E00C;	// 备份APP激活
-constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801E010; // 最新APP校验
-constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801E014; // 备份APP校验
+constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801E000;	// 最新 APP 可用地址
+constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801E004;	// 备份 APP 可用地址
+constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801E008;	// 最新 APP 激活
+constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801E00C;	// 备份 APP 激活
+constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801E010; // 最新 APP 校验
+constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801E014; // 备份 APP 校验
 constexpr u32 OTA_NOT_AVAL = 0xFFFFFFFF;
 constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用
 
 // 参数配置
 constexpr u8 PP = 11;
-constexpr float DIR = -1.0f;
+constexpr float DIR = 1.0f;  // 默认方向：-1.0f 反转，1.0f 正转
 constexpr u16 REDUCTION_RATIO = 25;
-constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移
-constexpr float HOLD_PWM_DUTY = 0.95f;
-constexpr u16 ANGLESENS_OFFSET = +0;
-constexpr float KE_FACTOR = 14.0f;
-// PID配置
-constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P
-constexpr float vel_loop_p = 0.01f / 100 * 12;	   // 速度P
-constexpr float angle_loop_p = 20.0f / 100 * 12;	   // 角度P
-constexpr float vel_angloop_p = 0.12f / 100 * 12;   // 速度 速度-角度P
-constexpr float ang_angloop_p = 100.0f / 100 * 12;  // 角度 速度-角度P
+constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移
+constexpr float HOLD_PWM_DUTY = 0.1f;
+constexpr u16 ANGLESENS_OFFSET = +15;
+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 VELOCITY_KALMAN_INITIAL_STATE = 0.0f;
+constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
+constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
+	1e-6f; // 过程噪声协方差 - 通常保持较小
+constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
+	1.0f; // 测量噪声协方差 - 主要调节参数 1.0
+
+// 电流卡尔曼滤波器参数
+constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
+constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f; // 增大初始协方差
+constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f;		// 降低过程噪声
+constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声
+
+// PID 配置
+constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流 P
+constexpr float current_loop_Ki = 0.55f;				  // 电流 P
+constexpr float current_loop_Kd = 3e-4f;				  // 电流 P
+
+constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度 P
+constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
+constexpr float vel_loop_Tu = 17.0f * 1e-3f;
+constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
+constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
+
+constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度 - 角度 P
+constexpr float vel_angloop_Ki = 5.0f;				// 速度 速度 - 角度 I
+constexpr float vel_angloop_Kd = 5e-05f;			// 速度 速度 - 角度 D
+
+constexpr float DEFAULT_angle_angloop_p =
+	150.0f / 100 * 12;					 // 角度 速度 - 角度 P
+constexpr float ang_angloop_Ki = 0.00f;	 // 角度 速度 - 角度 I
+constexpr float ang_angloop_Kd = 8e-04f; // 角度 速度 - 角度 D
+
+constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度 P
+
+constexpr float DEFAULT_angle_MIT_p = 100.0f / 100 * 12; // 角度 MIT P
+constexpr float angle_MIT_Ki = 0.0f;
+constexpr float angle_MIT_Kd = 8e-04f;
+
+constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
+constexpr float vel_MIT_Ki = 15.0f;
+constexpr float vel_MIT_Kd = 1e-04f;
+
+constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
+constexpr float current_MIT_Ki = 0.1f;
+constexpr float current_MIT_Kd = 3e-4f;
 // 常量定义
 constexpr u8 SIZE_FLOAT = sizeof(float);
 constexpr u8 SIZE_U32 = sizeof(uint32_t);
 constexpr float ADC_MAX = 65536.0f;
 constexpr float PI = 3.14159265358979323846f;
+constexpr float INV_PI_3 = 3.0f / PI;
+constexpr float PI_3 = PI / 3.0f;
+constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
+constexpr float _3PI_2 = 4.71238898038f;
+constexpr float SQRT3 = 1.7320508075688772f;
+constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
+constexpr float COS_PI_3 = 0.5f;
 constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
 constexpr float DEG2RAD = 1.0f / 180.0f * PI;
-constexpr float RAD2DEG = 180.0f / PI;	   // 修正RAD2DEG定义，应该是180/π
-constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
-constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
+constexpr float RAD2DEG = 180.0f / PI; // 修正 RAD2DEG 定义，应该是 180/π
 constexpr u8 DEFAULT_VALUE = 0;
 constexpr u8 CONFIG_SAVED_NUM = 6;
 
-// 三角函数快速计算LUT
+// 三角函数快速计算 LUT
 constexpr u16 LUT_SIZE = 1024;
 constexpr u16 LUT_MASK = (LUT_SIZE - 1);
 constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
 
-// CAN相关
+// CAN 相关
+// 注意：以下为 CAN 基础 ID（只保留低 16 位），实际发送/接收时会自动叠加节点 ID
+// 到高 16 位 格式：[节点 ID(16 位)][基础 ID(16 位)] 例如：节点 ID=1，发送命令
+// ID = (1 << 16) | CANFD_CMD_ID = 0x00010200
 constexpr u16 DEV_CAN0_ID = 0x0000;
 constexpr u16 DEV_CAN0_MASK = 0x0000;
 constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
-// 反馈ID
-constexpr u32 CANFD_STATUS_ID = 0x00010100;
-constexpr u32 CANFD_CONFIG_ID = 0x00010101;
-constexpr u32 CANFD_VERSION_ID = 0x00010000;
-// 命令ID
-constexpr u32 CANFD_CMD_ID = 0x00010200;
-constexpr u32 CANFD_RESET_ID = 0x00010301;
-// OTA相关ID
-constexpr u32 CANFD_BOOT_ID = 0x00000000;
-constexpr u32 CANFD_OTA_REQ_ID = 0x00000100;
-constexpr u32 CANFD_OTA_DATA_ID = 0x00000101;
-constexpr u32 CANFD_OTA_ACK_ID = 0x00000102;
-constexpr u32 CANFD_OTA_WAIT_ID = 0x00000103;
+// 反馈基础 ID
+constexpr u16 CANFD_STATUS_ID = 0x0100;	 // 状态反馈基础 ID
+constexpr u16 CANFD_CONFIG_ID = 0x0101;	 // 配置信息基础 ID
+constexpr u16 CANFD_VERSION_ID = 0x00FF; // 版本信息基础 ID
+// 命令基础 ID
+constexpr u16 CANFD_CMD_ID = 0x0200;   // 控制命令基础 ID
+constexpr u16 CANFD_RESET_ID = 0x0301; // 重置命令基础 ID
+// 节点 ID 管理基础 ID
+constexpr u16 CANFD_SET_NODE_ID = 0x0400; // 设置节点 ID 基础 ID
+constexpr u16 CANFD_GET_NODE_ID = 0x0401; // 查询节点 ID 基础 ID
+
+// OTA 相关基础 ID 不受到node ID影响
+constexpr u16 CANFD_BOOT_ID = 0x0000; // BOOT 模式基础 ID (进入 BOOT 使用此 ID)
+constexpr u16 CANFD_OTA_REQ_ID = 0x0100;  // OTA 请求基础 ID
+constexpr u16 CANFD_OTA_DATA_ID = 0x0101; // OTA 数据基础 ID
+constexpr u16 CANFD_OTA_ACK_ID = 0x0102;  // OTA 确认基础 ID
+constexpr u16 CANFD_OTA_WAIT_ID = 0x0103; // OTA 等待基础 ID
 
 // ADC
 constexpr u8 ADC_FILTER_SIZE = 6;
@@ -122,14 +222,31 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
 
 // TIMER2-任务调度
 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
-constexpr int TIMER2_PERIOD = 49;	  // 1MHz / 50 = 50us
-constexpr float TIMER2_FREQUENCY = 1 / 5e-5;
+constexpr int TIMER2_PERIOD = 49;	  // 1 / 1MHz * 50 = 50us
+constexpr int MOTOR_PERIOD = 2;		  // 100us/50us = 2
+constexpr float TIME_SLICE =
+	(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
+constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE;				 // 频率
+constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
 
 // TIMER1-PWM
-constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
+constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为 120MHz
 constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
 constexpr int TIMER1_DEFAULT_PULSE = 0;
 constexpr int TIMER1_CH3_COMPARE_VALUE =
-	(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
+	(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM 周期 50% 位置触发中断
+
+// 电压检测参数（基于跌幅检测，不依赖固定额定电压）
+// 快速检测层（fast_voltage_check，100us周期）：
+//   - 使用峰值检测算法跟踪当前额定电压（vbus_reference）
+//   - 当电压相对于基准电压下跌超过 VBUS_DROP_THRESHOLD → 关机
+//   - 当电压低于绝对下限 VBUS_ABSOLUTE_MIN → 关机
+// 慢速管理层（power_manage，~10ms周期）：
+//   - 执行Flash保存
+//   - 状态恢复判断（电压恢复到基准电压的90%以上）
+constexpr float VBUS_DROP_THRESHOLD = 5.0f;			  // 电压跌幅阈值 (V)
+constexpr float VBUS_ABSOLUTE_MIN = 12.0f;			  // 绝对最低电压 (V)
+constexpr u32 VBUS_DEBOUNCE_COUNT = 5;				  // 快速检测：防抖计数 (5次×100us = 500us)
+constexpr float VBUS_RECOVERY_RATIO = 0.9f;			  // 恢复比例（基准电压的90%）
 
 #endif

Robot_V2/sys/flash.cpp

@@ -2,7 +2,7 @@
 #include "FOC.h"
 #include "ang_sensor.h"
 #include "config.h"
-#include "gd32c10x.h" // GD32主头文件，确保外设函数声明可用
+#include "gd32c10x.h" // GD32 主头文件，确保外设函数声明可用
 #include "gd32c10x_crc.h"
 #include "gd32c10x_fmc.h"
 #include "systick.h"
@@ -10,10 +10,24 @@
 #include <cstdio>
 #include <cstring>
 
+// EEP 调试宏定义
+#if EEP_DEBUG
+#define EEP_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define EEP_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
+
+// OTA 调试宏定义
+#if OTA_DEBUG
+#define OTA_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define OTA_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
+
 FlashEEPROM flashEEP = FlashEEPROM();
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
-extern FOC_Controlroller FOC; // FOC控制
+extern FOC_Controller FOC; // FOC 控制
 
 fmc_state_enum write_ADDRESS(u32 address, u32 data) {
 	fmc_unlock();
@@ -25,19 +39,19 @@ fmc_state_enum write_ADDRESS(u32 address, u32 data) {
 	fmc_lock();
 	return fmc_status;
 }
-// FlashEEPROM类实现
+// FlashEEPROM 类实现
 FlashEEPROM::FlashEEPROM() {
 	// 构造函数可以为空，或者做一些初始化工作
 }
 
 /**
- * @brief  初始化Flash EEPROM模拟
+ * @brief  初始化 Flash EEPROM 模拟
  * @param  None
  * @retval None
  */
 u8 FlashEEPROM::init() {
 	rcu_periph_clock_enable(RCU_CRC);
-	crc_deinit(); // 初始化CRC模块
+	crc_deinit(); // 初始化 CRC 模块
 
 	FlashEepromStatus status = FLASH_EEPROM_OK;
 	// 读取系统配置
@@ -52,7 +66,7 @@ u8 FlashEEPROM::init() {
 }
 
 /**
- * @brief  读取EEPROM数据
+ * @brief  读取 EEPROM 数据
  * @param  virtualAddr: 虚拟地址
  * @param  data: 存储读取数据的指针
  * @retval EEPROMStatus: 是否成功
@@ -62,7 +76,7 @@ u8 FlashEEPROM::read_saved_system_config(void) {
 	u32 CRC_NUM = 0x0000;
 	FlashEepromStatus status_read[CONFIG_SAVED_NUM + 1] = {FLASH_EEPROM_OK};
 	float data[CONFIG_SAVED_NUM] = {0.0f};
-	// 尝试从Flash读取参数，如果不存在则使用默认值
+	// 尝试从 Flash 读取参数，如果不存在则使用默认值
 	status_read[1] = readFloat(ADDR_CONTROL_MODE, &data[0]);
 	status_read[2] = readFloat(ADDR_VOLTAGE_LIMIT, &data[1]);
 	status_read[3] = readFloat(ADDR_VEL_LIMIT, &data[2]);
@@ -83,10 +97,10 @@ u8 FlashEEPROM::read_saved_system_config(void) {
 	// 读取到全部数据
 	if (status_read[0] == FLASH_EEPROM_OK) {
 		u32 next_address = EEPROM_END_ADDRESS;
-		// 遍历Flash页面寻找下一个可写入位置
+		// 遍历 Flash 页面寻找下一个可写入位置
 		for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32;
 			 address > EEPROM_START_ADDRESS; address -= SIZE_U32) {
-			// 安全读取Flash数据
+			// 安全读取 Flash 数据
 			volatile u32 *flash_ptr = (volatile u32 *)address;
 			u32 data_word = *flash_ptr;
 			// 找到空位置
@@ -102,18 +116,16 @@ u8 FlashEEPROM::read_saved_system_config(void) {
 			return FLASH_EEPROM_OK;
 		}
 
-		crc_data_register_reset(); // 确保CRC计算从初始状态开始
+		crc_data_register_reset(); // 确保 CRC 计算从初始状态开始
 		CRC_NUM = crc_block_data_calculate((uint32_t *)data, CONFIG_SAVED_NUM);
-
-#if EEP_DEBUG
-		printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
-#endif
-		if (CRC_NUM != *((u32 *)next_address)) { // CRC错误
+		EEP_DEBUG_PRINT("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM,
+						*((u32 *)next_address));
+		if (CRC_NUM != *((u32 *)next_address)) { // CRC 错误
 			for (u8 i = 0; i < CONFIG_SAVED_NUM; i++) {
 				data[i] = DEFAULT_VALUE;
 			}
 			EEPROMStatus = FLASH_EEPROM_CRC_ERROR;
-		} else { //  CRC正确
+		} else { //  CRC 正确
 			EEPROMStatus = FLASH_EEPROM_OK;
 		}
 	} else {
@@ -125,7 +137,7 @@ u8 FlashEEPROM::read_saved_system_config(void) {
 
 	FOC.control_mode = (u8)data[0];
 	FOC.limit_voltage = data[1];
-	FOC.limit_velocity = data[2];
+	FOC.target_current = data[2];
 	KTH7823.angle_set(data[3]);
 	FOC.target_angle = data[4];
 	FOC.target_velocity = data[5];
@@ -133,7 +145,7 @@ u8 FlashEEPROM::read_saved_system_config(void) {
 }
 
 /**
- * @brief  从Flash EEPROM读取数据
+ * @brief  从 Flash EEPROM 读取数据
  * @param  virtualAddr: 虚拟地址
  * @param  data: 指向存储读取数据的指针
  * @retval FlashEepromStatus: 操作状态
@@ -144,7 +156,7 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const {
 		return FLASH_EEPROM_INVALID_ADDR;
 	}
 
-	// 从Flash末尾开始向前搜索，找到最新的数据
+	// 从 Flash 末尾开始向前搜索，找到最新的数据
 	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
 		 address >= EEPROM_START_ADDRESS; address -= SIZE_U32) {
 		u32 data_word = *((u32 *)address);
@@ -153,7 +165,9 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const {
 			u8 addr = (u8)(data_word & DATA_ADDR_MASK);
 			// 找到匹配的虚拟地址
 			if (addr == virtualAddr) {
-				*data = (u16)(data_word & DATA_U16_MASK);
+				// 右移 16 位以提取高 16 位的 u16 数据
+				*data = (u16)((data_word & DATA_U16_MASK) >> 16);
+				EEP_DEBUG_PRINT("read u16: addr=0x%x, val=%d\r\n", address, *data);
 				return FLASH_EEPROM_OK;
 			}
 		}
@@ -162,7 +176,7 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const {
 }
 
 /**
- * @brief  从Flash EEPROM读取float数据
+ * @brief  从 Flash EEPROM 读取 float 数据
  * @param  virtualAddr: 虚拟地址
  * @param  data: 指向存储读取数据的指针
  * @retval FlashEepromStatus: 操作状态
@@ -173,10 +187,10 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const {
 		return FLASH_EEPROM_INVALID_ADDR;
 	}
 
-	// 从Flash末尾开始向前搜索，找到最新的数据
+	// 从 Flash 末尾开始向前搜索，找到最新的数据
 	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
 		 address >= EEPROM_START_ADDRESS; address -= SIZE_U32) {
-		// 使用volatile确保每次都从内存读取，防止编译器优化
+		// 使用 volatile 确保每次都从内存读取，防止编译器优化
 		volatile u32 *flash_ptr = (volatile u32 *)address;
 		u32 data_word = *flash_ptr;
 
@@ -185,13 +199,11 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const {
 			u8 addr = (u8)(data_word & DATA_ADDR_MASK);
 			// 找到匹配的虚拟地址
 			if (addr == virtualAddr) {
-				// 清除最低的8位地址信息，保留高24位数据
+				// 清除最低的 8 位地址信息，保留高 24 位数据
 				u32 float_data = data_word & DATA_FLOAT_MASK;
-				// 使用内存拷贝将处理后的数据解释为float
+				// 使用内存拷贝将处理后的数据解释为 float
 				memcpy(data, &float_data, SIZE_FLOAT);
-#if EEP_DEBUG
-				printf("read:0x%x=%f\r\n", address, *data);
-#endif
+				EEP_DEBUG_PRINT("read:0x%x=%f\r\n", address, *data);
 				return FLASH_EEPROM_OK;
 			}
 		}
@@ -201,7 +213,7 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const {
 }
 
 /**
- * @brief  向Flash EEPROM写入数据
+ * @brief  向 Flash EEPROM 写入数据
  * @param  virtualAddr: 虚拟地址
  * @param  data: 要写入的数据
  * @retval FlashEepromStatus: 操作状态
@@ -210,22 +222,23 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
 	// 查找下一个可写入的位置
 	u32 next_address = EEPROM_START_ADDRESS;
 	u32 data_read;
-	// 插入虚拟地址到低8位	格式: [data:16bit][reserved:8bit][addr:8bit]
-	data = (data & DATA_U16_MASK) | (u32)virtualAddr;
+	// 构建要写入的 32 位数据：格式 [data:16bit][reserved:8bit][addr:8bit]
+	u32 data_word_to_write = ((u32)data << 16) | (u32)virtualAddr;
 
 	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
 		 address >= EEPROM_DATA_START_ADDRESS; address -= SIZE_U32) {
-		// 安全读取Flash数据
+		// 安全读取 Flash 数据
 		volatile u32 *flash_ptr = (volatile u32 *)address;
 		data_read = *flash_ptr;
 
 		if ((data_read & DATA_ADDR_MASK) == (u32)virtualAddr) { // 虚拟地址相同
 			if ((data_read & DATA_U16_MASK) ==
-				(data & DATA_U16_MASK)) { // 数据相同
+				(data_word_to_write & DATA_U16_MASK)) { // 数据相同
 #if EEP_DEBUG
 				data_read = data_read & DATA_U16_MASK;
 				u16 float_data_read = *((u16 *)&data_read);
-				printf("Found data in:0x%x=%u\r\n", address, data_read);
+				EEP_DEBUG_PRINT("Found data in:0x%x=%u\r\n", address,
+								data_read);
 #endif
 				return FLASH_EEPROM_OK;
 			} else {
@@ -234,10 +247,10 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
 		}
 	}
 
-	// 遍历Flash页面寻找下一个可写入位置
+	// 遍历 Flash 页面寻找下一个可写入位置
 	for (u32 address = EEPROM_DATA_START_ADDRESS; address < EEPROM_END_ADDRESS;
 		 address += SIZE_U32) {
-		// 安全读取Flash数据
+		// 安全读取 Flash 数据
 		volatile u32 *flash_ptr = (volatile u32 *)address;
 		u32 data_word = *flash_ptr;
 		// 找到空位置
@@ -251,18 +264,15 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
 
 	// 如果有足够空间，直接写入
 	if (next_address < EEPROM_END_ADDRESS) {
-		u32 data_word = ((u32)data << 16) | (u32)virtualAddr;
-
-		fmc_state_enum fmc_status = write_ADDRESS(next_address, data_word);
+		fmc_state_enum fmc_status =
+			write_ADDRESS(next_address, data_word_to_write);
 
 		if (fmc_status != FMC_READY) {
 			return FLASH_EEPROM_WRITE_ERROR;
 		}
 		return FLASH_EEPROM_OK;
 	} else { // 空间不足，需要格式化页面
-#if EEP_DEBUG
-		printf("no enough space\r\n");
-#endif
+		EEP_DEBUG_PRINT("no enough space\r\n");
 		FlashEepromStatus status = format();
 		if (status == FLASH_EEPROM_OK) {
 			status = save_system_config(false);
@@ -274,7 +284,7 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
 }
 
 /**
- * @brief  向Flash EEPROM写入float数据
+ * @brief  向 Flash EEPROM 写入 float 数据
  * @param  virtualAddr: 虚拟地址
  * @param  data: 要写入的数据
  * @retval FlashEepromStatus: 操作状态
@@ -284,15 +294,15 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
 	u32 next_address = EEPROM_START_ADDRESS;
 	u32 data_word;
 
-	// 将完整的float数据复制到32位变量中
+	// 将完整的 float 数据复制到 32 位变量中
 	memcpy(&data_word, &data, SIZE_FLOAT);
-	// 插入虚拟地址到低8位	格式: [data:24bit][addr:8bit]
+	// 插入虚拟地址到低 8 位	格式：[data:24bit][addr:8bit]
 	data_word = (data_word & DATA_FLOAT_MASK) | (u32)virtualAddr;
 
-	// 从Flash末尾开始向前搜索，找到最新的数据
+	// 从 Flash 末尾开始向前搜索，找到最新的数据
 	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
 		 address >= EEPROM_DATA_START_ADDRESS; address -= SIZE_U32) {
-		// 安全读取Flash数据
+		// 安全读取 Flash 数据
 		volatile u32 *flash_ptr = (volatile u32 *)address;
 		u32 data_word_read = *flash_ptr;
 
@@ -303,7 +313,8 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
 #if EEP_DEBUG
 				data_word_read = data_word_read & DATA_FLOAT_MASK;
 				float float_data_read = *((float *)&data_word_read);
-				printf("Found data in:0x%x=%f\r\n", address, float_data_read);
+				EEP_DEBUG_PRINT("Found data in:0x%x=%f\r\n", address,
+								float_data_read);
 #endif
 				return FLASH_EEPROM_OK;
 			} else {
@@ -312,10 +323,10 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
 		}
 	}
 
-	// 遍历Flash页面寻找下一个可写入位置
+	// 遍历 Flash 页面寻找下一个可写入位置
 	for (u32 address = EEPROM_DATA_START_ADDRESS; address < EEPROM_END_ADDRESS;
 		 address += SIZE_U32) {
-		// 安全读取Flash数据
+		// 安全读取 Flash 数据
 		volatile u32 *flash_ptr = (volatile u32 *)address;
 		u32 data_word_read = *flash_ptr;
 		// 找到空位置
@@ -334,7 +345,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
 		u32 data_word_c = *((u32 *)next_address);
 		data_word_c = data_word_c & DATA_FLOAT_MASK;
 		float float_data = *((float *)&data_word_c);
-		printf("save:0x%x=%f\r\n", next_address, float_data);
+		EEP_DEBUG_PRINT("save:0x%x=%f\r\n", next_address, float_data);
 #endif
 		if (fmc_status != FMC_READY) {
 			return FLASH_EEPROM_WRITE_ERROR;
@@ -343,7 +354,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
 	} else {
 		// 空间不足，需要格式化页面
 #if EEP_DEBUG
-		printf("no enough space\r\n");
+		EEP_DEBUG_PRINT("no enough space\r\n");
 #endif
 		FlashEepromStatus status = format();
 		if (status == FLASH_EEPROM_OK) {
@@ -356,15 +367,13 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
 }
 
 /**
- * @brief  格式化Flash EEPROM页面
+ * @brief  格式化 Flash EEPROM 页面
  * @param  None
  * @retval FlashEepromStatus: 操作状态
  */
 FlashEepromStatus FlashEEPROM::format() {
 	fmc_state_enum fmc_status;
-#if EEP_DEBUG
-	printf("擦除页面\r\n");
-#endif
+	EEP_DEBUG_PRINT("擦除页面\r\n");
 
 	fmc_unlock();
 	fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); // 擦除第一页
@@ -382,14 +391,14 @@ FlashEepromStatus FlashEEPROM::format() {
 }
 
 /**
- * @brief 更新系统配置并保存到Flash
+ * @brief 更新系统配置并保存到 Flash
  * @param config 新的系统配置值
  */
 FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
 	u32 CRC_NUM = 0x0000;
 	float data[CONFIG_SAVED_NUM] = {
 		(float)FOC.control_mode, FOC.limit_voltage,
-		FOC.limit_velocity,		 FOC.rt_angle,
+		FOC.target_current,		 FOC.rt_angle,
 		FOC.target_angle,		 FOC.target_velocity};
 
 	// 如果系统配置标记已保存，则无需再次保存
@@ -437,18 +446,18 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
 		return status;
 	}
 
-	// 创建用于CRC计算的模拟数据数组，模拟实际存储到Flash中的数据格式
+	// 创建用于 CRC 计算的模拟数据数组，模拟实际存储到 Flash 中的数据格式
 	u32 crc_data_words[CONFIG_SAVED_NUM];
 
-	// 按照writeFloat函数的方式处理数据
+	// 按照 writeFloat 函数的方式处理数据
 	for (u8 i = 0; i < CONFIG_SAVED_NUM; i++) {
-		// 将完整的float数据复制到32位变量中
+		// 将完整的 float 数据复制到 32 位变量中
 		memcpy(&crc_data_words[i], &data[i], SIZE_FLOAT);
-		// 去除最后2位精度
+		// 去除最后 2 位精度
 		crc_data_words[i] = crc_data_words[i] & DATA_FLOAT_MASK;
 	}
 
-	crc_data_register_reset(); // 确保CRC计算从初始状态开始
+	crc_data_register_reset(); // 确保 CRC 计算从初始状态开始
 	CRC_NUM = crc_block_data_calculate(crc_data_words, CONFIG_SAVED_NUM);
 	writeCRC(CRC_NUM, saved);
 
@@ -457,7 +466,7 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
 }
 
 /**
- * @brief 读取角度数据从Flash
+ * @brief 读取角度数据从 Flash
  * @return float: 角度数据
  */
 float FlashEEPROM::read_angle() {
@@ -468,17 +477,239 @@ float FlashEEPROM::read_angle() {
 	return angle;
 }
 
+// ============================================================================
+// 节点 ID 独立读写函数实现（使用魔数+双重备份+计数器机制）
+// ============================================================================
+
+/*!
+ * @brief 计算节点 ID 数据的 CRC32 校验值
+ * @param node_id: 节点 ID 值
+ * @param counter: 写入计数器值
+ * @retval u32: CRC32 校验值
+ */
+static u32 calculate_node_id_crc(u16 node_id, u32 counter) {
+	u32 crc_data[2];
+	crc_data[0] = (u32)node_id; // 节点 ID 值
+	crc_data[1] = counter;		 // 写入计数器
+	crc_data_register_reset();
+	return crc_block_data_calculate(crc_data, 2);
+}
+
+/*!
+ * @brief 从指定地址读取节点 ID 数据结构
+ * @param addr: Flash 地址
+ * @param node_id: 输出节点 ID 值
+ * @param counter: 输出写入计数器值
+ * @retval FlashEepromStatus: 操作状态
+ */
+static FlashEepromStatus read_node_id_struct(u32 addr, u16 *node_id, u32 *counter) {
+	// 检查地址是否对齐
+	if (addr & 0x3) {
+		return FLASH_EEPROM_ALIGNMENT_ERROR;
+	}
+
+	volatile u32 *flash_ptr = (volatile u32 *)addr;
+	
+	// 读取魔数
+	u32 magic = flash_ptr[NODE_ID_MAGIC_OFFSET / sizeof(u32)];
+	if (magic != NODE_ID_MAGIC) {
+		EEP_DEBUG_PRINT("Node ID magic mismatch at 0x%08X: 0x%08X\r\n", addr, magic);
+		return FLASH_EEPROM_DATA_NOT_FOUND;
+	}
+
+	// 读取节点 ID 值
+	u16 id_value = (u16)flash_ptr[NODE_ID_VALUE_OFFSET / sizeof(u32)];
+	if (id_value > MAX_NODE_ID && id_value != 0xFFFF) {
+		EEP_DEBUG_PRINT("Invalid node ID value: %d\r\n", id_value);
+		return FLASH_EEPROM_READ_ERROR;
+	}
+
+	// 读取写入计数器
+	u32 write_counter = flash_ptr[NODE_ID_COUNTER_OFFSET / sizeof(u32)];
+	
+	// 读取并验证 CRC
+	u32 stored_crc = flash_ptr[NODE_ID_CRC_OFFSET / sizeof(u32)];
+	u32 calculated_crc = calculate_node_id_crc(id_value, write_counter);
+	if (stored_crc != calculated_crc) {
+		EEP_DEBUG_PRINT("Node ID CRC mismatch at 0x%08X: stored=0x%08X, calc=0x%08X\r\n",
+						addr, stored_crc, calculated_crc);
+		return FLASH_EEPROM_CRC_ERROR;
+	}
+
+	*node_id = id_value;
+	*counter = write_counter;
+	return FLASH_EEPROM_OK;
+}
+
+/*!
+ * @brief 写入节点 ID 数据结构到指定地址
+ * @param addr: Flash 地址
+ * @param node_id: 节点 ID 值
+ * @param counter: 写入计数器值
+ * @retval FlashEepromStatus: 操作状态
+ */
+static FlashEepromStatus write_node_id_struct(u32 addr, u16 node_id, u32 counter) {
+	// 检查地址是否对齐
+	if (addr & 0x3) {
+		return FLASH_EEPROM_ALIGNMENT_ERROR;
+	}
+
+	// 计算 CRC
+	u32 crc = calculate_node_id_crc(node_id, counter);
+	
+	// 准备写入数据
+	u32 write_data[4];
+	write_data[0] = NODE_ID_MAGIC;					 // 魔数
+	write_data[1] = (u32)node_id;						 // 节点 ID 值
+	write_data[2] = counter;							 // 写入计数器
+	write_data[3] = crc;									 // CRC 校验
+
+	// 写入 Flash（需要解锁）
+	fmc_unlock();
+	fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
+	
+	fmc_state_enum fmc_status;
+	for (u8 i = 0; i < 4; i++) {
+		fmc_status = fmc_word_program(addr + i * sizeof(u32), write_data[i]);
+		if (fmc_status != FMC_READY) {
+			fmc_lock();
+			EEP_DEBUG_PRINT("Flash write failed at offset %d, status=%d\r\n", i, fmc_status);
+			return FLASH_EEPROM_WRITE_ERROR;
+		}
+	}
+	
+	fmc_lock();
+	return FLASH_EEPROM_OK;
+}
+
+/*!
+ * @brief 擦除节点 ID 存储页
+ * @retval FlashEepromStatus: 操作状态
+ */
+FlashEepromStatus FlashEEPROM::eraseNodeId() {
+	fmc_state_enum fmc_status;
+	
+	EEP_DEBUG_PRINT("Erasing Node ID page at 0x%08X\r\n", NODE_ID_PAGE_ADDR);
+	
+	fmc_unlock();
+	fmc_status = fmc_page_erase(NODE_ID_PAGE_ADDR);
+	if (fmc_status != FMC_READY) {
+		fmc_lock();
+		EEP_DEBUG_PRINT("Node ID page erase failed, status=%d\r\n", fmc_status);
+		return FLASH_EEPROM_ERASE_ERROR;
+	}
+	fmc_lock();
+	
+	return FLASH_EEPROM_OK;
+}
+
+/*!
+ * @brief 从 Flash 读取节点 ID
+ * @param node_id: 输出节点 ID 值
+ * @retval FlashEepromStatus: 操作状态
+ */
+FlashEepromStatus FlashEEPROM::readNodeId(u16 *node_id) const {
+	if (node_id == nullptr) {
+		return FLASH_EEPROM_INVALID_ADDR;
+	}
+
+	u16 id_value = 0;
+	u32 counter = 0;
+
+	// 从主存储区读取
+	FlashEepromStatus status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &id_value, &counter);
+	
+	if (status == FLASH_EEPROM_OK) {
+		*node_id = id_value;
+		EEP_DEBUG_PRINT("Node ID read from primary (counter=%u): %d\r\n", counter, *node_id);
+		return FLASH_EEPROM_OK;
+	} else {
+		EEP_DEBUG_PRINT("Node ID read failed from primary: status=%d\r\n", status);
+		return FLASH_EEPROM_DATA_NOT_FOUND;
+	}
+}
+
+/*!
+ * @brief 保存节点 ID 到 Flash（每次写入前先擦除整个页面，写入后校验）
+ * @param node_id: 要保存的节点 ID 值
+ * @retval FlashEepromStatus: 操作状态
+ * @note       由于Flash只能将1写为0，不能将0写为1，因此每次写入新数据前
+ *             需要先擦除整个页面。简化方案：不再使用备用备份区，
+ *             每次写入前擦除页面，然后写入主存储区。
+ *             写入后立即校验数据，如果不对则再次写入，最多3次。
+ */
+FlashEepromStatus FlashEEPROM::writeNodeId(u16 node_id) {
+	if (node_id > MAX_NODE_ID) {
+		EEP_DEBUG_PRINT("Invalid node ID: %d (max=%d)\r\n", node_id, MAX_NODE_ID);
+		return FLASH_EEPROM_INVALID_ADDR;
+	}
+
+	// 获取当前写入计数器（从主存储区读取）
+	u16 current_id = 0;
+	u32 current_counter = 0;
+	
+	FlashEepromStatus primary_status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &current_id, &current_counter);
+	
+	// 计数器 +1
+	u32 new_counter = current_counter + 1;
+	// 如果计数器溢出（0xFFFFFFFF + 1 = 0），则使用1
+	if (new_counter == 0) {
+		new_counter = 1;
+	}
+
+	// 先擦除整个页面
+	EEP_DEBUG_PRINT("Erasing Node ID page before write\r\n");
+	FlashEepromStatus status = eraseNodeId();
+	if (status != FLASH_EEPROM_OK) {
+		return status;
+	}
+
+	// 写入并校验，最多3次
+	#define MAX_WRITE_RETRY 3
+	for (u8 retry = 0; retry < MAX_WRITE_RETRY; retry++) {
+		// 写入主存储区
+		EEP_DEBUG_PRINT("Writing to primary (counter=%u, retry=%d)\r\n", new_counter, retry);
+		FlashEepromStatus write_status = write_node_id_struct(NODE_ID_PRIMARY_ADDR, node_id, new_counter);
+
+		if (write_status != FLASH_EEPROM_OK) {
+			EEP_DEBUG_PRINT("Node ID write failed: status=%d\r\n", write_status);
+			continue;
+		}
+
+		// 立即校验数据
+		u16 read_id = 0;
+		u32 read_counter = 0;
+		FlashEepromStatus read_status = read_node_id_struct(NODE_ID_PRIMARY_ADDR, &read_id, &read_counter);
+		
+		if (read_status == FLASH_EEPROM_OK && read_id == node_id && read_counter == new_counter) {
+			EEP_DEBUG_PRINT("Node ID saved and verified successfully: %d (counter=%u)\r\n", node_id, new_counter);
+			return FLASH_EEPROM_OK;
+		} else {
+			EEP_DEBUG_PRINT("Node ID verification failed: read_id=%d (expected=%d), read_counter=%u (expected=%u)\r\n",
+							read_id, node_id, read_counter, new_counter);
+			// 校验失败，重新擦除并写入
+			if (retry < MAX_WRITE_RETRY - 1) {
+				EEP_DEBUG_PRINT("Retrying...\r\n");
+				eraseNodeId();
+			}
+		}
+	}
+
+	EEP_DEBUG_PRINT("Node ID save failed after %d retries\r\n", MAX_WRITE_RETRY);
+	return FLASH_EEPROM_WRITE_ERROR;
+}
+
 /**
- * @brief 保存CRC数据到Flash
+ * @brief 保存 CRC 数据到 Flash
  * @param saved: 是否一次性保存
  * @return FlashEepromStatus: 状态
  */
 FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
 	static u32 next_address;
-	// 遍历Flash页面寻找下一个可写入crc的位置
+	// 遍历 Flash 页面寻找下一个可写入 crc 的位置
 	for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32;
 		 address > EEPROM_START_ADDRESS; address -= SIZE_U32) {
-		// 安全读取Flash数据
+		// 安全读取 Flash 数据
 		volatile u32 *flash_ptr = (volatile u32 *)address;
 		u32 data_word = *flash_ptr;
 		// 找到空位置
@@ -490,14 +721,13 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
 		next_address = address - SIZE_U32;
 	}
 
-	// 如果当前CRC与现在的crc值不同，则写入
+	// 如果当前 CRC 与现在的 crc 值不同，则写入
 	if (*((u32 *)next_address) != CRC_NUM) {
-		// 如果有足够空间，直接写入crc值
+		// 如果有足够空间，直接写入 crc 值
 		if (next_address > EEPROM_START_ADDRESS) {
 			fmc_state_enum fmc_status = write_ADDRESS(next_address, CRC_NUM);
-#if EEP_DEBUG
-			printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
-#endif
+			EEP_DEBUG_PRINT("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM,
+							*((u32 *)next_address));
 			// 检查写入是否成功
 			if (fmc_status != FMC_READY) {
 				EEPROMStatus = fmc_status;
@@ -505,9 +735,7 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
 			}
 		} else {
 			// 空间不足，需要格式化页面
-#if EEP_DEBUG
-			printf("no enough space\r\n");
-#endif
+			EEP_DEBUG_PRINT("no enough space\r\n");
 			FlashEepromStatus status = format();
 			if (status == FLASH_EEPROM_OK) {
 				status = save_system_config(false);
@@ -519,33 +747,33 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
 		}
 	}
 
-	// 重新读取CRC校验
+	// 重新读取 CRC 校验
 	if (CRC_NUM == *((u32 *)next_address)) {
 		system_config_saved = saved;	// 标记已保存过
-		EEPROMStatus = FLASH_EEPROM_OK; // 设置EEPROM状态
+		EEPROMStatus = FLASH_EEPROM_OK; // 设置 EEPROM 状态
 		return FLASH_EEPROM_OK;
 	} else {
-		EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置EEPROM错误状态
+		EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置 EEPROM 错误状态
 		return FLASH_EEPROM_CRC_ERROR;
 	}
 }
 
 /**
- * @brief  设置OTA配置参数到Flash
+ * @brief  设置 OTA 配置参数到 Flash
  *
- * 此函数用于更新存储在Flash中的OTA相关标志位（如APP可用、启用、校验等）。
- * GD32的Flash特性是：只能将1写为0，不能将0写为1。若要将0变为1，必须先擦除整个页面。
- * 因此，本函数通过读取同一页内的所有参数，擦除该页，然后重新写入有效的参数来实现“写1”操作。
+ * 此函数用于更新存储在 Flash 中的 OTA 相关标志位（如 APP 可用、启用、校验等）。
+ * GD32 的 Flash 特性是：只能将 1 写为 0，不能将 0 写为 1。若要将 0 写为
+ * 1，必须先擦除整个页面。
+ * 因此，本函数通过读取同一页内的所有参数，擦除该页，然后重新写入有效的参数来实现"写
+ * 1"操作。
  *
- * @param  addr: 要设置的Flash地址
- * @param  data: 要写入的数据 (通常为OTA_AVAL=0x00000000 或
+ * @param  addr: 要设置的 Flash 地址
+ * @param  data: 要写入的数据 (通常为 OTA_AVAL=0x00000000 或
  * OTA_NOT_AVAL=0xFFFFFFFF)
- * @retval FLASH_EEPROM_OK (0) 表示成功, 其他值表示失败
+ * @retval FLASH_EEPROM_OK (0) 表示成功，其他值表示失败
  */
 u8 OTA_SET(u32 addr, u32 data) {
-#if EEP_DEBUG
-	printf("OTA_prepare write%08x at %08x\r\n", data, addr);
-#endif
+	EEP_DEBUG_PRINT("OTA_prepare write%08x at %08x\r\n", data, addr);
 	fmc_state_enum fmc_status;
 	u32 temp_val[OTA_CONFIG_NUM]; // 临时数组，用于存储当前所有配置参数的值
 	volatile u32 *flash_ptr = (volatile u32 *)addr;
@@ -556,17 +784,17 @@ u8 OTA_SET(u32 addr, u32 data) {
 		return FLASH_EEPROM_OK; // 值已正确，直接返回成功
 	}
 
-	// 2. 尝试直接写入数据。这适用于将1修改为0的情况（因为Flash允许1->0）
+	// 2. 尝试直接写入数据。这适用于将 1 修改为 0 的情况（因为 Flash 允许 1->0）
 	if (data == OTA_AVAL) {
 		fmc_status = write_ADDRESS(addr, data);
 		// 如果写入成功，则返回成功
 		if (fmc_status == FMC_READY) {
 			return FLASH_EEPROM_OK;
 		}
-		// 如果写入失败（例如地址已被写为0），则继续执行后续的擦除-重写流程
+		// 如果写入失败（例如地址已被写为 0），则继续执行后续的擦除 - 重写流程
 	}
 
-	// 3. 对于需要将0改回1的操作，必须先擦除整个Flash页
+	// 3. 对于需要将 0 改回 1 的操作，必须先擦除整个 Flash 页
 	flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR;
 	temp_val[0] = *flash_ptr;
 	flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
@@ -580,20 +808,17 @@ u8 OTA_SET(u32 addr, u32 data) {
 	flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR;
 	temp_val[5] = *flash_ptr;
 
-#if EEP_DEBUG
-	printf("OTA_prepare write%08x at %08x\r\n", data, addr);
-	// 修复：完整输出所有6个配置项的值，便于调试
-	printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, "
-		   "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n",
-		   temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4],
-		   temp_val[5]);
-#endif
-	// 4. 擦除包含所有配置参数的Flash页
+	EEP_DEBUG_PRINT("OTA_prepare write%08x at %08x\r\n", data, addr);
+	// 修复：完整输出所有 6 个配置项的值，便于调试
+	EEP_DEBUG_PRINT("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, "
+					"BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n",
+					temp_val[0], temp_val[1], temp_val[2], temp_val[3],
+					temp_val[4], temp_val[5]);
+	// 4. 擦除包含所有配置参数的 Flash 页
 	fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR);
 	if (fmc_status != FMC_READY) {
-#if EEP_DEBUG
-		printf("OTA_SET - Page erase failed! Status: %d\r\n", fmc_status);
-#endif
+		EEP_DEBUG_PRINT("OTA_SET - Page erase failed! Status: %d\r\n",
+						fmc_status);
 		return FLASH_EEPROM_ERASE_ERROR;
 	}
 
@@ -604,18 +829,16 @@ u8 OTA_SET(u32 addr, u32 data) {
 		if (current_addr != addr and temp_val[i] != OTA_NOT_AVAL) {
 			fmc_status = write_ADDRESS(current_addr, temp_val[i]);
 			if (fmc_status != FMC_READY) {
-#if EEP_DEBUG
-				printf("OTA_SET - Write backup value %08X failed at "
-					   "0x%08X!status:%d\r\n",
-					   temp_val[i], current_addr, fmc_status);
-#endif
+				EEP_DEBUG_PRINT("OTA_SET - Write backup value %08X failed at "
+								"0x%08X!status:%d\r\n",
+								temp_val[i], current_addr, fmc_status);
 				return FLASH_EEPROM_WRITE_ERROR;
 			}
 		}
 	}
 
 #if EEP_DEBUG
-	// 3. 对于需要将0改回1的操作，必须先擦除整个Flash页
+	// 3. 对于需要将 0 改回 1 的操作，必须先擦除整个 Flash 页
 	flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR;
 	temp_val[0] = *flash_ptr;
 	flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
@@ -628,11 +851,11 @@ u8 OTA_SET(u32 addr, u32 data) {
 	temp_val[4] = *flash_ptr;
 	flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR;
 	temp_val[5] = *flash_ptr;
-	// 修复：完整输出所有6个配置项的值，便于调试
-	printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, "
-		   "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n",
-		   temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4],
-		   temp_val[5]);
+	// 修复：完整输出所有 6 个配置项的值，便于调试
+	EEP_DEBUG_PRINT("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, "
+					"BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n",
+					temp_val[0], temp_val[1], temp_val[2], temp_val[3],
+					temp_val[4], temp_val[5]);
 #endif
 
 	return FLASH_EEPROM_OK;
@@ -649,7 +872,7 @@ void jumpToBOOT() {
 	for (int i = 0; i < 8; i++) {
 		NVIC->ICER[i] = 0xFFFFFFFF;
 	}
-	// 2. 重置外设（特别是CAN，防止冲突）
+	// 2. 重置外设（特别是 CAN，防止冲突）
 	can_deinit(CAN0);
 
 	// 3. 重置系统时钟到默认状态
@@ -667,7 +890,7 @@ void jumpToBOOT() {
 	// 5. 设置中断向量表偏移到应用程序起始地址
 	SCB->VTOR = BOOTLOADER_START_ADDR;
 
-	// 6. 设置MSP并跳转
+	// 6. 设置 MSP 并跳转
 	uint32_t msp_value = *(__IO uint32_t *)BOOTLOADER_START_ADDR;
 	uint32_t reset_handler = *(__IO uint32_t *)(BOOTLOADER_START_ADDR + 4);
 	typedef void (*AppEntryFunc)(void);
@@ -677,28 +900,26 @@ void jumpToBOOT() {
 	app_entry();
 
 	// 如果程序执行到这里，说明跳转失败
-	printf("Jump to application failed!\r\n");
+	EEP_DEBUG_PRINT("Jump to application failed!\r\n");
 }
 
 /**
- * @brief 将0x08011000地址后0xC000大小区域内的数据搬运到0x08005000地址
+ * @brief 将 0x08011000 地址后 0xC000 大小区域内的数据搬运到 0x08005000 地址
  * @param None
  * @retval fmc_state_enum: 操作状态
  */
 fmc_state_enum move_app_data() {
-	// 源地址: APPLATEST_START_ADDR (0x08011000)
-	// 目标地址: APPBACK_START_ADDR (0x08005000)
-	// 数据大小: APP_MAX_SIZE (0xC000 = 48KB)
+	// 源地址：APPLATEST_START_ADDR (0x08011000)
+	// 目标地址：APPBACK_START_ADDR (0x08005000)
+	// 数据大小：APP_MAX_SIZE (0xC000 = 48KB)
 
 	fmc_state_enum fmc_status = FMC_READY;
-#if OTA_DEBUG
-	printf("move_app_data\r\n");
-#endif
-	// 关闭全局中断以防止Flash操作期间的中断干扰
+	OTA_DEBUG_PRINT("move_app_data\r\n");
+	// 关闭全局中断以防止 Flash 操作期间的中断干扰
 	__disable_irq();
 	fmc_unlock();
 
-	// 首先擦除目标地址的Flash页面
+	// 首先擦除目标地址的 Flash 页面
 	uint32_t num_pages =
 		(APP_MAX_SIZE + FMC_PAGE_SIZE - 1) / FMC_PAGE_SIZE; // 向上取整
 
@@ -710,7 +931,7 @@ fmc_state_enum move_app_data() {
 		if (fmc_status != FMC_READY) {
 			fmc_lock();
 #if OTA_DEBUG
-			printf("move_app_data erase error\r\n");
+			OTA_DEBUG_PRINT("move_app_data erase error\r\n");
 			delay_1ms(1000);
 #endif
 			__enable_irq(); // 重新开启中断
@@ -718,13 +939,13 @@ fmc_state_enum move_app_data() {
 		}
 	}
 
-	// 复制数据 - 按字（32位）进行复制以提高效率
+	// 复制数据 - 按字（32 位）进行复制以提高效率
 	uint32_t offset = 0;
 	uint32_t write_count = 0; // 记录实际写入的字数
 	const uint32_t *src_ptr = (const uint32_t *)APPLATEST_START_ADDR;
 	uint32_t *dst_ptr = (uint32_t *)APPBACK_START_ADDR;
 
-	// 预计算源数据的CRC，避免重复访问内存
+	// 预计算源数据的 CRC，避免重复访问内存
 	crc_deinit();
 	crc_data_register_reset();
 	uint32_t src_crc = crc_block_data_calculate(
@@ -733,7 +954,7 @@ fmc_state_enum move_app_data() {
 	while (offset < APP_MAX_SIZE) {
 		uint32_t src_data = src_ptr[offset / 4];
 
-		// 检查源地址是否为有效的Flash数据（非0xFFFFFFFF表示有数据）
+		// 检查源地址是否为有效的 Flash 数据（非 0xFFFFFFFF 表示有数据）
 		if (src_data != 0xFFFFFFFF) {
 			fmc_unlock();
 			// 清除之前的标志位
@@ -745,7 +966,7 @@ fmc_state_enum move_app_data() {
 			// 检查写入操作是否成功
 			if (fmc_status != FMC_READY) {
 #if OTA_DEBUG
-				printf(
+				OTA_DEBUG_PRINT(
 					"Flash write failed at offset 0x%08X with status: %d\r\n",
 					offset, fmc_status);
 				delay_1ms(1000);
@@ -759,7 +980,7 @@ fmc_state_enum move_app_data() {
 		offset += 4;
 
 		// 每写入一定数量的数据后，喂狗一次
-		if ((offset % 4096) == 0) { // 每4KB喂狗一次
+		if ((offset % 4096) == 0) { // 每 4KB 喂狗一次
 			fwdgt_counter_reload();
 		}
 	}
@@ -768,23 +989,20 @@ fmc_state_enum move_app_data() {
 
 	// 重新开启全局中断
 	__enable_irq();
-	crc_data_register_reset(); // 确保CRC计算从初始状态开始
+	crc_data_register_reset(); // 确保 CRC 计算从初始状态开始
 
-	// 计算目标数据的CRC值
+	// 计算目标数据的 CRC 值
 	uint32_t dst_crc = crc_block_data_calculate((uint32_t *)APPBACK_START_ADDR,
 												APP_MAX_SIZE / 4);
 
-	// 比较CRC值，确保数据完整性
+	// 比较 CRC 值，确保数据完整性
 	if (src_crc != dst_crc) {
-#if OTA_DEBUG
-		printf("APPbackup CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n",
-			   src_crc, dst_crc);
-#endif
-		return (fmc_state_enum)0xFF; // CRC校验失败
+		OTA_DEBUG_PRINT(
+			"APPbackup CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n",
+			src_crc, dst_crc);
+		return (fmc_state_enum)0xFF; // CRC 校验失败
 	}
-#if OTA_DEBUG
-	printf("APPbackup CRC OK\r\n");
-#endif
+	OTA_DEBUG_PRINT("APPbackup CRC OK\r\n");
 	// 设置备份应用可用标志
 	fmc_unlock();
 	OTA_SET(n_APP_BACKUP_AVAL_ADDR, OTA_AVAL);

Robot_V2/sys/flash.h

@@ -24,7 +24,7 @@ class FlashEEPROM {
   public:
 	// EEPROM emulation constants
 	static constexpr u32 EEPROM_START_ADDRESS = 0x0801F800;		// 第一页(1KB)
-	static constexpr u32 EEPROM_DATA_PAGE2_ADDRESS = 0x801FC00; // 第二页(1KB)
+	static constexpr u32 EEPROM_DATA_PAGE2_ADDRESS = 0x0801FC00; // 第二页(1KB)
 	static constexpr u32 EEPROM_END_ADDRESS = 0x08020000;		// Flash结束地址
 	static constexpr u32 EEPROM_MAX_SIZE = 0x800; // EEPROM最大容量
 	static constexpr u32 EEPROM_DATA_START_ADDRESS =
@@ -40,6 +40,7 @@ class FlashEEPROM {
 	static constexpr u8 ADDR_TARGET_VEL = 0x03;	  // 速度命令数据存储地址
 	static constexpr u8 ADDR_VOLTAGE_LIMIT = 0x4; // 电压限制
 	static constexpr u8 ADDR_VEL_LIMIT = 0x05;	  // 角度数据存储地址
+	static constexpr u8 ADDR_NODE_ID = 0x06;	  // 节点 ID 存储地址
 
 	bool angle_saved = false;
 	bool system_config_saved = false;
@@ -66,6 +67,14 @@ class FlashEEPROM {
 	u8 read_saved_system_config(void);
 	float read_angle();
 
+	// 节点 ID 独立读写函数（使用魔数+双重备份+计数器机制）
+	// 从 Flash 读取节点 ID（带魔数验证和双重备份回退）
+	FlashEepromStatus readNodeId(u16 *node_id) const;
+	// 保存节点 ID 到 Flash（写入主备份和备用备份）
+	FlashEepromStatus writeNodeId(u16 node_id);
+	// 擦除节点 ID 存储区（恢复默认值）
+	FlashEepromStatus eraseNodeId();
+
   private:
 	FlashEepromStatus writeCRC(u32 CRC_NUM, bool saved);
 };

Robot_V2/sys/health.cpp

@@ -8,13 +8,17 @@
 
 extern sampler sampler;
 extern CAN_transmiter CAN0_transmiter;
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 extern FlashEEPROM flashEEP;
 ThermalController thermalController; // 热量控制器初始化
 
 #define max(a, b, c)                                                           \
 	((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
 constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
+
+// 电压管理状态机
+static VoltageState voltage_state = VoltageState::NORMAL;
+static float last_vbus_slow = 0.0f;	// 上次电压值（慢速任务用）
 /**
 ** @brief  热能计算
 ** @param  None
@@ -40,7 +44,7 @@ u32 ThermalController::update() {
 			remainingTime10Ms = 1; // 可以恢复工作
 		}
 		if (remainingTime10Ms == 1) { // 过热恢复
-			FOC.resume();
+			FOC.resume_from_thermal();
 			CAN0_transmiter.ThermalStatus = 0;
 			return --remainingTime10Ms;
 		}
@@ -73,7 +77,7 @@ u32 ThermalController::update() {
 	}
 
 	if (thermal_value > thermal_capacity) {
-		FOC.shut_down();
+		FOC.shut_down_thermal();
 		remainingTime10Ms = PROTECT_TIME; // 过热停机
 		CAN0_transmiter.ThermalStatus = 1;
 		return remainingTime10Ms;
@@ -90,15 +94,48 @@ void angsens_MG_check(void) {
 	CAN0_transmiter.MagStatus = MGH or MGL;
 }
 
+/**
+ * @brief 电压管理（低频任务，约10ms调用一次）
+ *
+ * 负责：
+ * 1. 执行Flash保存（阻塞操作，不能在高频循环中执行）
+ * 2. 状态恢复判断（基于基准电压的比例恢复）
+ * 3. 更新FOC的电源电压参数
+ */
 void power_manage(void) {
-	float last_vbus = sampler.get_vbus_voltage();
 	sampler.update_vbus_voltage();
-	FOC.update_power();
 	float vbus = sampler.get_vbus_voltage();
-	if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) {
-		FOC.shut_down();
+
+	// 更新FOC的电源电压参数
+	FOC.update_power(last_vbus_slow);
+	last_vbus_slow = vbus;
+
+	// 如果快速检测已触发紧急关机，执行保存
+	if (FOC.emergency_shutdown) {
 		flashEEP.save_system_config(true);
-	} else {
-		FOC.resume();
+		FOC.emergency_shutdown = false; // 清除紧急关机标志
+		voltage_state = VoltageState::SHUTDOWN_SAVE;
+		return;
+	}
+
+	// 状态机处理
+	switch (voltage_state) {
+	case VoltageState::NORMAL:
+		// 正常状态：无需操作
+		break;
+
+	case VoltageState::SHUTDOWN_SAVE:
+		// 关机保存状态：检查是否可以恢复
+		// 恢复条件：电压恢复到基准电压的90%以上
+		if (FOC.vbus_reference > 0 && vbus > FOC.vbus_reference * VBUS_RECOVERY_RATIO) {
+			if (!FOC.thermal_shutdown) {
+				FOC.resume();
+				voltage_state = VoltageState::NORMAL;
+			}
+		}
+		break;
+
+	default:
+		break;
 	}
 }

Robot_V2/sys/health.h

@@ -10,6 +10,13 @@ typedef int32_t q16_16_t; // Q16.16格式
 #define Q16_16_MUL(a, b) ((q16_16_t)(((int64_t)(a) * (b)) >> 16))
 #define Q16_16_SQUARE(x) Q16_16_MUL((x), (x))
 
+// 电压监测状态机状态
+enum class VoltageState : uint8_t {
+	NORMAL,			// 正常运行
+	VOLTAGE_DROP,	// 电压跌落检测中（防抖等待）
+	SHUTDOWN_SAVE	// 已关机，等待保存
+};
+
 class ThermalController {
   private:
 	u8 temperature;

Robot_V2/sys/initial.cpp

@@ -11,7 +11,21 @@
 #include <cstdio>
 #include <cstring>
 
-extern CAN_transmiter CAN0_transmiter; // CAN初始化
+// OTA 调试宏定义
+#if OTA_DEBUG
+#define OTA_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define OTA_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
+
+// EEP 调试宏定义
+#if EEP_DEBUG
+#define EEP_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define EEP_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
+
+extern CAN_transmiter CAN0_transmiter; // CAN 初始化
 VersionManager Version;
 
 VersionManager::VersionManager() {
@@ -74,11 +88,11 @@ char *VersionManager::versionToString(const VersionInfo &version, char *buffer,
 
 void VersionManager::sendVersionInfoViaCan() const {
 	// 准备发送版本信息 (ID: CANFD_VERSION_ID)
-	// 只使用必要的字节数（24字节）来传输三个版本的信息（每个版本6字节）
+	// 只使用必要的字节数（24 字节）来传输三个版本的信息（每个版本 6 字节）
 	u8 versionData[24] = {0};
 
-	// 将版本信息打包到24字节数组中
-	// 前8字节：软件版本信息 (4个uint16_t，每个占2字节)
+	// 将版本信息打包到 24 字节数组中
+	// 前 8 字节：软件版本信息 (4 个 uint16_t，每个占 2 字节)
 	versionData[0] = softwareVersion.major & 0xFF;		  // 低字节
 	versionData[1] = (softwareVersion.major >> 8) & 0xFF; // 高字节
 	versionData[2] = softwareVersion.minor & 0xFF;		  // 低字节
@@ -88,7 +102,7 @@ void VersionManager::sendVersionInfoViaCan() const {
 	versionData[6] = softwareVersion.build & 0xFF;		  // 低字节
 	versionData[7] = (softwareVersion.build >> 8) & 0xFF; // 高字节
 
-	// 8-15字节：硬件版本信息 (4个uint16_t，每个占2字节)
+	// 8-15 字节：硬件版本信息 (4 个 uint16_t，每个占 2 字节)
 	versionData[8] = hardwareVersion.major & 0xFF;		   // 低字节
 	versionData[9] = (hardwareVersion.major >> 8) & 0xFF;  // 高字节
 	versionData[10] = hardwareVersion.minor & 0xFF;		   // 低字节
@@ -98,7 +112,7 @@ void VersionManager::sendVersionInfoViaCan() const {
 	versionData[14] = hardwareVersion.build & 0xFF;		   // 低字节
 	versionData[15] = (hardwareVersion.build >> 8) & 0xFF; // 高字节
 
-	// 16-23字节：BOOT版本信息 (4个uint16_t，每个占2字节)
+	// 16-23 字节：BOOT 版本信息 (4 个 uint16_t，每个占 2 字节)
 	versionData[16] = bootVersion.major & 0xFF;		   // 低字节
 	versionData[17] = (bootVersion.major >> 8) & 0xFF; // 高字节
 	versionData[18] = bootVersion.minor & 0xFF;		   // 低字节
@@ -108,21 +122,24 @@ void VersionManager::sendVersionInfoViaCan() const {
 	versionData[22] = bootVersion.build & 0xFF;		   // 低字节
 	versionData[23] = (bootVersion.build >> 8) & 0xFF; // 高字节
 
-	// 使用CANFD发送函数发送版本信息
-	CAN0_transmiter.CANFD_AsyncSend(CANFD_VERSION_ID, 24, versionData);
+	// 使用 CANFD 发送函数发送版本信息，使用 build_can_id 构建带 node_id 的 CAN
+	// ID
+	CAN0_transmiter.CANFD_AsyncSend(
+		CAN0_transmiter.build_can_id(CANFD_VERSION_ID), 24, versionData);
 }
 
 void VersionManager::printVersions() const {
 	char versionStr[32];
 
-	printf(
+	OTA_DEBUG_PRINT(
 		"Software Version: %s\r\n",
 		versionToString(getSoftwareVersion(), versionStr, sizeof(versionStr)));
-	printf(
+	OTA_DEBUG_PRINT(
 		"Hardware Version: %s\r\n",
 		versionToString(getHardwareVersion(), versionStr, sizeof(versionStr)));
-	printf("BOOT Version: %s\r\n",
-		   versionToString(getBootVersion(), versionStr, sizeof(versionStr)));
+	OTA_DEBUG_PRINT(
+		"BOOT Version: %s\r\n",
+		versionToString(getBootVersion(), versionStr, sizeof(versionStr)));
 }
 
 /* retarget the C library printf function to the USART */
@@ -154,9 +171,9 @@ int fputc(int ch, FILE *f) throw() {
 }
 
 void unused_pin_init() {
-	rcu_periph_clock_enable(RCU_GPIOA); // 使能GPIOA时钟
-	rcu_periph_clock_enable(RCU_GPIOB); // 使能GPIOB时钟
-	rcu_periph_clock_enable(RCU_GPIOC); // 使能GPIOC时钟
+	rcu_periph_clock_enable(RCU_GPIOA); // 使能 GPIOA 时钟
+	rcu_periph_clock_enable(RCU_GPIOB); // 使能 GPIOB 时钟
+	rcu_periph_clock_enable(RCU_GPIOC); // 使能 GPIOC 时钟
 	gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ,
 			  GPIO_PIN_8 | GPIO_PIN_11 | GPIO_PIN_12);
 	gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ,
@@ -177,43 +194,43 @@ void unused_pin_init() {
 u16 get_reset_cause(void) {
 	u16 cause = 0;
 	if (RESET != rcu_flag_get(RCU_FLAG_FWDGTRST)) {
-		// printf("Free Watchdog Timer Reset\r\n");
+		// OTA_DEBUG_PRINT("Free Watchdog Timer Reset\r\n");
 		cause = RCU_FLAG_FWDGTRST;
 	}
 	if (RESET != rcu_flag_get(RCU_FLAG_EPRST)) {
-		// printf("External Pin Reset\r\n");
+		// OTA_DEBUG_PRINT("External Pin Reset\r\n");
 		cause = RCU_FLAG_EPRST;
 	}
 
 	if (RESET != rcu_flag_get(RCU_FLAG_PORRST)) {
-		// printf("Power Reset\r\n");
+		// OTA_DEBUG_PRINT("Power Reset\r\n");
 		cause = RCU_FLAG_PORRST;
 	}
 
 	if (RESET != rcu_flag_get(RCU_FLAG_WWDGTRST)) {
-		// printf("Window Watchdog Timer Reset\r\n");
+		// OTA_DEBUG_PRINT("Window Watchdog Timer Reset\r\n");
 		cause = RCU_FLAG_WWDGTRST;
 	}
 
 	if (RESET != rcu_flag_get(RCU_FLAG_LPRST)) {
-		// printf("Low Power Reset\r\n");
+		// OTA_DEBUG_PRINT("Low Power Reset\r\n");
 		cause = RCU_FLAG_LPRST;
 	}
 
 	if (RESET != rcu_flag_get(RCU_FLAG_SWRST)) {
-		// printf("Software Reset\r\n");
+		// OTA_DEBUG_PRINT("Software Reset\r\n");
 		cause = RCU_FLAG_SWRST;
 	}
 	// 清除所有复位标志
 	rcu_all_reset_flag_clear();
-	// printf("cause:%d\r\n", cause);
+	// OTA_DEBUG_PRINT("cause:%d\r\n", cause);
 	return cause;
 }
 
 void OTA_initial(void) {
 #if OTA_DEBUG
 	u32 temp_val[6];
-	// 3. 对于需要将0改回1的操作，必须先擦除整个Flash页
+	// 3. 对于需要将 0 改回 1 的操作，必须先擦除整个 Flash 页
 	volatile u32 *flash_ptr1 = (volatile u32 *)n_APP_LATEST_AVAL_ADDR;
 	temp_val[0] = *flash_ptr1;
 	flash_ptr1 = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
@@ -226,20 +243,19 @@ void OTA_initial(void) {
 	temp_val[4] = *flash_ptr1;
 	flash_ptr1 = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR;
 	temp_val[5] = *flash_ptr1;
-	// 修复：完整输出所有6个配置项的值，便于调试
-	printf("APP:CHECK LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, "
-		   "BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n",
-		   temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4],
-		   temp_val[5]);
+	// 修复：完整输出所有 6 个配置项的值，便于调试
+	OTA_DEBUG_PRINT(
+		"APP:CHECK LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, "
+		"BackupEn:0x%x, LatestCheck:0x%x, BackupCheck:0x%x\r\n",
+		temp_val[0], temp_val[1], temp_val[2], temp_val[3], temp_val[4],
+		temp_val[5]);
 #endif
 
-	// 确认当前APP可用
+	// 确认当前 APP 可用
 	volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR;
 	u32 app_check = *flash_ptr;
-	if (app_check == OTA_NOT_AVAL) { // 新的APP刷入后第一次启动
-#if OTA_DEBUG
-		std::printf("APP enabled\r\n");
-#endif
+	if (app_check == OTA_NOT_AVAL) { // 新的 APP 刷入后第一次启动
+		OTA_DEBUG_PRINT("APP enabled\r\n");
 		move_app_data();
 	}
 
@@ -248,7 +264,7 @@ void OTA_initial(void) {
 	fmc_lock();
 
 	u8 transmit_data[2] = {0x0A, 0xAC};
-	// 确认APP可用
+	// 确认 APP 可用
 	CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
 
 	flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
@@ -256,18 +272,14 @@ void OTA_initial(void) {
 	flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR;
 	u32 back_check = *flash_ptr;
 
-	if (back_aval == OTA_NOT_AVAL) { // 如果BACK不可用
-#if OTA_DEBUG
-		std::printf("notaval\r\n");
-#endif
+	if (back_aval == OTA_NOT_AVAL) { // 如果 BACK 不可用
+		OTA_DEBUG_PRINT("notaval\r\n");
 		move_app_data();
 	} else if (back_aval == OTA_AVAL &&
-			   back_check == OTA_NOT_AVAL) { // BACK已刷写，但未检查
-#if OTA_DEBUG
-		std::printf("aval\r\n");
-#endif
+			   back_check == OTA_NOT_AVAL) { // BACK 已刷写，但未检查
+		OTA_DEBUG_PRINT("aval\r\n");
 		move_app_data();
 	} else {
-		printf("check\r\n");
+		EEP_DEBUG_PRINT("check\r\n");
 	}
 }

Robot_V2/sys/systick.cpp

@@ -35,19 +35,34 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "systick.h"
 #include "Can.h"
 #include "config.h"
-#include "core_cmFunc.h"
 #include "flash.h"
 #include "gd32c10x.h"
-#include "gd32c10x_exti.h"
 #include "gd32c10x_fmc.h"
-#include "initial.h"
 #include "motor.h"
 #include <cstdio>
 
+// SLEEP 调试宏定义
+#if SLEEP_DEBUG
+#define SLEEP_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define SLEEP_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
+
 volatile static uint32_t delay;
+volatile static uint32_t systick_ms = 0; // 系统运行时间（毫秒）
 extern CAN_transmiter CAN0_transmiter;
 extern FlashEEPROM flashEEP;
 
+/*!
+	\brief      get system tick time in milliseconds
+	\param[in]  none
+	\param[out] none
+	\retval     system time in ms
+*/
+uint32_t get_systick_ms(void) {
+	return systick_ms;
+}
+
 /*!
 	\brief      configure systick
 	\param[in]  none
@@ -64,6 +79,17 @@ void systick_config(void) {
 	/* configure the systick handler priority */
 	NVIC_SetPriority(SysTick_IRQn, 0x00U);
 }
+
+/*!
+	\brief      SysTick handler - increment millisecond counter
+	\param[in]  none
+	\param[out] none
+	\retval     none
+*/
+extern "C" void SysTick_Handler(void) {
+	systick_ms++;      // 递增系统时间
+	delay_decrement(); // 递减延时计数器
+}
 /*!
 	\brief      delay a time in seconds
 	\param[in]  count: count in seconds
@@ -131,7 +157,7 @@ void app_prepare_deep_sleep(bool save_data) {
 	if (save_data) {
 		flashEEP.save_system_config(true);
 	}
-	// 将SIT1044设置为低功耗待机模式
+	// 将 SIT1044 设置为低功耗待机模式
 	// CAN0_transmiter.CAN_Sleep();
 	motor_sleep();
 	// 在睡眠前调整看门狗超时时间为最大值，使其在唤醒前不会触发
@@ -140,7 +166,7 @@ void app_prepare_deep_sleep(bool save_data) {
 	fwdgt_write_disable();
 
 	__disable_irq();
-	// 遍历最后一个数字，后一个地址写+1
+	// 遍历最后一个数字，后一个地址写 +1
 	for (u32 address = SLEEP_CONFIG_END - SIZE_U32;
 		 address >= SLEEP_CONFIG_START; address -= SIZE_U32) { // 遍历睡眠存储区
 		volatile u32 *flash_ptr = (volatile u32 *)address;
@@ -149,9 +175,7 @@ void app_prepare_deep_sleep(bool save_data) {
 			address > SLEEP_CONFIG_START) { // 空数据且未到头跳过
 			continue;
 		} else { // 有数据的话读取
-#if SLEEP_DEBUG
-			printf("app sleep found %08x at %08x\r\n", sleep_enable, address);
-#endif
+			SLEEP_DEBUG_PRINT("app sleep found %08x at %08x\r\n", sleep_enable, address);
 			if (address == SLEEP_CONFIG_END - SIZE_U32) {
 				fmc_unlock();
 				fmc_state_enum fmc_status =
@@ -164,27 +188,23 @@ void app_prepare_deep_sleep(bool save_data) {
 				address = SLEEP_CONFIG_START - SIZE_U32; // 重新开始写入
 			}
 			address += SIZE_U32;
-#if SLEEP_DEBUG
-			printf("app sleep prepare write %08x at %08x\r\n", SLEEP_ENABLE,
+			SLEEP_DEBUG_PRINT("app sleep prepare write %08x at %08x\r\n", SLEEP_ENABLE,
 				   address);
-#endif
 			fmc_state_enum fmc_status =
 				write_ADDRESS(address, SLEEP_ENABLE); // 写入数据
 #if SLEEP_DEBUG
 			volatile u32 *flash_ptr = (volatile u32 *)address;
 			u32 sleep_check = *flash_ptr;
-			printf("app sleep wrote %08x at %08x\r\n", sleep_check, address);
+			SLEEP_DEBUG_PRINT("app sleep wrote %08x at %08x\r\n", sleep_check, address);
 #endif
 			if (fmc_status != FMC_READY) { // 写入失败报错
 				flashEEP.EEPROMStatus = FLASH_EEPROM_WRITE_ERROR;
-#if SLEEP_DEBUG
-				printf("app wrote %08x at %08x ERROR\r\n", SLEEP_ENABLE,
+				SLEEP_DEBUG_PRINT("app wrote %08x at %08x ERROR\r\n", SLEEP_ENABLE,
 					   address);
-#endif
 				return;
 			}
 			break;
 		}
 	}
-	jumpToBOOT(); // 跳转到bootloader正式休眠，进入低功耗待机模式
+	jumpToBOOT(); // 跳转到 bootloader 正式休眠，进入低功耗待机模式
 }

Robot_V2/sys/systick.h

@@ -42,6 +42,8 @@ extern "C" {
 
 /* configure systick */
 void systick_config(void);
+/* get system tick time in milliseconds */
+uint32_t get_systick_ms(void);
 /* delay a time in milliseconds */
 void delay_1ms(uint32_t count);
 /* delay a time in seconds */

vcpkg-configuration.json

@@ -9,8 +9,8 @@
  "requires": {
   "arm:tools/open-cmsis-pack/cmsis-toolbox": "2.12.0",
   "arm:tools/kitware/cmake": "3.31.5",
-  "arm:tools/ninja-build/ninja": "1.12.1",
   "arm:tools/arm/mdk-toolbox": "1.1.0",
-  "arm:compilers/arm/armclang": "6.24.0"
+  "arm:compilers/arm/armclang": "6.24.0",
+  "arm:tools/ninja-build/ninja": "1.12.1"
  }
 }