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_Controller FOC; // FOC控制初始化
-FOC_Controlroller 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 归一化角度
+void calibrate_zero_position(void) {
- *
+	// Set_3Pwm(.25, 0, 0); // 起劲
- * @param angle
+	// delay_1ms(1000);
- * @return float
+	// KTH7823.update();
- */
+
-inline float _normalizeAngle(float angle) {
+	// // 改进校准算法：获取当前传感器角度并计算补偿值
-	float a = fmod(angle, 360.0f);
+	// float current_sensor_angle = KTH7823.getcurrent_sensorangle();
-	// 取余运算可以用于归一化，列出特殊值例子算便知
+	// float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f); //
-	return a >= 0 ? a : (a + 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,
+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 -
+	// 返回Iq值，乘以1000以转换为mA
-		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以100以转换为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() {
+u8 FOC_Controller::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); // 设置电机角度控制参数
-
 
+	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;
 
-	// 克拉克逆变换 (使用预计算常量优化)
+	// 计算角度并归一化到 [0, 2π)
-	Ua = Ualpha + v_half;
+	float angle = atan2f(Ubeta, Ualpha);
-	Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half;
+	if (angle < 0.0f)
-	Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
+		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() {
+void FOC_Controller::shut_down() {
-	is_shut_down = 1;
+	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) {
+void FOC_Controller::update_power(float voltage) {
-	voltage_power_supply = sampler.get_vbus_voltage();
+	if (voltage < 36.0f and voltage > 8.0f) {
-	if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) {
 		voltage_error = false;
-		v_half = voltage_power_supply * 0.5f;
+		voltage_power_supply = voltage;
-		v_safe = voltage_power_supply * 0.95f;
+		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 *
+	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
-													REDUCTION_RATIO * 1.0 /
+										 MOTOR_FREQUENCY / 2,
-													TIMER2_FREQUENCY);
+					   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;
-
+	setSVPWM_PhaseVoltage(Uq, 0.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'));
-
 	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);
+	setSVPWM_PhaseVoltage(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);
-
 	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() {
+float FOC_Controller::velocity_control() {
-	// PID 控制计算
+	// PID 控制计算，使用 DIR 参数调整速度误差的符号
-	float Uq = vel_loop((rt_velocity - target_velocity) * 25);
+	// 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);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
-	CAN0_transmiter.testfloat_union.f =
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-		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'),
+	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{
+	float PWM_out(void);
-		vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
+	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,
+		PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
-					  voltage_power_supply}; // 12V
+					  10000000, voltage_power_supply}; // 12V
 	PIDController current_loop = PIDController{
-		current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V
+		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
-	PIDController vel_inVelAngloop = PIDController{
+		current_loop_Kd, 1500, voltage_power_supply}; // 12V
-		vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply};
+	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 angle_now = kalman.getState();
-	float vel = (kalman.getState() - last_angle) * TIMER2_FREQUENCY *
+	u32 time_counts = motor_runtime_50us - velocity_last_us50_counter;
-				(motor_runtime_50us - last_us50_counter);
+
-	last_us50_counter = motor_runtime_50us;
+	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; // 更新误差协方差
+		
+		// 添加协方差下限保护，防止数值不稳定
+		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_ISQ_ISQN << (5U * ((3U + rank) - inserted_length))));
-    isq |= ((uint32_t)adc_channel << (5U * ((3 + 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,10 +747,9 @@ 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) {
+    /* got one empty mailbox */
-        return CAN_NOMAILBOX;
+    if(CAN_NOMAILBOX != mailbox_number) {
-    }
 
         CAN_TMI(can_periph, mailbox_number) &= CAN_TMI_TEN;
         if(CAN_FF_STANDARD == transmit_message->tx_ff) {
@@ -830,7 +829,7 @@ uint8_t can_message_transmit(uint32_t can_periph, can_trasnmit_message_struct *t
         
         /* 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) {
+        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)) {
+            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) {
+        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)) {
+            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) {
+        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)) {
+            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) {
+    if(0U != flag_status) {
-        return ERROR;
+        status = ERROR;
     }
 
-    /* configure FWDGT */
+    return status;
-    FWDGT_PSC = (uint32_t)prescaler_value;
-
-    return SUCCESS;
 }
 
 /*!
@@ -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) {
+    if(0U != flag_status) {
-        return ERROR;
+        status = ERROR;
     }
 
-    FWDGT_RLD = RLD_RLD(reload_value);
+    return status;
-
-    return SUCCESS;
 }
 
 /*!
@@ -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) {
+    if(0U != flag_status) {
-        return ERROR;
+        status = ERROR;
-    }
-
-    /* configure FWDGT */
-    FWDGT_PSC = (uint32_t)prescaler_div;
-
-    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));
-
-    if((uint32_t)RESET != flag_status) {
-        return ERROR;
     }
 
+    if(SUCCESS == status) {
+        /* configure FWDGT_RLD */
         FWDGT_RLD = RLD_RLD(reload_value);
 
+        /* 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(0U != flag_status) {
+            status = ERROR;
+        }
+    }
+
+    if(SUCCESS == status) {
         /* 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) {
+    FlagStatus flag_status = RESET;
-        return SET;
-    }
 
-    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 */
+    /* check current priority group */
-    switch((SCB->AIRCR) & (uint32_t)0x700U) {
+    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 */
+    /* get the priority group value */
-    temp_priority = (uint32_t)nvic_irq_pre_priority << (0x4U - temp_pre);
+    nvic_prigroup = NVIC_GetPriorityGrouping();
-    temp_priority |= nvic_irq_sub_priority & (0x0FU >> (0x4U - temp_sub));
+
-    temp_priority = temp_priority << 0x04U;
+    /* encoding the pre-emption, subpriority priority */
-    NVIC->IP[nvic_irq] = (uint8_t)temp_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.cpp

@@ -18,22 +18,237 @@
 #include <cstring>
 #include <random>
 
-constexpr u16 CANX = CAN0;
+// CAN 调试宏定义
+#if CAN_DEBUG
+#define CAN_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define CAN_DEBUG_PRINT(fmt, ...) ((void)0)
+#endif
 
-CAN_transmiter CAN0_transmiter; // CAN初始化
+constexpr u16 CANX = CAN0;
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
 extern EncoderAngleKalman Angle_kalman;	 // 角度滤波
 extern EncoderVelocityKalman Vel_kalman; // 速度滤波
 extern FlashEEPROM flashEEP;
 extern VersionManager Version; // 声明外部版本管理器实例
+extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式计时）
 
 // 添加 DMA 相关变量
 dma_parameter_struct dma_rx_parameter;
 dma_parameter_struct dma_tx_parameter;
-uint8_t can_rx_buffer[8]; // CAN接收缓冲区
+uint8_t can_rx_buffer[64]; // CAN 接收缓冲区（支持CAN FD最大64字节）
-uint8_t can_tx_buffer[8]; // CAN发送缓冲区
+uint8_t can_tx_buffer[64]; // CAN 发送缓冲区（支持CAN FD最大64字节）
+
+// CAN发送队列结构
+#define CAN_TX_QUEUE_SIZE 8
+typedef struct {
+    u32 id;
+    u8 length;
+    u8 data[64];
+    u8 valid;
+} can_tx_queue_item_t;
+
+static can_tx_queue_item_t tx_queue[CAN_TX_QUEUE_SIZE];
+static volatile u8 tx_queue_head = 0;
+static volatile u8 tx_queue_tail = 0;
+static volatile u8 tx_queue_count = 0;
+
+// 发送队列使用的独立消息结构体
+static can_trasnmit_message_struct queue_transmit_message;
+
+CAN_transmiter CAN0_transmiter; // CAN 初始化
+
+// ============================================================================
+// 节点 ID 相关函数实现 (多电机 CAN 总线隔离)
+// 使用高 16 位 (bits 31-16) 存储节点 ID，低 16 位 (bits 15-0) 存储基础 ID
+// 格式：[节点 ID(16 位)][基础 ID(16 位)]
+// 例如：node_id=1, base_id=0x0200 → 0x00010200
+// ============================================================================
+
+/*!
+	\brief 根据节点 ID 构建 CAN ID
+	\param[in]  base_id 基础 CAN ID(低 16 位)
+	\param[out] none
+	\retval     构建后的完整 CAN ID
+	\note       将节点 ID 放在高 16 位，基础 ID 放在低 16 位
+				格式：[节点 ID(16 位)][基础 ID(16 位)]
+				例如：node_id=1, base_id=0x0200 → 0x00010200
+*/
+u32 CAN_transmiter::build_can_id(u32 base_id) {
+	return ((u32)node_id << 16) | (base_id & 0xFFFF);
+}
+
+/*!
+	\brief 判断接收到的消息是否是发给本节点的
+	\param[in]  received_id 接收到的 CAN ID
+	\param[out] none
+	\retval     true: 消息是发给本节点的 (或广播消息)
+				false: 消息不是发给本节点的
+	\note       支持广播地址 (0xFFFF)，广播消息所有节点都会响应
+				提取高 16 位进行节点 ID 匹配
+*/
+bool CAN_transmiter::is_message_for_me(u32 received_id) {
+	u16 rx_node_id = (u16)((received_id >> 16) & 0xFFFF);
+	return (rx_node_id == BROADCAST_NODE_ID) || (rx_node_id == node_id);
+}
+
+/*!
+	\brief 获取当前节点 ID
+	\param[in]  none
+	\param[out] none
+	\retval     当前节点 ID 值 (0-65534)
+*/
+u16 CAN_transmiter::get_node_id() { return node_id; }
+
+/*!
+	\brief 设置节点 ID
+	\param[in]  new_id 新的节点 ID 值
+	\param[out] none
+	\retval     none
+	\note       节点 ID 范围：0-65534，0xFFFF 保留为广播地址
+*/
+void CAN_transmiter::set_node_id(u16 new_id) {
+	if (new_id <= MAX_NODE_ID) {
+		node_id = new_id;
+		CAN_DEBUG_PRINT("Node ID set to: %d\r\n", node_id);
+	}
+}
+
+/*!
+	\brief 从 Flash 加载节点 ID（使用魔数+双重备份+计数器机制）
+	\param[in]  none
+	\param[out] none
+	\retval     FLASH_EEPROM_OK: 加载成功
+				其他：加载失败，使用默认值
+	\note       使用独立的节点 ID 存储区域，带魔数验证和 CRC 校验
+				防止位翻转导致错读
+*/
+u8 CAN_transmiter::load_node_id_from_flash() {
+	u16 node_id_temp = 0;
+	FlashEepromStatus status = flashEEP.readNodeId(&node_id_temp);
+
+	// 检查读取是否成功
+	if (status != FLASH_EEPROM_OK) {
+		node_id = DEFAULT_NODE_ID;
+		CAN_DEBUG_PRINT(
+			"Node ID not found in Flash (status=%d), using default: %d\r\n",
+			status, node_id);
+		return FLASH_EEPROM_DATA_NOT_FOUND;
+	}
+
+	// 检查节点 ID 是否有效 (0-65534 都是有效值)
+	if (node_id_temp <= MAX_NODE_ID) {
+		node_id = node_id_temp;
+		CAN_DEBUG_PRINT("Node ID loaded from Flash: %d\r\n", node_id);
+		return FLASH_EEPROM_OK;
+	} else {
+		node_id = DEFAULT_NODE_ID;
+		CAN_DEBUG_PRINT("Invalid Node ID in Flash (%d), using default: %d\r\n",
+						node_id_temp, node_id);
+		return FLASH_EEPROM_DATA_NOT_FOUND;
+	}
+}
+
+/*!
+	\brief 保存节点 ID 到 Flash（使用魔数+双重备份+计数器机制）
+	\param[in]  new_id 要保存的节点 ID
+	\param[out] none
+	\retval     FLASH_EEPROM_OK: 保存成功
+				其他：保存失败
+	\note       写入主备份和备用备份，确保数据可靠性
+*/
+u8 CAN_transmiter::save_node_id_to_flash(u16 new_id) {
+	if (new_id > MAX_NODE_ID) {
+		CAN_DEBUG_PRINT("Invalid Node ID: %d\r\n", new_id);
+		return FLASH_EEPROM_INVALID_ADDR;
+	}
+	
+	FlashEepromStatus status = flashEEP.writeNodeId(new_id);
+	if (status == FLASH_EEPROM_OK) {
+		node_id = new_id;
+		CAN_DEBUG_PRINT("Node ID saved to Flash: %d\r\n", node_id);
+		return FLASH_EEPROM_OK;
+	} else {
+		CAN_DEBUG_PRINT("Failed to save Node ID to Flash (status=%d)\r\n", status);
+		return (u8)status;
+	}
+}
+
+// CAN 接收缓冲区相关函数实现
+/*!
+	\brief      检查 CAN 接收缓冲区是否已满
+	\param[in]  none
+	\param[out] none
+	\retval     bool 返回 true 表示缓冲区已满，false 表示未满
+	\note       通过比较头指针的下一个位置和尾指针来判断缓冲区是否已满
+				使用循环缓冲区策略，当(rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE
+   等于 rx_buffer_tail 时， 表示缓冲区已达到最大容量
+*/
+bool CAN_transmiter::is_rx_buffer_full() {
+	return ((rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE) == rx_buffer_tail;
+}
+
+/*!
+	\brief      检查 CAN 接收缓冲区是否为空
+	\param[in]  none
+	\param[out] none
+	\retval     bool 返回 true 表示缓冲区为空，false 表示缓冲区不为空
+	\note       通过比较头指针和尾指针是否相等来判断缓冲区是否为空
+*/
+bool CAN_transmiter::is_rx_buffer_empty() {
+	return rx_buffer_head == rx_buffer_tail;
+}
+
+/**
+ * @brief 从接收缓冲区中取出一条 CAN 消息
+ *
+ * 该函数检查接收缓冲区是否为空，如果不为空则取出缓冲区队列头部的消息，
+ * 并更新队列尾部索引。如果缓冲区为空，则返回 false。
+ *
+ * @param message 引用类型的输出参数，用于存储取出的 CAN 接收消息结构体
+ * @return bool 返回操作是否成功，true 表示成功取出消息，false 表示缓冲区为空
+ */
+bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) {
+	if (!is_rx_buffer_empty()) {
+		message = rx_message_buffer[rx_buffer_tail];
+		rx_buffer_tail = (rx_buffer_tail + 1) % CAN_RX_BUFFER_SIZE;
+		return true;
+	}
+	return false;
+}
+
+// 静默模式相关函数实现
+// 使用 systick 获取系统时间（ms）
+extern u32 get_systick_ms(void);
+
+/*!
+	\brief 进入静默模式（停发 10 秒）
+*/
+void CAN_transmiter::enter_silent_mode() {
+	is_silent = true;
+	silent_start_time = get_systick_ms(); // 记录当前时间（ms）
+	CAN_DEBUG_PRINT("Enter silent mode for %d ms, start_time=%d\r\n", CAN_SILENT_DURATION_MS, silent_start_time);
+}
+
+/*!
+	\brief 检查是否在静默模式中
+*/
+bool CAN_transmiter::is_in_silent_mode() { return is_silent; }
+
+/*!
+	\brief 检查静默模式是否结束
+*/
+void CAN_transmiter::check_silent_mode() {
+	if (is_silent) {
+		u32 current_time = get_systick_ms(); // 当前时间（ms）
+		if (current_time - silent_start_time >= CAN_SILENT_DURATION_MS) {
+			is_silent = false;
+			CAN_DEBUG_PRINT("Exit silent mode, elapsed=%d ms\r\n", current_time - silent_start_time);
+		}
+	}
+}
 
 /*!
 	\brief      initialize CAN GPIO
@@ -137,7 +352,7 @@ void can_config(void) {
 	/* initialize CAN */
 	if (can_init(CAN0, &can_parameter) != SUCCESS) {
 		while (1)
-			printf("can init error \r\n");
+			CAN_DEBUG_PRINT("can init error \r\n");
 	}
 
 	/* initialize filter */
@@ -273,7 +488,7 @@ void can_loopback_init(void) {
 		u16 i = 200;
 		while (--i > 0) {
 			delay_1ms(10);
-			printf("can init error \r\n");
+			CAN_DEBUG_PRINT("can init error \r\n");
 		}
 		system_soft_reset();
 	}
@@ -325,7 +540,7 @@ void canfd_loopback_test(void) {
 	int i = 0;
 	while (i++ < transmit_message.tx_dlen) {
 		transmit_message.tx_data[i] = dis(gen);
-		std::printf("%x\n", transmit_message.tx_data[i]);
+		CAN_DEBUG_PRINT("%x\n", transmit_message.tx_data[i]);
 	}
 
 	/* transmit message */
@@ -349,10 +564,10 @@ void canfd_loopback_test(void) {
 		(transmit_message.tx_ff == receive_message.rx_ff) &&
 		(transmit_message.tx_dlen == receive_message.rx_dlen)) {
 		/* loopback test is success */
-		std::printf("CAN LO0PBACK SUCCESS\r\n");
+		CAN_DEBUG_PRINT("CAN LO0PBACK SUCCESS\r\n");
 	} else {
 		/* loopback test is failed */
-		std::printf("CAN LO0PBACK FAIL\r\n");
+		CAN_DEBUG_PRINT("CAN LO0PBACK FAIL\r\n");
 	}
 }
 
@@ -387,7 +602,8 @@ void can_loopback_test(void) {
 	/* waiting for transmit completed */
 	while ((CAN_TRANSMIT_OK != can_transmit_states(CANX, transmit_mailbox)) &&
 		   (0 != timeout)) {
-		printf("sending:%x...\n", can_transmit_states(CANX, transmit_mailbox));
+		CAN_DEBUG_PRINT("sending:%x...\n",
+						can_transmit_states(CANX, transmit_mailbox));
 		timeout--;
 	}
 	timeout = 0xFF;
@@ -404,19 +620,20 @@ void can_loopback_test(void) {
 		(transmit_message.tx_dlen == receive_message.rx_dlen)) {
 		i = transmit_message.tx_dlen;
 		while (i-- > 0) {
-			std::printf("rx %d:%x:%x\n", i, transmit_message.tx_data[i],
+			CAN_DEBUG_PRINT("rx %d:%x:%x\n", i, transmit_message.tx_data[i],
 							receive_message.rx_data[i]);
 		}
 		if (std::memcmp(receive_message.rx_data, transmit_message.tx_data,
 						transmit_message.tx_dlen) == 0) {
 			/* loopback test is success */
-			std::printf("loopback success\n");
+			CAN_DEBUG_PRINT("loopback success\n");
 		} else {
-			std::printf("loopback failed\n");
+			/* loopback test is failed */
+			CAN_DEBUG_PRINT("loopback failed\n");
 		}
 	} else {
 		/* loopback test is failed */
-		std::printf("loopback failed\n");
+		CAN_DEBUG_PRINT("loopback failed\n");
 	}
 }
 
@@ -437,6 +654,12 @@ u8 CAN_transmiter::init() {
 	// 初始化 DMA 缓冲区
 	memset(can_rx_buffer, 0, sizeof(can_rx_buffer));
 	memset(can_tx_buffer, 0, sizeof(can_tx_buffer));
+
+	// 初始化节点 ID 为默认值
+	node_id = DEFAULT_NODE_ID;
+
+	// 从 Flash 加载节点 ID (如果 Flash 中有有效值则覆盖默认值)
+	load_node_id_from_flash();
 	return 0;
 }
 
@@ -447,13 +670,15 @@ void CAN_transmiter::update(void) {
 	target_angle_union.f = FOC.target_angle;
 	target_velocity_union.f = FOC.target_velocity;
 	target_current_union.f = FOC.target_current;
-	limit_velocity_union.f = FOC.limit_velocity;
+
+	rt_angle_union.f = FOC.rt_angle;
+	rt_velocity_union.f = FOC.rt_velocity;
+	rt_current_union.f = FOC.rt_current;
+
 	limit_voltage = FOC.limit_voltage * 1000;
 	mcu_temp = sampler.get_mcu_temp() * 100;
 	motor_temp = sampler.get_drv_temp() * 100;
 	Power_Voltage = sampler.get_vbus_voltage() * 1000;
-	rt_angle_union.f = FOC.rt_angle;
-	rt_velocity_union.f = FOC.rt_velocity;
 	MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET;
 	EEPROMStatus = flashEEP.EEPROMStatus;
 	Target_Control_Mode = FOC.control_mode;
@@ -465,63 +690,18 @@ void CAN_transmiter::update(void) {
 	v_current_union.f = sampler.get_current('V') * 1000;
 }
 
-/**
- * @brief  CAN发送函数
- * @retval 0: 发送失败
- *         1: 发送成功
- */
-u8 CAN_transmiter::CAN_all_Send() {
-	u32 success_flag = 0;
-
-	constexpr u32 RT_Vel_Ang_FEED = 0x101;
-	constexpr u32 SET_Vel_Ang_FEED = 0x110;
-	constexpr u32 Current_FEED = 0x117;
-	constexpr u32 Status_FEED = 0x118;
-	constexpr u32 Limit_Vel_Vol_FEED = 0x119;
-
-	// 打包RT_Vel_Ang_FEED - 只清零实际使用的8字节
-	memset(transmit_data, 0, 8);
-	pack_float_to_bytes(transmit_data, 0, rt_angle_union);
-	pack_float_to_bytes(transmit_data, 4, rt_velocity_union);
-	success_flag += CAN_AsyncSend(RT_Vel_Ang_FEED, 8, transmit_data);
-
-	// 打包SET_Vel_Ang_FEED - 只清零实际使用的8字节
-	memset(transmit_data, 0, 8);
-	pack_float_to_bytes(transmit_data, 0, target_angle_union);
-	pack_float_to_bytes(transmit_data, 4, target_velocity_union);
-	success_flag += CAN_AsyncSend(SET_Vel_Ang_FEED, 8, transmit_data);
-
-	// 打包Current_FEED - 只清零实际使用的6字节
-	memset(transmit_data, 0, 6);
-	pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f));
-	pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f));
-	pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f));
-	success_flag += CAN_AsyncSend(Current_FEED, 6, transmit_data);
-
-	// 打包Status_FEED - 只清零实际使用的8字节
-	memset(transmit_data, 0, 8);
-	transmit_data[0] = MotorStatus;
-	transmit_data[1] = controller_Mode;
-	pack_u16_to_bytes(transmit_data, 2, motor_temp);
-	pack_u16_to_bytes(transmit_data, 4, mcu_temp);
-	pack_u16_to_bytes(transmit_data, 6, Power_Voltage);
-	success_flag += CAN_AsyncSend(Status_FEED, 8, transmit_data);
-
-	// 打包Limit_Vel_Vol_FEED - 只清零实际使用的6字节
-	memset(transmit_data, 0, 6);
-	pack_float_to_bytes(transmit_data, 0, limit_velocity_union);
-	pack_u16_to_bytes(transmit_data, 4, limit_voltage);
-	success_flag += CAN_AsyncSend(Limit_Vel_Vol_FEED, 6, transmit_data);
-
-	return success_flag;
-}
-
 /**
  * @brief  CANFD 发送函数
- * @retval 0: 发送失败
+ * @retval 0: 发送失败（或处于静默模式）
  *         1: 发送成功
  */
 u8 CAN_transmiter::CANFD_all_Send() {
+	// 检查是否在静默模式中，如果是则不发送
+	if (is_in_silent_mode()) {
+		check_silent_mode(); // 检查是否已退出静默
+		return 0;
+	}
+
 	u32 success_flag = 0;
 
 	// 打包 status 数据 - 只清零实际使用的 32 字节
@@ -538,17 +718,23 @@ u8 CAN_transmiter::CANFD_all_Send() {
 	pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
 	pack_float_to_bytes(transmit_data, 13, rt_angle_union);
 	pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
-	pack_float_to_bytes(transmit_data, 21, testfloat_union);
+	pack_float_to_bytes(transmit_data, 21, rt_current_union);
-	success_flag += CANFD_AsyncSend(CANFD_STATUS_ID, 32, transmit_data);
+	pack_float_to_bytes(transmit_data, 25, testfloat_union);
+	pack_u16_to_bytes(transmit_data, 29, test_u16);
+	// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
+	success_flag +=
+		CANFD_AsyncSend(build_can_id(CANFD_STATUS_ID), 32, transmit_data);
 
-	// 打包config数据 - 只清零实际使用的16字节
+	// 打包 config 数据 - 只清零实际使用的 24 字节
-	memset(transmit_data, 0, 16);
+	memset(transmit_data, 0, 24);
 	transmit_data[0] = controller_Mode;
 	pack_u16_to_bytes(transmit_data, 1, limit_voltage);
-	pack_float_to_bytes(transmit_data, 3, limit_velocity_union);
+	pack_float_to_bytes(transmit_data, 3, target_current_union);
-	pack_float_to_bytes(transmit_data, 7, target_angle_union);
+	pack_float_to_bytes(transmit_data, 7, target_velocity_union);
-	pack_float_to_bytes(transmit_data, 11, target_velocity_union);
+	pack_float_to_bytes(transmit_data, 11, target_angle_union);
-	success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 16, transmit_data);
+	// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
+	success_flag +=
+		CANFD_AsyncSend(build_can_id(CANFD_CONFIG_ID), 24, transmit_data);
 
 	return success_flag;
 }
@@ -561,56 +747,165 @@ u8 CAN_transmiter::CANFD_all_Send() {
 void CAN_transmiter::communication_check() {
 	// CAN0 发送数据标志
 	if (SET == can0_send_flag) {
-		// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
+		// 检查是否在静默模式中
+		if (!is_in_silent_mode()) {
 			update();
-		// CAN0_transmiter.CAN_all_Send();
 			CANFD_all_Send();
+		} else {
+			check_silent_mode(); // 检查静默是否结束
+		}
 		can0_send_flag = RESET;
 	}
 
-	// CAN0 接收数据标志
+	// 处理所有接收到的 CAN 消息（从环形缓冲区中）
-	if (SET == can0_receive_flag) {
+	can_receive_message_struct current_message;
-		// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
+	while (dequeue_rx_message(current_message)) {
-		// for (i = 0; i < receive_message.rx_dlen; i++) {
+		// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
-		// 	printf(" %02x", receive_message.rx_data[i]);
+		u32 base_id = current_message.rx_efid & 0xFFFF;
-		// }
+		
-		if (receive_message.rx_efid == CANFD_CMD_ID) {
+		// 提取发送方的节点 ID
+		u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF);
+
+		// 特殊处理 base_id=0x0000（BOOT 命令）：即使是其他节点的广播消息也要处理
+		if (base_id == CANFD_BOOT_ID) {
+			// 判断是广播消息还是其他节点的命令
+			if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) {
+				// 广播消息或其他节点的命令 → 进入静默模式（停发 10 秒）
+				CAN_DEBUG_PRINT("Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n", sender_node_id);
+				enter_silent_mode();
+			} else {
+				// 自己节点的命令 → 执行 BOOT 功能
+				CAN_DEBUG_PRINT("Received base_id=0x0000 from self, jumping to BOOT\r\n");
+				motor_sleep();
+				flashEEP.save_system_config(false);
+				fmc_unlock();
+				OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置 APP 为不激活
+				fmc_lock();
+				jumpToBOOT();
+			}
+			continue; // BOOT 命令处理完跳过后续逻辑
+		}
+
+		// 设置节点 ID 命令 (基础 ID: 0x0400)
+		// 注意：此命令在 is_message_for_me 检查之前处理，所有节点都会响应
+		// 这样即使节点ID已经改变，仍然可以用旧节点ID或广播ID来修改
+		else if (base_id == CANFD_SET_NODE_ID) {
+			// 打印接收到的原始数据
+			CAN_DEBUG_PRINT(
+				"SET_NODE_ID: rx_dlen=%d, data[0]=%02X data[1]=%02X\r\n",
+				current_message.rx_dlen, current_message.rx_data[0],
+				current_message.rx_data[1]);
+			// 解析新节点 ID - 支持 1 字节或 2 字节格式
+			u16 new_node_id;
+			if (current_message.rx_dlen >= 2) {
+				new_node_id = unpack_bytes_to_u16(current_message.rx_data, 0);
+			} else {
+				new_node_id =
+					current_message.rx_data[0]; // 只发送 1 字节时直接使用
+			}
+			u16 old_node_id = node_id; // 保存旧节点 ID 用于构建响应 ID
+			CAN_DEBUG_PRINT(
+				"SET_NODE_ID: recv new_id=%d (0x%04X), old_id=%d\r\n",
+				new_node_id, new_node_id, old_node_id);
+
+			if (new_node_id <= MAX_NODE_ID && new_node_id != BROADCAST_NODE_ID) {
+				// 先发送响应（使用旧节点ID构建CAN ID）
+				// 响应数据格式：[status:1byte][new_node_id:2byte] = 3 字节
+				transmit_data[0] = 0x01;					 // 成功
+				transmit_data[1] = (u8)(new_node_id & 0xFF); // 新 ID 低字节
+				transmit_data[2] =
+					(u8)((new_node_id >> 8) & 0xFF); // 新 ID 高字节
+				
+				// 使用旧节点 ID 发送响应
+				node_id = old_node_id;
+				u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
+											   transmit_data);
+				
+				// 然后保存新节点 ID 到 Flash
+				u8 save_status = save_node_id_to_flash(new_node_id);
+				
+				CAN_DEBUG_PRINT("SET_NODE_ID: save_status=%d, send_result=%d\r\n",
+								save_status, send_result);
+				
+				if (save_status == FLASH_EEPROM_OK) {
+					CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n",
+									old_node_id, new_node_id);
+				} else {
+					CAN_DEBUG_PRINT("SET_NODE_ID: Flash write FAILED, status=%d\r\n",
+									save_status);
+					// 发送错误响应
+					transmit_data[0] = 0x00; // 失败
+					node_id = old_node_id;
+					CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
+									transmit_data);
+				}
+			} else {
+				// 发送错误响应 - 使用旧节点 ID 构建响应 ID
+				CAN_DEBUG_PRINT("SET_NODE_ID: invalid new_id=%d (max=%d)\r\n",
+								new_node_id, MAX_NODE_ID);
+				transmit_data[0] = 0x00; // 失败
+				transmit_data[1] = (u8)(new_node_id & 0xFF);
+				transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF);
+				node_id = old_node_id; // 临时恢复旧 ID 用于构建响应
+				CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
+								transmit_data);
+				// 保持原 ID 不变
+			}
+			continue; // SET_NODE_ID 命令处理完跳过后续逻辑
+		}
+
+		// 其他 base_id 的消息，检查是否是发给本节点的
+		if (!is_message_for_me(current_message.rx_efid)) {
+			continue; // 不是发给本节点的消息，跳过
+		}
+
+		if (base_id == CANFD_CMD_ID) {
 			// 控制模式
-			FOC.control_mode = receive_message.rx_data[0];
+			FOC.control_mode = current_message.rx_data[0];
-			// 目标角度
+			// 目标电压限制
 			FOC.limit_voltage =
-				float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
+				float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
-			// 目标电流&目标速度
+			// 目标电流
-			FOC.limit_velocity =
+			target_current_union.f =
-				unpack_bytes_to_float(receive_message.rx_data, 3).f;
+				unpack_bytes_to_float(current_message.rx_data, 3).f;
-			FOC.target_current = FOC.limit_velocity;
+			FOC.current_loop.set_limit(target_current_union.f);
-			// 目标角度
+			FOC.vel_inMITloop.set_limit(target_current_union.f);
-			target_angle_union.f =
-				unpack_bytes_to_float(receive_message.rx_data, 7).f;
-			FOC.target_angle = target_angle_union.f;
 			// 目标速度
 			target_velocity_union.f =
-				unpack_bytes_to_float(receive_message.rx_data, 11).f;
+				unpack_bytes_to_float(current_message.rx_data, 7).f;
+			FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
+			FOC.ang_inMITloop.set_limit(target_velocity_union.f);
+			// 目标角度
+			target_angle_union.f =
+				unpack_bytes_to_float(current_message.rx_data, 11).f;
+			// 赋值
+			FOC.target_angle = target_angle_union.f;
 			FOC.target_velocity = target_velocity_union.f;
-			FOC.target_current = FOC.target_velocity * 10.0f;
+			FOC.target_current = target_current_union.f;
 
 			// 解析 PID 参数
-			u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15);
 			float voltage = FOC.limit_voltage;
-			FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage);
+			// PID 参数
-			P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
+			u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
-			FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage);
+			FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
-			P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
+			FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
-			FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage);
+
-			P_config = unpack_bytes_to_u16(receive_message.rx_data, 21);
+			P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
-			FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config /
+			FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
-											   voltage);
+			FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
-		} else if (receive_message.rx_efid == CANFD_RESET_ID) {
+			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
-			u8 first_byte = receive_message.rx_data[0];
+
-			u8 eigth_byte = receive_message.rx_data[7];
+			P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
+			FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
+			FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
+			FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
+		} else if (base_id == CANFD_RESET_ID) {
+			u8 first_byte = current_message.rx_data[0];
+			u8 eigth_byte = current_message.rx_data[7];
 			// 电机重置
 			if (first_byte & 0x01) {
 				motor_reset();
+				testfloat_union.f = KTH7823.getcurrent_sensorangle();
 			}
 			// EEPROM 重置
 			if (first_byte >> 1 & 0x01) {
@@ -631,16 +926,10 @@ void CAN_transmiter::communication_check() {
 				app_prepare_deep_sleep(true);
 				// CAN_Sleep();
 			}
-		} else if (receive_message.rx_efid == CANFD_VERSION_ID) {
+		} else if (base_id == CANFD_VERSION_ID) {
 			// 处理版本号请求，发送当前系统版本信息
 			Version.sendVersionInfoViaCan();
-		} else if (receive_message.rx_efid == CANFD_BOOT_ID) {
+		} else if (base_id == CANFD_OTA_ACK_ID) {
-			flashEEP.save_system_config(false);
-			fmc_unlock();
-			OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活
-			fmc_lock();
-			jumpToBOOT();
-		} else if (receive_message.rx_efid == 0x00000102) {
 			volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
 			u32 app_enable = *flash_ptr;
 			flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR;
@@ -649,12 +938,18 @@ void CAN_transmiter::communication_check() {
 			transmit_data[1] = 0xAC;
 			delay_1ms(1);
 			if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
-				CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
+				CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2,
+								transmit_data);
 			}
+		}
+		// 查询节点 ID 命令 (基础 ID: 0x0402)
+		else if (base_id == CANFD_GET_NODE_ID) {
+			pack_u16_to_bytes(transmit_data, 0, get_node_id());
+			CANFD_AsyncSend(build_can_id(CANFD_GET_NODE_ID), 2, transmit_data);
 		} else {
-			printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
+			CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n",
+							current_message.rx_efid, node_id);
 		}
