diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 1a46ccd..2c47229 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/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 速度开环控制 * diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 601692f..f87a46e 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/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 = diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index f6df0bb..1322d0f 100644 --- a/Robot_V2/device/Can.cpp +++ b/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,29 +1197,50 @@ 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); can_flag_clear(CAN0, CAN_FLAG_TME2); } -} \ No newline at end of file +} diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 488d078..2e67bc2 100644 --- a/Robot_V2/main.cpp +++ b/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) { diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index 6647874..7e17ba6 100644 --- a/Robot_V2/sys/config.h +++ b/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 \ No newline at end of file diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 8151865..88b765e 100644 --- a/Robot_V2/sys/flash.cpp +++ b/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, ¤t_id, ¤t_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: 是否一次性保存 diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h index 8989761..18b75c1 100644 --- a/Robot_V2/sys/flash.h +++ b/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); }; diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index 64ca22f..74fc56f 100644 --- a/Robot_V2/sys/health.cpp +++ b/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; } } diff --git a/Robot_V2/sys/health.h b/Robot_V2/sys/health.h index 81925b4..3c3cc00 100644 --- a/Robot_V2/sys/health.h +++ b/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;