feat(FOC): 添加快速电压检测功能和PWM调试模式

新增基于跌幅检测的快速电压保护机制，在100us周期内响应电压异常，
当电压跌幅超过阈值或低于绝对下限时立即关机保护。同时添加PWM输出
调试模式便于硬件测试。

BREAKING CHANGE: 重构了电压检测逻辑，采用峰值跟踪算法替代固
定阈值检测，提高在动态供电环境下的可靠性

Robot_V2/Algorithm/FOC.cpp

@@ -18,6 +18,7 @@ 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)                                             \
@@ -293,6 +294,18 @@ float FOC_Controller::drv_hold() {
 	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 电机恢复
  *
@@ -318,6 +331,67 @@ void FOC_Controller::update_power(float voltage) {
 	}
 }
 
+/**
+ * @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 速度开环控制
  *

Robot_V2/Algorithm/FOC.h

@@ -30,12 +30,18 @@ class FOC_Controller {
 	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 / 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;
@@ -60,8 +66,10 @@ class FOC_Controller {
 	void resume(void);
 	void drv_stop(void);
 	float drv_hold(void);
+	float PWM_out(void);
 	void shut_down_thermal(void);	// 过热关机
 	void resume_from_thermal(void); // 过热恢复
+	void fast_voltage_check(void);	// 快速电压检测（100us周期调用）
 	
 	// 此处默认值不作数
 	PIDController vel_loop =

Robot_V2/device/Can.cpp

@@ -38,8 +38,25 @@ 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_tx_buffer[8]; // CAN 发送缓冲区
+uint8_t can_rx_buffer[64]; // CAN 接收缓冲区（支持CAN FD最大64字节）
+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 初始化
 
@@ -100,28 +117,24 @@ void CAN_transmiter::set_node_id(u16 new_id) {
 }
 
 /*!
-	\brief 从 Flash 加载节点 ID
+	\brief 从 Flash 加载节点 ID（使用魔数+双重备份+计数器机制）
 	\param[in]  none
 	\param[out] none
 	\retval     FLASH_EEPROM_OK: 加载成功
 				其他：加载失败，使用默认值
-	\note       Flash 擦除状态为 0xFFFF，需要特殊处理避免误判
-				注意：0 是有效的节点 ID，不应被视为无效值
+	\note       使用独立的节点 ID 存储区域，带魔数验证和 CRC 校验
+				防止位翻转导致错读
 */
 u8 CAN_transmiter::load_node_id_from_flash() {
-	u16 node_id_temp = 0xFFFF; // 初始化为 Flash 擦除值
-	FlashEepromStatus status =
-		flashEEP.read(FlashEEPROM::ADDR_NODE_ID, &node_id_temp);
+	u16 node_id_temp = 0;
+	FlashEepromStatus status = flashEEP.readNodeId(&node_id_temp);
 
-	// 检查 Flash 是否为擦除状态 (0xFFFF) 或读取失败
-	// 注意：Flash 未写入时读到的值是 0xFFFF
-	// 0 是有效的节点 ID，不应被视为无效值
-	if (status != FLASH_EEPROM_OK || node_id_temp == 0xFFFF) {
+	// 检查读取是否成功
+	if (status != FLASH_EEPROM_OK) {
 		node_id = DEFAULT_NODE_ID;
 		CAN_DEBUG_PRINT(
-			"Node ID not found in Flash (status=%d, val=0x%04X), using "
-			"default: %d\r\n",
-			status, node_id_temp, node_id);
+			"Node ID not found in Flash (status=%d), using default: %d\r\n",
+			status, node_id);
 		return FLASH_EEPROM_DATA_NOT_FOUND;
 	}
 
@@ -139,25 +152,26 @@ u8 CAN_transmiter::load_node_id_from_flash() {
 }
 
 /*!
-	\brief 保存节点 ID 到 Flash
+	\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.write(FlashEEPROM::ADDR_NODE_ID, new_id);
+	
+	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\r\n");
+		CAN_DEBUG_PRINT("Failed to save Node ID to Flash (status=%d)\r\n", status);
 		return (u8)status;
 	}
 }
@@ -746,10 +760,6 @@ void CAN_transmiter::communication_check() {
 	// 处理所有接收到的 CAN 消息（从环形缓冲区中）
 	can_receive_message_struct current_message;
 	while (dequeue_rx_message(current_message)) {
-		// CAN_DEBUG_PRINT("\r\n can0 %X receive data:",
-		// current_message.rx_efid); for (i = 0; i < current_message.rx_dlen;
-		// i++) { 	CAN_DEBUG_PRINT(" %02x", current_message.rx_data[i]);
-		// }
 		// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
 		u32 base_id = current_message.rx_efid & 0xFFFF;
 		
@@ -776,6 +786,74 @@ void CAN_transmiter::communication_check() {
 			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; // 不是发给本节点的消息，跳过
@@ -864,75 +942,6 @@ void CAN_transmiter::communication_check() {
 								transmit_data);
 			}
 		}
-		// 设置节点 ID 命令 (基础 ID: 0x0400)
-		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) {
-				u8 save_status = save_node_id_to_flash(new_node_id);
-				CAN_DEBUG_PRINT(
-					"SET_NODE_ID: save_status=%d, current node_id=%d\r\n",
-					save_status, node_id);
-				if (save_status == FLASH_EEPROM_OK) {
-					// 发送确认响应 - 使用旧节点 ID 构建响应 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 高字节
-					CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, send response with "
-									"old_id=%d, new_id_bytes=%02X %02X\r\n",
-									old_node_id, transmit_data[1],
-									transmit_data[2]);
-					// 注意：这里不修改 node_id，因为 save_node_id_to_flash
-					// 已经更新了 但构建响应时需要用旧 ID，所以临时修改
-					node_id = old_node_id;
-					CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
-									transmit_data); // 发送 3 字节
-					node_id = new_node_id;			// 恢复新 ID
-					CAN_DEBUG_PRINT("SET_NODE_ID: node_id updated to %d\r\n",
-									node_id);
-				} else {
-					// Flash 写入失败，发送错误响应
-					CAN_DEBUG_PRINT(
-						"SET_NODE_ID: Flash write FAILED, status=%d\r\n",
-						save_status);
-					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;
-					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 不变
-			}
-		}
 		// 查询节点 ID 命令 (基础 ID: 0x0402)
 		else if (base_id == CANFD_GET_NODE_ID) {
 			pack_u16_to_bytes(transmit_data, 0, get_node_id());
@@ -969,7 +978,7 @@ u8 CAN_transmiter::CAN_Wakeup() {
 }
 
 /**
- * @brief  CAN0 中断服务程序
+ * @brief  CAN0 中断服务程序（优化版：直接读取数据，减少DMA竞态条件）
  * @retval 无
  */
 void CAN0_RX0_IRQHandler(void) {
@@ -978,14 +987,7 @@ void CAN0_RX0_IRQHandler(void) {
 		// 清除中断标志
 		can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0);
 
-		// 使用 DMA 读取接收到的数据
-		dma_channel_disable(DMA0, DMA_CH4);
-		dma_rx_parameter.number =
-			can_receive_message_length_get(CAN0, CAN_FIFO0);
-		dma_init(DMA0, DMA_CH4, &dma_rx_parameter);
-		dma_channel_enable(DMA0, DMA_CH4);
-
-		// 读取接收到的数据
+		// 直接读取消息（包括数据），不依赖DMA
 		can_receive_message_struct temp_message;
 		can_message_receive(CAN0, CAN_FIFO0, &temp_message);
 
@@ -1086,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 消息数据
@@ -1151,26 +1197,47 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
 
 	// 检查是否成功获取 mailbox
 	if (transmit_mailbox == CAN_NOMAILBOX) {
-		// 没有可用的 mailbox，启用发送中断以便在邮箱可用时重试
-		CAN_DEBUG_PRINT(
-			"No available mailbox for CANFD transmission, enabling TX "
-			"interrupt\r\n");
-		return 0;
+		// 没有可用的 mailbox，将消息加入发送队列
+		CAN_DEBUG_PRINT("No available mailbox, adding to TX queue\r\n");
+		return CAN_TX_QueuePush(id, valid_length, data);
 	}
 
-	// 启用 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/main.cpp

@@ -43,6 +43,7 @@ static FOCFunctionPtr control_functions[] = {
 	&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();
@@ -106,12 +107,15 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务 (最高优先级)
 		motor_runtime_50us++;
 
+		// 快速电压检测（每100us执行一次，响应时间 < 200us）
+		FOC.fast_voltage_check();
+
 		// 静态变量用于跟踪上一次的控制模式
 		static int last_control_mode = -1;
 
 		// 边界检查，防止数组越界
 		if (FOC.control_mode >= 0 &&
-			FOC.control_mode < 7) { // 更新为 7 个控制模式
+			FOC.control_mode < 8) { // 更新为 8 个控制模式
 
 			// 检查控制模式是否发生变化
 			if (FOC.control_mode != last_control_mode) {

Robot_V2/sys/config.h

@@ -16,8 +16,8 @@
 
 // 版本号定义 - 可根据实际项目进行调整
 constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
-constexpr u16 SOFTWARE_VERSION_MINOR = 3;
-constexpr u16 SOFTWARE_VERSION_PATCH = 16;
+constexpr u16 SOFTWARE_VERSION_MINOR = 4;
+constexpr u16 SOFTWARE_VERSION_PATCH = 26;
 constexpr u16 SOFTWARE_VERSION_BUILD = 2;
 
 constexpr u16 HARDWARE_VERSION_MAJOR = 25;
@@ -46,10 +46,31 @@ 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
-constexpr u32 NODE_ID_CONFIG_ADDR = 0x0801E800; // 节点 ID 存储地址
+// 
+// 节点 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 = 0xFFFF;		// 广播 ID (所有节点响应)
-constexpr u16 MAX_NODE_ID = 0xFFFE;				// 最大有效节点 ID (0~0xFFFE)
+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;
@@ -215,4 +236,17 @@ constexpr int TIMER1_DEFAULT_PULSE = 0;
 constexpr int TIMER1_CH3_COMPARE_VALUE =
 	(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

@@ -477,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: 是否一次性保存

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 =
@@ -67,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

@@ -4,6 +4,7 @@
 #include "FOC.h"
 #include "adc.h"
 #include "flash.h"
+#include "systick.h"
 
 
 extern sampler sampler;
@@ -15,6 +16,10 @@ 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
@@ -90,18 +95,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(last_vbus);
 	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 {
-		// 只有在没有过热关机时才恢复
-		if (!FOC.thermal_shutdown) {
-			FOC.resume();
+		FOC.emergency_shutdown = false; // 清除紧急关机标志
+		voltage_state = VoltageState::SHUTDOWN_SAVE;
+		return;
+	}
+
+	// 状态机处理
+	switch (voltage_state) {
+	case VoltageState::NORMAL:
+		// 正常状态：无需操作
+		break;
+
+	case VoltageState::SHUTDOWN_SAVE:
+		// 关机保存状态：检查是否可以恢复
+		// 恢复条件：电压恢复到基准电压的90%以上
+		if (FOC.vbus_reference > 0 && vbus > FOC.vbus_reference * VBUS_RECOVERY_RATIO) {
+			if (!FOC.thermal_shutdown) {
+				FOC.resume();
+				voltage_state = VoltageState::NORMAL;
+			}
 		}
+		break;
+
+	default:
+		break;
 	}
 }

Robot_V2/sys/health.h

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