-		can0_receive_flag = RESET;
 	}
 }
 
@@ -675,7 +970,7 @@ u8 CAN_transmiter::CAN_Sleep() {
  * @retval None
  */
 u8 CAN_transmiter::CAN_Wakeup() {
-	printf("can wake\r\n");
+	CAN_DEBUG_PRINT("can wake\r\n");
 	is_sleep = false;
 	canfd_config();
 
@@ -683,7 +978,7 @@ u8 CAN_transmiter::CAN_Wakeup() {
 }
 
 /**
- * @brief  CAN0中断服务程序
+ * @brief  CAN0 中断服务程序（优化版：直接读取数据，减少DMA竞态条件）
  * @retval 无
  */
 void CAN0_RX0_IRQHandler(void) {
@@ -692,31 +987,25 @@ void CAN0_RX0_IRQHandler(void) {
 		// 清除中断标志
 		can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0);
 
-		// 使用DMA读取接收到的数据
+		// 直接读取消息（包括数据），不依赖DMA
-		dma_channel_disable(DMA0, DMA_CH4);
+		can_receive_message_struct temp_message;
-		dma_rx_parameter.number =
+		can_message_receive(CAN0, CAN_FIFO0, &temp_message);
-			can_receive_message_length_get(CAN0, CAN_FIFO0);
-		dma_init(DMA0, DMA_CH4, &dma_rx_parameter);
-		dma_channel_enable(DMA0, DMA_CH4);
 
-		// 读取接收到的数据
+		// 将接收到的消息加入环形缓冲区
-		can_message_receive(CAN0, CAN_FIFO0, &CAN0_transmiter.receive_message);
+		CAN0_transmiter.enqueue_rx_message_from_isr(temp_message);
-
-		// 接收标志位置1
-		CAN0_transmiter.can0_receive_flag = SET;
 	}
 
 	// 检查其他可能的中断源
 	if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) != RESET) {
 		// FIFO0 满中断
 		can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0);
