From 0180ad3a630e256693eca8e9546881bb17f9f910 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 13 Jun 2025 13:38:43 +1200
Subject: [PATCH] mavlink: implement extended MISSION_CURRENT

The message MISSION_CURRENT got extensions regarding mission state in
https://github.com/mavlink/mavlink/pull/1869

This is an attempt to actually populate those fields.
---
 src/modules/mavlink/mavlink_mission.cpp | 54 +++++++++++++++++++++++++
 src/modules/mavlink/mavlink_mission.h   | 13 ++++++
 2 files changed, 67 insertions(+)

diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index c24796b19d..00a466b2d5 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -278,9 +278,14 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
 void
 MavlinkMissionManager::send_mission_current(uint16_t seq)
 {
+	// Update mission state before sending
+	update_mission_state();
+
 	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_state = static_cast<uint8_t>(_mission_state);
+	wpc.mission_mode = static_cast<uint8_t>(_mission_mode);
 	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];
@@ -911,6 +916,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 			_transfer_partner_compid = msg->compid;
 			_mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
 			_transfer_current_crc32 = 0;
+			_last_reached = -1; // Reset last reached waypoint when new mission starts
 
 			if (wpc.count > current_max_item_count()) {
 				PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count());
@@ -1907,3 +1913,51 @@ uint32_t MavlinkMissionManager::crc32_for_mission_item(const mavlink_mission_ite
 
 	return crc32part(u.raw, sizeof(u), prev_crc32);
 }
+
+void
+MavlinkMissionManager::update_mission_state()
+{
+	// Get vehicle status
+	vehicle_status_s vehicle_status;
+
+	if (!_vehicle_status_sub.update(&vehicle_status)) {
+		return;
+	}
+
+	// Get mission result
+	mission_result_s mission_result;
+
+	if (!_mission_result_sub.update(&mission_result)) {
+		return;
+	}
+
+	// Update mission mode
+	if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
+	    vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+	    vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+		_mission_mode = MISSION_MODE_ACTIVE;
+
+	} else {
+		_mission_mode = MISSION_MODE_SUSPENDED;
+	}
+
+	// Update mission state
+	if (_count[MAV_MISSION_TYPE_MISSION] == 0) {
+		_mission_state = MISSION_STATE_NO_MISSION;
+
+	} else if (mission_result.finished) {
+		// Mission is complete if the navigator says it's finished
+		_mission_state = MISSION_STATE_COMPLETE;
+
+	} else if (_mission_mode == MISSION_MODE_ACTIVE
+		   && vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		_mission_state = MISSION_STATE_ACTIVE;
+
+	} else if (_mission_mode == MISSION_MODE_SUSPENDED && _last_reached >= 0) {
+		// Only PAUSED if we were actually in the middle of a mission
+		_mission_state = MISSION_STATE_PAUSED;
+
+	} else {
+		_mission_state = MISSION_STATE_NOT_STARTED;
+	}
+}
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 0b5c6e25d8..2c26dec266 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -49,6 +49,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_rate_limiter.h"
@@ -60,6 +61,13 @@ enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_ENUM_END
 };
 
+// Mission mode states
+enum MISSION_MODE {
+	MISSION_MODE_UNKNOWN = 0,
+	MISSION_MODE_ACTIVE = 1,
+	MISSION_MODE_SUSPENDED = 2
+};
+
 enum MAVLINK_WPM_CODES {
 	MAVLINK_WPM_CODE_OK = 0,
 	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
@@ -94,9 +102,13 @@ public:
 private:
 	enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE};	///< Current state
 	enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION};	///< mission type of current transmission (only one at a time possible)
+	enum MISSION_STATE _mission_state {MISSION_STATE_UNKNOWN}; ///< Current mission state machine state
+	enum MISSION_MODE _mission_mode {MISSION_MODE_UNKNOWN}; ///< Current mission mode
 
 	DatamanClient _dataman_client{};
 
+	void update_mission_state();
+
 	uint64_t		_time_last_recv{0};
 	uint64_t		_time_last_sent{0};
 
@@ -138,6 +150,7 @@ private:
 
 	uORB::Subscription	_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<mission_s> 	_mission_sub{ORB_ID(mission)};
+	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};	///< vehicle status subscription
 
 	uORB::Publication<mission_s>	_offboard_mission_pub{ORB_ID(mission)};