lingyunxsh/mqtt_power
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修复mqtt消息历史分页问题

Browse Source
This commit is contained in:
lingyunxsh
2025-05-30 18:15:27 +08:00
parent 5612b2e782
commit d0a550cab6
4 changed files with 127 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
esp32_robot_firmware/esp32_robot_firmware.ino
View File

@@ -27,47 +27,53 @@ void handle_mqtt_command(const char* commandType, const char* taskId, const char
Serial.println();
current_active_task_id = String(taskId);
const char* cmd_ack_val = nullptr; // 用于存储指令的中文ACK值
if (strcmp(commandType, "MOVE") == 0 || strcmp(commandType, "MOVE_TO_SPOT") == 0) {
cmd_ack_val = "移动到指定点";
if (targetSpotId && strlen(targetSpotId) > 0) {
if (current_robot_status == IDLE || current_robot_status == COMPLETED || current_robot_status == FAULTED) {
Serial.print("指令有效: 准备移动到 "); Serial.println(targetSpotId);
target_spot_for_move = String(targetSpotId);
// 状态将在navigation_move_to_spot开始时变为MOVING
// navigation_move_to_spot会阻塞，直到完成、失败或超时
// 注意：这里的ACK是针对指令接收无效的情况，移动完成的ACK在loop中处理
} else {
Serial.println("指令无效: 机器人当前不处于可接收MOVE指令的状态。");
mqtt_publish_ack(taskId, false, "Robot busy or not in correct state for MOVE", current_robot_status, nullptr);
mqtt_publish_ack(taskId, false, "Robot busy or not in correct state for MOVE", current_robot_status, nullptr, cmd_ack_val);
current_active_task_id = "";
}
} else {
Serial.println("指令错误: MOVE 指令缺少 targetSpotId/target_spot_uid。");
mqtt_publish_ack(taskId, false, "MOVE command missing target spot ID", current_robot_status, nullptr);
mqtt_publish_ack(taskId, false, "MOVE command missing target spot ID", current_robot_status, nullptr, cmd_ack_val);
current_active_task_id = "";
}
} else if (strcmp(commandType, "STOP_CHARGE") == 0) {
cmd_ack_val = "停止充电";
if (current_robot_status == CHARGING) {
Serial.println("指令有效: 停止充电。");
current_robot_status = COMPLETED; // 立即改变状态
relay_stop_charging();
unsigned long charge_duration = (robot_charge_start_time_ms > 0) ? (millis() - robot_charge_start_time_ms) / 1000 : 0;
robot_charge_start_time_ms = 0;
robot_current_location = navigation_get_current_spot_count() > 0 ? String(SPOT_IDS[navigation_get_current_spot_count()-1]) : robot_current_location; // 假设停在最后一个检测到的桩
mqtt_publish_ack(taskId, true, ("Charging stopped. Duration: " + String(charge_duration) + "s").c_str(), current_robot_status, robot_current_location.c_str());
// 立即发布一次状态更新
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr, nullptr, robot_current_location.c_str(), 0, 0, nullptr);
// 假设停在最后一个检测到的桩或已知位置
// robot_current_location 更新逻辑可以更精细，例如，如果充电时就知道spotId
mqtt_publish_ack(taskId, true, ("Charging stopped. Duration: " + String(charge_duration) + "s").c_str(), current_robot_status, robot_current_location.c_str(), cmd_ack_val);
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr, nullptr, robot_current_location.c_str(), 0, 0, nullptr, nullptr);
} else {
Serial.println("指令无效: 机器人未在充电状态。");
mqtt_publish_ack(taskId, false, "Robot not in CHARGING state for STOP_CHARGE", current_robot_status, nullptr);
mqtt_publish_ack(taskId, false, "Robot not in CHARGING state for STOP_CHARGE", current_robot_status, nullptr, cmd_ack_val);
}
current_active_task_id = ""; // STOP_CHARGE任务通常是瞬时完成
} else if (strcmp(commandType, "START_CHARGE") == 0) { // 保留旧指令处理的ACK，但业务上已忽略
} else if (strcmp(commandType, "START_CHARGE") == 0) {
cmd_ack_val = "开始充电";
Serial.println("收到 START_CHARGE 指令，但业务逻辑为到达即充，仅ACK。");
mqtt_publish_ack(taskId, true, "START_CHARGE acknowledged (auto-charge on arrival)", current_robot_status, robot_current_location.c_str());
mqtt_publish_ack(taskId, true, "START_CHARGE acknowledged (auto-charge on arrival)", current_robot_status, robot_current_location.c_str(), cmd_ack_val);
current_active_task_id = "";
} else {
Serial.print("未知指令类型: "); Serial.println(commandType);
mqtt_publish_ack(taskId, false, "Unknown command type", current_robot_status, nullptr);
// 对于未知指令，不确定后端是否能处理没有正确 command_ack 的ACK，或者是否应该发送ACK
// 为安全起见，可以不发送，或发送一个带通用错误信息的（但后端可能不认）
// mqtt_publish_ack(taskId, false, "Unknown command type", current_robot_status, nullptr, "未知指令");
current_active_task_id = "";
}
}
@@ -109,31 +115,40 @@ void loop() {
// --- 状态机和任务处理 ---
if (target_spot_for_move.length() > 0 && (current_robot_status == IDLE || current_robot_status == COMPLETED || current_robot_status == FAULTED) ) {
Serial.print("开始执行移动任务到: "); Serial.println(target_spot_for_move);
current_robot_status = MOVING; // 明确在开始导航前设置状态为MOVING
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, current_active_task_id.c_str(), target_spot_for_move.c_str(), nullptr, 0, 0, nullptr, nullptr); // 上报MOVING状态
bool move_success = navigation_move_to_spot(target_spot_for_move.c_str(), &current_robot_status, &robot_current_location);
const char* move_cmd_ack_val = "移动到指定点"; // MOVE 指令的中文ACK值
if (move_success) {
Serial.println("移动成功，准备充电。");
current_robot_status = CHARGING; // 到达即充电
// current_robot_status 已在 navigation_move_to_spot 中被设为 CHARGING (如果它内部这么做)
// 或者在这里强制设置，确保到达即充逻辑
current_robot_status = CHARGING;
robot_charge_start_time_ms = millis();
relay_start_charging();
mqtt_publish_ack(current_active_task_id.c_str(), true, "Robot arrived and started charging.", current_robot_status, robot_current_location.c_str());
// robot_current_location 应该由 navigation_move_to_spot 更新为实际到达的spotId
mqtt_publish_ack(current_active_task_id.c_str(), true, "Robot arrived and started charging.", current_robot_status, robot_current_location.c_str(), move_cmd_ack_val);
} else {
Serial.println("移动失败或超时。");
// current_robot_status 已经在 navigation_move_to_spot 中被设为 FAULTED (如果超时)
// 如果不是超时而是其他导航错误，可能需要在这里显式设置为FAULTED
if(current_robot_status != FAULTED) current_robot_status = FAULTED;
mqtt_publish_ack(current_active_task_id.c_str(), false, "Failed to reach target spot.", current_robot_status, robot_current_location.c_str());
// current_robot_status 应该已在 navigation_move_to_spot 中被设为 FAULTED
if(current_robot_status != FAULTED) { // 以防万一，如果导航模块未正确设置
current_robot_status = FAULTED;
// robot_current_location 可能还是旧的或 EN_ROUTE_TO_...
}
mqtt_publish_ack(current_active_task_id.c_str(), false, "Failed to reach target spot.", current_robot_status, robot_current_location.c_str(), move_cmd_ack_val);
}
target_spot_for_move = ""; // 清除移动目标
current_active_task_id = ""; // 清除任务ID
// 立即上报一次状态
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr,
nullptr, // target_spot (已到达或失败)
(current_robot_status == CHARGING) ? robot_current_location.c_str() : nullptr, // current_spot
0, // duration (刚开始充电是0)
(current_robot_status == FAULTED) ? 1 : 0, // errorCode
(current_robot_status == FAULTED) ? "Navigation failed" : nullptr // errorMessage
);
// current_active_task_id = ""; // 在ACK发送后清除，确保ACK能使用正确的ID。如果ACK失败，任务ID可能还需要。
// 考虑到ACK是即时发送，可以在这里清除
if(current_active_task_id.length() > 0 && (strcmp(current_active_task_id.c_str(), "") != 0) ) {
// 只有在 active_task_id 有效时才清除，避免重复清除或错误清除
}
current_active_task_id = ""; // 移动任务完成后清除
// 立即发布一次最终状态
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr, nullptr, (current_robot_status == CHARGING || current_robot_status == COMPLETED || current_robot_status == FAULTED) ? robot_current_location.c_str() : nullptr, (current_robot_status == CHARGING) ? (millis()-robot_charge_start_time_ms)/1000 : 0, (current_robot_status == FAULTED) ? 1 : 0 /*示例错误码*/, (current_robot_status == FAULTED) ? "Navigation or other error" : nullptr, nullptr);
}
// --- 定期任务 ---