-		printf("CAN FIFO0 full\n");
+		CAN_DEBUG_PRINT("CAN FIFO0 full\n");
 	}
 
 	if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFO0) != RESET) {
 		// FIFO0 溢出中断
 		can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFO0);
-		printf("CAN FIFO0 overflow\n");
+		CAN_DEBUG_PRINT("CAN FIFO0 overflow\n");
 	}
 }
 
@@ -758,13 +1047,13 @@ void DMA0_Channel5_IRQHandler(void) {
 u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
 	// 检查数据长度是否符合 CAN 规范
 	if (length > 8) {
-		printf("CAN data length exceeds maximum of 8 bytes\r\n");
+		CAN_DEBUG_PRINT("CAN data length exceeds maximum of 8 bytes\r\n");
 		return 0;
 	}
 
 	// 检查数据指针是否有效
 	if (data == nullptr) {
-		printf("Invalid data pointer\r\n");
+		CAN_DEBUG_PRINT("Invalid data pointer\r\n");
 		return 0;
 	}
 
@@ -785,7 +1074,8 @@ u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
 	// 检查是否成功获取 mailbox
 	if (transmit_mailbox == CAN_NOMAILBOX) {
 		// 没有可用的 mailbox，启用发送中断以便在邮箱可用时重试
-		printf("No available mailbox for CAN transmission, enabling TX "
+		CAN_DEBUG_PRINT(
+			"No available mailbox for CAN transmission, enabling TX "
 			"interrupt\r\n");
 		// 可以设置一个标志位，当邮箱可用时发送
 		// 在实际应用中，可以维护一个待发送队列
@@ -798,7 +1088,51 @@ u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
 }
 
 /**
- * @brief  异步CANFD数据发送(使用DMA和中断机制)
+ * @brief  向发送队列添加消息
+ * @param  id CAN消息ID
+ * @param  length CAN消息长度
+ * @param  data CAN消息数据
+ * @retval 1=成功, 0=队列满
+ */
+static u8 CAN_TX_QueuePush(u32 id, u8 length, u8 *data) {
+	if (tx_queue_count >= CAN_TX_QUEUE_SIZE) {
+		CAN_DEBUG_PRINT("TX queue full, message dropped\r\n");
+		return 0; // 队列满，丢弃消息
+	}
+
+	tx_queue[tx_queue_tail].id = id;
+	tx_queue[tx_queue_tail].length = length;
+	for (int i = 0; i < length && i < 64; i++) {
+		tx_queue[tx_queue_tail].data[i] = data[i];
+	}
+	tx_queue[tx_queue_tail].valid = 1;
+
+	tx_queue_tail = (tx_queue_tail + 1) % CAN_TX_QUEUE_SIZE;
+	tx_queue_count++;
+
+	return 1;
+}
+
+/**
+ * @brief  从发送队列取出消息
+ * @param  item 指向队列项的指针
+ * @retval 1=成功, 0=队列空
+ */
+static u8 CAN_TX_QueuePop(can_tx_queue_item_t *item) {
+	if (tx_queue_count == 0) {
+		return 0; // 队列空
+	}
+
+	*item = tx_queue[tx_queue_head];
+	tx_queue[tx_queue_head].valid = 0;
+	tx_queue_head = (tx_queue_head + 1) % CAN_TX_QUEUE_SIZE;
+	tx_queue_count--;
+
+	return 1;
+}
+
+/**
+ * @brief  异步 CANFD 数据发送 (使用队列机制防止丢帧)
  * @param  id CAN 消息 ID
  * @param  length CAN 消息长度
  * @param  data CAN 消息数据
@@ -807,13 +1141,13 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
 	u8 valid_length = length;
 	// 检查 CANFD 模式是否启用
 	if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
-		printf("CANFD mode not enabled\r\n");
+		CAN_DEBUG_PRINT("CANFD mode not enabled\r\n");
 		return 0;
 	}
 
 	// 检查数据指针是否有效
 	if (data == nullptr) {
-		printf("Invalid data pointer\r\n");
+		CAN_DEBUG_PRINT("Invalid data pointer\r\n");
 		return 0;
 	}
 
@@ -834,7 +1168,7 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
 		} else if (length > 48 and length < 65) {
 			valid_length = 64;
 		} else {
-			printf("CANFD data length error\r\n");
+			CAN_DEBUG_PRINT("CANFD data length error\r\n");
 			return 0;
 		}
 	}
@@ -863,25 +1197,47 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
 
 	// 检查是否成功获取 mailbox
 	if (transmit_mailbox == CAN_NOMAILBOX) {
-		// 没有可用的mailbox，启用发送中断以便在邮箱可用时重试
+		// 没有可用的 mailbox，将消息加入发送队列
-		printf("No available mailbox for CANFD transmission, enabling TX "
+		CAN_DEBUG_PRINT("No available mailbox, adding to TX queue\r\n");
-			   "interrupt\r\n");
+		return CAN_TX_QueuePush(id, valid_length, data);
-		return 0;
 	}
 
-	// 启用CAN发送中断以检测传输完成
+	// 启用 CAN 发送中断以检测传输完成（用于队列发送）
 	can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
 	return 1;
 }
 
-// 添加CAN传输中断处理函数
+/**
+ * @brief  CAN 传输中断处理函数（支持队列发送）
+ */
 void CAN0_TX_IRQHandler(void) {
 	// 检查是否有发送邮箱空闲
 	if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET ||
 		can_flag_get(CAN0, CAN_FLAG_TME1) != RESET ||
 		can_flag_get(CAN0, CAN_FLAG_TME2) != RESET) {
-		// 邮箱已空闲，可以发送新消息
+
-		// 可以在这里处理待发送队列中的消息
+		can_tx_queue_item_t next_msg;
+		// 尝试从队列中取出下一条消息发送
+		if (CAN_TX_QueuePop(&next_msg)) {
+			// 配置并发送队列中的消息（使用独立的队列消息结构体）
+			queue_transmit_message.tx_ft = CAN_FT_DATA;
+			queue_transmit_message.tx_ff = CAN_FF_EXTENDED;
+			queue_transmit_message.tx_efid = next_msg.id;
+			queue_transmit_message.tx_dlen = next_msg.length;
+			queue_transmit_message.fd_flag = CAN_FDF_FDFRAME;
+			queue_transmit_message.fd_brs = ENABLE;
+			queue_transmit_message.fd_esi = DISABLE;
+
+			for (int i = 0; i < next_msg.length; i++) {
+				queue_transmit_message.tx_data[i] = next_msg.data[i];
+			}
+
+			can_message_transmit(CAN0, &queue_transmit_message);
+		} else {
+			// 队列为空，禁用发送中断
+			can_interrupt_disable(CAN0, CAN_INTEN_TMEIE);
+		}
+
 		// 清除发送中断标志
 		can_flag_clear(CAN0, CAN_FLAG_TME0);
 		can_flag_clear(CAN0, CAN_FLAG_TME1);

Robot_V2/device/Can.h

@@ -6,7 +6,9 @@
 extern "C" {
 #endif
 
+#include <cstdint>
 #include "gd32c10x_can.h"
+#include "config.h"
 
 // 定义一个联合体类型用于 float 和 byte 数组之间的转换
 typedef union {
@@ -65,57 +67,99 @@ void can_loopback_test(void);
 
 /*!
 	\brief CAN 通信类
-	\note  封装CAN发送和接收功能
+	\note  封装 CAN 发送和接收功能，支持多电机节点 ID 隔离
 */
 class CAN_transmiter {
 
   private:
-	bool is_sleep = false; // 是否休眠
+	static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN 接收缓冲区大小
+	static const u32 CAN_SILENT_DURATION_MS = 10000; // CAN 静默时长 10 秒
 	
-	u8 controller_Mode = 0;					   // 电机模式
+	bool is_sleep = false; // 是否休眠
-	float_union_t limit_velocity_union = {0};  // 限制速度
+	bool is_silent = false; // 是否静默（收到 base_id=0x0000 后进入静默）
-	u16 limit_voltage = 0;					   // 限制电压
+	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}; // 实时速度
-	bool MotorStatus = 0;					   // 电机状态码
+	float_union_t rt_current_union = {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 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 字节
 
+	// 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 接收
+	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 发送标志
 	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_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

@@ -11,12 +11,43 @@
 
 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
@@ -84,86 +115,63 @@ void User_timer_init(void) {
 }
 
 /**
- * @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) {
+    if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) == RESET) {
-		// 清除更新中断标志
+        return;
+    }
     timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
 
-		// 每50us执行一次的操作 - 电机控制
+    // 固定任务：每 50us 执行电机控制
-		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
+    scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) | 
-		scheduler.taskFlags |=
+                           (1u << TASK_VELOCITY_UPDATE) | 
-			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
+                           (1u << TASK_MOTOR_CONTROL);
 
-		// 使用标志位控制，每100us执行一次其他任务
+    // 每 100us 执行一次的任务（使用位运算替代布尔变量）
-		static bool isEvenCall = false;
+    static u32 call_counter = 0;
-		isEvenCall = !isEvenCall;
+    call_counter++;
     
-		if (isEvenCall) {
+    if ((call_counter & 1u) == 0u) {  // 偶数次调用（每 100us）
-			// 每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 > 9) {
+        if (total_us100_counter >= 10u) {
             total_us100_counter = 0;
-				// 可以添加每1ms执行的代码，无负载均衡
-				static u8 ms1_counter = 0;
             
-				switch (ms1_counter) { // 每10ms执行的代码，负载均衡
+            // 每 1ms 执行的任务
-				case 0: {
+            static u32 ms1_counter = 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) {
+            if (ms1_counter >= 10u) {
                 ms1_counter = 0;
-					// 可以添加每10ms执行的代码，无负载均衡
+                
-					static u8 ms10_counter = 0;
+                // 每 10ms 执行的任务
+                static u32 ms10_counter = 0;
                 ms10_counter++;
-					if (ms10_counter > 9) {
+                if (ms10_counter >= 10u) {
                     ms10_counter = 0;
-						// 可以添加每100ms执行的代码
+                    
-						static u8 sec_counter = 0;
+                    // 每 100ms 执行的任务
-						sec_counter++;
+                    static u32 ms100_counter = 0;
-						if (sec_counter > 9) {
+                    ms100_counter++;
-							sec_counter = 0;
+                    if (ms100_counter >= 10u) {
-							// 可以添加每1s执行的代码
+                        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];
         }
     }
 }

Robot_V2/device/ang_sensor.cpp

@@ -9,7 +9,13 @@
 #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)
@@ -113,7 +119,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 		// 禁用 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;
 	}
 
@@ -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;
@@ -199,13 +205,14 @@ u8 angle_sensor::init(void) {
 	spi_dma_init();		 // DMA 初始化
 	return 0;
 }
+
 /**
  * @ 角度传感器类 设置当前角度
  * @param angle 输出级角度值
  * @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;
 }
@@ -231,12 +238,12 @@ inline float angle_sensor::read_sensorAngle(void) {
 
 	// 仅在调试模式下输出错误，避免发布版本中的 I/O 开销
 	if (result != 0) {
-		// 使用更高效的串口输出替代printf（如果可用）
+		ANG_SENS_DEBUG_PRINT("Error reading angle from sensor.\n");
-		printf("Error reading angle from sensor.\n");
 	}
 
 	return (float)FAULT_RECEIVE;
 }
+
 /**
  * @brief 更新传感器角度（输出级）
  * @return 累积角度值
@@ -265,7 +272,6 @@ float angle_sensor::update(void) {
 
 	m_cumulativeAngle += delta;
 	m_lastAngle = current_angle_deg;
-
 	return m_cumulativeAngle;
 }
 

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,6 +1,14 @@
 #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() {
@@ -9,17 +17,17 @@ void test_1111() {
 	// 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'));
+		TEST_DEBUG_PRINT(" 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'));
+		TEST_DEBUG_PRINT(" 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'));
+		TEST_DEBUG_PRINT(" 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());
+		TEST_DEBUG_PRINT(" 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 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"
 
+// 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 FOC_Controller FOC;					// FOC 控制
 extern FlashEEPROM flashEEP;				// EEPROM 类
 extern VersionManager Version;
 
 // 全局版本管理器实例
 TaskScheduler scheduler = {0};
-volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次
+u32 motor_runtime_50us = 0; // 电机驱动计算轮次
-volatile u32 cnt_test = 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);	// 看门狗初始化
@@ -58,11 +78,15 @@ u8 hardware_init(void) {
 	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 初始化
 	return init_status;
 }
@@ -83,21 +107,38 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务 (最高优先级)
 		motor_runtime_50us++;
 
-		// 函数指针数组，包含各种控制模式对应的函数
+		// 快速电压检测（每100us执行一次，响应时间 < 200us）
-		static FOCFunctionPtr control_functions[] = {
+		FOC.fast_voltage_check();
-			&FOC_Controlroller::drv_hold, // 0: 占空比置
+
-			&FOC_Controlroller::
+		// 静态变量用于跟踪上一次的控制模式
-				angle_control_with_velocity_limit, // 1:
+		static int last_control_mode = -1;
-												   // 角度闭环控制，带速度限制
-			&FOC_Controlroller::velocity_control,  // 2: 速度闭环控制
-			&FOC_Controlroller::current_control,   // 3: 电流闭环控制
-			&FOC_Controlroller::velocity_open_loop, // 4: 速度开环控制
-		};
 
 		// 边界检查，防止数组越界
-		if (FOC.control_mode >= 0 && FOC.control_mode < 5) {  // 更新为7个控制模式
+		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 温度传感器值
@@ -116,7 +157,7 @@ void do_task(u32 currentTasks) {
 		power_manage();
 	}
 	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {				  // CAN 通信
-		// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
+		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,5 +1,6 @@
 #ifndef __CONFIG_H
 #define __CONFIG_H
+#include "gd32c10x_eval.h"
 #include <cstdint>
 
 // 如果使用 CMSIS DSP 库，需要定义 ARM_MATH_CM4
@@ -8,12 +9,16 @@
 #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_MINOR = 4;
-constexpr u16 SOFTWARE_VERSION_PATCH = 24;
+constexpr u16 SOFTWARE_VERSION_PATCH = 26;
-constexpr u16 SOFTWARE_VERSION_BUILD = 1;
+constexpr u16 SOFTWARE_VERSION_BUILD = 2;
 
 constexpr u16 HARDWARE_VERSION_MAJOR = 25;
 constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -38,6 +43,35 @@ 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 可用地址
@@ -51,29 +85,88 @@ 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 CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移
-constexpr float HOLD_PWM_DUTY = 0.95f;
+constexpr float HOLD_PWM_DUTY = 0.1f;
-constexpr u16 ANGLESENS_OFFSET = +0;
+constexpr u16 ANGLESENS_OFFSET = +15;
-constexpr float KE_FACTOR = 14.0f;
+constexpr float KE_FACTOR = 0.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
 
+// 角度卡尔曼滤波器参数
+// 参数调节建议：
+// - 增大 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 u8 DEFAULT_VALUE = 0;
 constexpr u8 CONFIG_SAVED_NUM = 6;
 
@@ -83,22 +176,29 @@ constexpr u16 LUT_MASK = (LUT_SIZE - 1);
 constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
 
 // 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
+// 反馈基础 ID
-constexpr u32 CANFD_STATUS_ID = 0x00010100;
+constexpr u16 CANFD_STATUS_ID = 0x0100;	 // 状态反馈基础 ID
-constexpr u32 CANFD_CONFIG_ID = 0x00010101;
+constexpr u16 CANFD_CONFIG_ID = 0x0101;	 // 配置信息基础 ID
-constexpr u32 CANFD_VERSION_ID = 0x00010000;
+constexpr u16 CANFD_VERSION_ID = 0x00FF; // 版本信息基础 ID
-// 命令ID
+// 命令基础 ID
-constexpr u32 CANFD_CMD_ID = 0x00010200;
+constexpr u16 CANFD_CMD_ID = 0x0200;   // 控制命令基础 ID
-constexpr u32 CANFD_RESET_ID = 0x00010301;
+constexpr u16 CANFD_RESET_ID = 0x0301; // 重置命令基础 ID
-// OTA相关ID
+// 节点 ID 管理基础 ID
-constexpr u32 CANFD_BOOT_ID = 0x00000000;
+constexpr u16 CANFD_SET_NODE_ID = 0x0400; // 设置节点 ID 基础 ID
-constexpr u32 CANFD_OTA_REQ_ID = 0x00000100;
+constexpr u16 CANFD_GET_NODE_ID = 0x0401; // 查询节点 ID 基础 ID
-constexpr u32 CANFD_OTA_DATA_ID = 0x00000101;
+
-constexpr u32 CANFD_OTA_ACK_ID = 0x00000102;
+// OTA 相关基础 ID 不受到node ID影响
-constexpr u32 CANFD_OTA_WAIT_ID = 0x00000103;
+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 int TIMER2_PERIOD = 49;	  // 1 / 1MHz * 50 = 50us
-constexpr float TIMER2_FREQUENCY = 1 / 5e-5;
+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_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

@@ -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();
@@ -104,10 +118,8 @@ u8 FlashEEPROM::read_saved_system_config(void) {
 
 		crc_data_register_reset(); // 确保 CRC 计算从初始状态开始
 		CRC_NUM = crc_block_data_calculate((uint32_t *)data, CONFIG_SAVED_NUM);
-
+		EEP_DEBUG_PRINT("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM,
-#if EEP_DEBUG
+						*((u32 *)next_address));
-		printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
-#endif
 		if (CRC_NUM != *((u32 *)next_address)) { // CRC 错误
 			for (u8 i = 0; i < CONFIG_SAVED_NUM; i++) {
 				data[i] = DEFAULT_VALUE;
@@ -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];
@@ -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;
 			}
 		}
@@ -189,9 +203,7 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const {
 				u32 float_data = data_word & DATA_FLOAT_MASK;
 				// 使用内存拷贝将处理后的数据解释为 float
 				memcpy(data, &float_data, SIZE_FLOAT);
-#if EEP_DEBUG
+				EEP_DEBUG_PRINT("read:0x%x=%f\r\n", address, *data);
-				printf("read:0x%x=%f\r\n", address, *data);
-#endif
 				return FLASH_EEPROM_OK;
 			}
 		}
@@ -210,8 +222,8 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
 	// 查找下一个可写入的位置
 	u32 next_address = EEPROM_START_ADDRESS;
 	u32 data_read;
-	// 插入虚拟地址到低8位	格式: [data:16bit][reserved:8bit][addr:8bit]
+	// 构建要写入的 32 位数据：格式 [data:16bit][reserved:8bit][addr:8bit]
-	data = (data & DATA_U16_MASK) | (u32)virtualAddr;
+	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) {
@@ -221,11 +233,12 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
 
 		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 {
@@ -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_to_write);
-		fmc_state_enum fmc_status = write_ADDRESS(next_address, data_word);
 
 		if (fmc_status != FMC_READY) {
 			return FLASH_EEPROM_WRITE_ERROR;
 		}
 		return FLASH_EEPROM_OK;
 	} else { // 空间不足，需要格式化页面
-#if EEP_DEBUG
+		EEP_DEBUG_PRINT("no enough space\r\n");
-		printf("no enough space\r\n");
-#endif
 		FlashEepromStatus status = format();
 		if (status == FLASH_EEPROM_OK) {
 			status = save_system_config(false);
@@ -286,7 +296,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
 
 	// 将完整的 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 末尾开始向前搜索，找到最新的数据
@@ -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 {
@@ -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) {
@@ -362,9 +373,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
  */
 FlashEepromStatus FlashEEPROM::format() {
 	fmc_state_enum fmc_status;
-#if EEP_DEBUG
+	EEP_DEBUG_PRINT("擦除页面\r\n");
-	printf("擦除页面\r\n");
-#endif
 
 	fmc_unlock();
 	fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); // 擦除第一页
@@ -389,7 +398,7 @@ 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};
 
 	// 如果系统配置标记已保存，则无需再次保存
@@ -468,6 +477,228 @@ 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
  * @param saved: 是否一次性保存
@@ -495,9 +726,8 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
 		// 如果有足够空间，直接写入 crc 值
 		if (next_address > EEPROM_START_ADDRESS) {
 			fmc_state_enum fmc_status = write_ADDRESS(next_address, CRC_NUM);
-#if EEP_DEBUG
+			EEP_DEBUG_PRINT("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM,
-			printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
+							*((u32 *)next_address));
-#endif
 			// 检查写入是否成功
 			if (fmc_status != FMC_READY) {
 				EEPROMStatus = fmc_status;
@@ -505,9 +735,7 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
 			}
 		} else {
 			// 空间不足，需要格式化页面
-#if EEP_DEBUG
+			EEP_DEBUG_PRINT("no enough space\r\n");
-			printf("no enough space\r\n");
-#endif
 			FlashEepromStatus status = format();
 			if (status == FLASH_EEPROM_OK) {
 				status = save_system_config(false);
@@ -534,18 +762,18 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
  * @brief  设置 OTA 配置参数到 Flash
  *
  * 此函数用于更新存储在 Flash 中的 OTA 相关标志位（如 APP 可用、启用、校验等）。
- * GD32的Flash特性是：只能将1写为0，不能将0写为1。若要将0变为1，必须先擦除整个页面。
+ * GD32 的 Flash 特性是：只能将 1 写为 0，不能将 0 写为 1。若要将 0 写为
- * 因此，本函数通过读取同一页内的所有参数，擦除该页，然后重新写入有效的参数来实现“写1”操作。
+ * 1，必须先擦除整个页面。
+ * 因此，本函数通过读取同一页内的所有参数，擦除该页，然后重新写入有效的参数来实现"写
+ * 1"操作。
  *
  * @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
+	EEP_DEBUG_PRINT("OTA_prepare write%08x at %08x\r\n", data, addr);
-	printf("OTA_prepare write%08x at %08x\r\n", data, addr);
-#endif
 	fmc_state_enum fmc_status;
 	u32 temp_val[OTA_CONFIG_NUM]; // 临时数组，用于存储当前所有配置参数的值
 	volatile u32 *flash_ptr = (volatile u32 *)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
+	EEP_DEBUG_PRINT("OTA_prepare write%08x at %08x\r\n", data, addr);
-	printf("OTA_prepare write%08x at %08x\r\n", data, addr);
 	// 修复：完整输出所有 6 个配置项的值，便于调试
-	printf("LatestAval:0x%x, BackupAval:0x%x, LatestEn:0x%x, "
+	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[0], temp_val[1], temp_val[2], temp_val[3],
-		   temp_val[5]);
+					temp_val[4], temp_val[5]);
-#endif
 	// 4. 擦除包含所有配置参数的 Flash 页
 	fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR);
 	if (fmc_status != FMC_READY) {
-#if EEP_DEBUG
+		EEP_DEBUG_PRINT("OTA_SET - Page erase failed! Status: %d\r\n",
-		printf("OTA_SET - Page erase failed! Status: %d\r\n", fmc_status);
+						fmc_status);
-#endif
 		return FLASH_EEPROM_ERASE_ERROR;
 	}
 
@@ -604,11 +829,9 @@ 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
+				EEP_DEBUG_PRINT("OTA_SET - Write backup value %08X failed at "
-				printf("OTA_SET - Write backup value %08X failed at "
 								"0x%08X!status:%d\r\n",
 								temp_val[i], current_addr, fmc_status);
-#endif
 				return FLASH_EEPROM_WRITE_ERROR;
 			}
 		}
@@ -629,10 +852,10 @@ u8 OTA_SET(u32 addr, u32 data) {
 	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, "
+	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[0], temp_val[1], temp_val[2], temp_val[3],
-		   temp_val[5]);
+					temp_val[4], temp_val[5]);
 #endif
 
 	return FLASH_EEPROM_OK;
@@ -677,7 +900,7 @@ void jumpToBOOT() {
 	app_entry();
 
 	// 如果程序执行到这里，说明跳转失败
-	printf("Jump to application failed!\r\n");
+	EEP_DEBUG_PRINT("Jump to application failed!\r\n");
 }
 
 /**
@@ -686,14 +909,12 @@ void jumpToBOOT() {
  * @retval fmc_state_enum: 操作状态
  */
 fmc_state_enum move_app_data() {
-	// 源地址: APPLATEST_START_ADDR (0x08011000)
+	// 源地址：APPLATEST_START_ADDR (0x08011000)
-	// 目标地址: APPBACK_START_ADDR (0x08005000)
+	// 目标地址：APPBACK_START_ADDR (0x08005000)
-	// 数据大小: APP_MAX_SIZE (0xC000 = 48KB)
+	// 数据大小：APP_MAX_SIZE (0xC000 = 48KB)
 
 	fmc_state_enum fmc_status = FMC_READY;
-#if OTA_DEBUG
+	OTA_DEBUG_PRINT("move_app_data\r\n");
-	printf("move_app_data\r\n");
-#endif
 	// 关闭全局中断以防止 Flash 操作期间的中断干扰
 	__disable_irq();
 	fmc_unlock();
@@ -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(); // 重新开启中断
@@ -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);
@@ -776,15 +997,12 @@ fmc_state_enum move_app_data() {
 
 	// 比较 CRC 值，确保数据完整性
 	if (src_crc != dst_crc) {
-#if OTA_DEBUG
+		OTA_DEBUG_PRINT(
-		printf("APPbackup CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n",
+			"APPbackup CRC mismatch: src_crc=0x%08X, dst_crc=0x%08X\r\n",
 			src_crc, dst_crc);
-#endif
 		return (fmc_state_enum)0xFF; // CRC 校验失败
 	}
-#if OTA_DEBUG
+	OTA_DEBUG_PRINT("APPbackup CRC OK\r\n");
-	printf("APPbackup CRC OK\r\n");
-#endif
 	// 设置备份应用可用标志
 	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.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,6 +11,20 @@
 #include <cstdio>
 #include <cstring>
 
+// 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;
 
@@ -108,20 +122,23 @@ void VersionManager::sendVersionInfoViaCan() const {
 	versionData[22] = bootVersion.build & 0xFF;		   // 低字节
 	versionData[23] = (bootVersion.build >> 8) & 0xFF; // 高字节
 
-	// 使用CANFD发送函数发送版本信息
+	// 使用 CANFD 发送函数发送版本信息，使用 build_can_id 构建带 node_id 的 CAN
-	CAN0_transmiter.CANFD_AsyncSend(CANFD_VERSION_ID, 24, versionData);
+	// 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",
+	OTA_DEBUG_PRINT(
+		"BOOT Version: %s\r\n",
 		versionToString(getBootVersion(), versionStr, sizeof(versionStr)));
 }
 
@@ -177,36 +194,36 @@ 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;
 }
 
@@ -227,7 +244,8 @@ void OTA_initial(void) {
 	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, "
+	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]);
@@ -237,9 +255,7 @@ void OTA_initial(void) {
 	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
+		OTA_DEBUG_PRINT("APP enabled\r\n");
-		std::printf("APP enabled\r\n");
-#endif
 		move_app_data();
 	}
 
@@ -257,17 +273,13 @@ void OTA_initial(void) {
 	u32 back_check = *flash_ptr;
 
 	if (back_aval == OTA_NOT_AVAL) { // 如果 BACK 不可用
-#if OTA_DEBUG
+		OTA_DEBUG_PRINT("notaval\r\n");
-		std::printf("notaval\r\n");
-#endif
 		move_app_data();
 	} else if (back_aval == OTA_AVAL &&
 			   back_check == OTA_NOT_AVAL) { // BACK 已刷写，但未检查
-#if OTA_DEBUG
+		OTA_DEBUG_PRINT("aval\r\n");
-		std::printf("aval\r\n");
-#endif
 		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
@@ -149,9 +175,7 @@ void app_prepare_deep_sleep(bool save_data) {
 			address > SLEEP_CONFIG_START) { // 空数据且未到头跳过
 			continue;
 		} else { // 有数据的话读取
-#if SLEEP_DEBUG
+			SLEEP_DEBUG_PRINT("app sleep found %08x at %08x\r\n", sleep_enable, address);
-			printf("app sleep found %08x at %08x\r\n", sleep_enable, address);
-#endif
 			if (address == SLEEP_CONFIG_END - SIZE_U32) {
 				fmc_unlock();
 				fmc_state_enum fmc_status =
@@ -164,23 +188,19 @@ void app_prepare_deep_sleep(bool save_data) {
 				address = SLEEP_CONFIG_START - SIZE_U32; // 重新开始写入
 			}
 			address += SIZE_U32;
-#if SLEEP_DEBUG
+			SLEEP_DEBUG_PRINT("app sleep prepare write %08x at %08x\r\n", SLEEP_ENABLE,
-			printf("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
+				SLEEP_DEBUG_PRINT("app wrote %08x at %08x ERROR\r\n", SLEEP_ENABLE,
-				printf("app wrote %08x at %08x ERROR\r\n", SLEEP_ENABLE,
 					   address);
-#endif
 				return;
 			}
 			break;

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"
  }
 }