From 2257e73f3e9cc12c6e58ae05197f2b7598ebd263 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 30 Jan 2025 16:09:52 +0100 Subject: [PATCH] Fill out MISSION_CURRENT.mission_state over MAVlink I'm not sure about all cases, this needs further clarification and testing. --- msg/MissionResult.msg | 2 -- src/modules/mavlink/mavlink_mission.cpp | 40 ++++++++++++++++++------- src/modules/mavlink/mavlink_mission.h | 2 +- 3 files changed, 31 insertions(+), 13 deletions(-) diff --git a/msg/MissionResult.msg b/msg/MissionResult.msg index f70326be3b..b18c574fea 100644 --- a/msg/MissionResult.msg +++ b/msg/MissionResult.msg @@ -16,5 +16,3 @@ bool failure # true if the mission cannot continue or be completed for some re bool item_do_jump_changed # true if the number of do jumps remaining has changed uint16 item_changed_index # indicate which item has changed uint16 item_do_jump_remaining # set to the number of do jumps remaining for that item - -uint8 execution_mode # indicates the mode in which the mission is executed diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 36d4453344..60aa976247 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -276,15 +276,35 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t } void -MavlinkMissionManager::send_mission_current(uint16_t seq) +MavlinkMissionManager::send_mission_current(uint16_t seq, mission_result_s mission_result) { - mavlink_mission_current_t wpc{}; - wpc.seq = seq; - wpc.total = _count[MAV_MISSION_TYPE_MISSION] > 0 ? _count[MAV_MISSION_TYPE_MISSION] : UINT16_MAX; - wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; - wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE]; - wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY]; - mavlink_msg_mission_current_send_struct(_mavlink.get_channel(), &wpc); + mavlink_mission_current_t mavlink_mission_current{}; + mavlink_mission_current.seq = seq; + mavlink_mission_current.total = _count[MAV_MISSION_TYPE_MISSION] > 0 ? _count[MAV_MISSION_TYPE_MISSION] : UINT16_MAX; + + if (!mission_result.valid) { + mavlink_mission_current.mission_state = MISSION_STATE::MISSION_STATE_NO_MISSION; + + } else if (mission_result.valid && mission_result.seq_current == 0) { + mavlink_mission_current.mission_state = MISSION_STATE::MISSION_STATE_NOT_STARTED; + + } else if (mission_result.valid && mission_result.seq_current > 0) { + mavlink_mission_current.mission_state = MISSION_STATE::MISSION_STATE_ACTIVE; + + } else if (false/* Mission mode only? mission result is also updated by land, takeoff, VTOL takeoff */) { + mavlink_mission_current.mission_state = MISSION_STATE::MISSION_STATE_PAUSED; + + } else if (mission_result.finished) { + mavlink_mission_current.mission_state = MISSION_STATE::MISSION_STATE_COMPLETE; + + } else { + mavlink_mission_current.mission_state = MISSION_STATE::MISSION_STATE_UNKNOWN; + } + + mavlink_mission_current.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; + mavlink_mission_current.fence_id = _crc32[MAV_MISSION_TYPE_FENCE]; + mavlink_mission_current.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY]; + mavlink_msg_mission_current_send_struct(_mavlink.get_channel(), &mavlink_mission_current); PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq); } @@ -506,7 +526,7 @@ MavlinkMissionManager::send() if (mission_result.seq_total > 0) { if (mission_result.seq_current < mission_result.seq_total) { - send_mission_current(_current_seq); + send_mission_current(_current_seq, mission_result); } else { _mavlink.send_statustext_critical("ERROR: wp index out of bounds\t"); @@ -541,7 +561,7 @@ MavlinkMissionManager::send() } } else if (_slow_rate_limiter.check(hrt_absolute_time())) { - send_mission_current(_current_seq); + send_mission_current(_current_seq, mission_result); if ((_count[MAV_MISSION_TYPE_MISSION] > 0) && (_current_seq >= 0)) { // send the reached message another 10 times diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 0b5c6e25d8..4af068726d 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -201,7 +201,7 @@ private: * * @param seq The waypoint sequence number the MAV should fly to. */ - void send_mission_current(uint16_t seq); + void send_mission_current(uint16_t seq, mission_result_s mission_result); void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type, uint32_t opaque_id);