Report nav_state of mission executor via MAVLink

src/modules/commander/Commander.cpp

@@ -2408,9 +2408,10 @@ bool Commander::handleModeIntentionAndFailsafe()
 	_mode_management.setFailsafeState(_failsafe.selectedAction() > FailsafeBase::Action::Warn);
 	_vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(),
 			false);
-	_vehicle_status.nav_state = _mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction(
-					    _failsafe.selectedAction(), _user_mode_intention.get()));
+	_vehicle_status.nav_state =
+		_mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get()));
 	_vehicle_status.executor_in_charge = _mode_management.modeExecutorInCharge(); // Set this in sync with nav_state
+	_vehicle_status.nav_state_display = _mode_management.getNavStateDisplay(_vehicle_status.nav_state);
 
 	switch (_failsafe.selectedAction()) {
 	case FailsafeBase::Action::Disarm:

src/modules/commander/ModeManagement.cpp

@@ -519,6 +519,18 @@ int ModeManagement::modeExecutorInCharge() const
 	return _mode_executor_in_charge;
 }
 
+uint8_t ModeManagement::getNavStateDisplay(uint8_t nav_state) const
+{
+	const int executor_in_charge = modeExecutorInCharge();
+
+	if (_mode_executors.valid(executor_in_charge)) {
+		return _mode_executors.executor(executor_in_charge).owned_nav_state;
+
+	} else {
+		return nav_state;
+	}
+}
+
 bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const
 {
 	bool ret = false;

src/modules/commander/ModeManagement.hpp

@@ -150,6 +150,12 @@ public:
 	 */
 	int modeExecutorInCharge() const;
 
+	/**
+	 * Returns the executor's navigation state if active, otherwise nav_state.
+	 * Exposes the high-level goal rather than the effective sub-mode.
+	 */
+	uint8_t getNavStateDisplay(uint8_t nav_state) const;
+
 	void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override;
 	uint8_t getReplacedModeIfAny(uint8_t nav_state) override;
 
@@ -208,6 +214,7 @@ public:
 	void setFailsafeState(bool failsafe_action_active) {}
 
 	int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; }
+	uint8_t getNavStateDisplay(uint8_t nav_state) const { return nav_state; }
 
 	void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {}
 	uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; }

src/modules/mavlink/streams/CURRENT_MODE.hpp

@@ -64,10 +64,10 @@ private:
 
 		if (_vehicle_status_sub.update(&vehicle_status)) {
 			mavlink_current_mode_t current_mode{};
-			current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data;
+			current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state_display).data;
 			current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data;
-			current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state,
-						     vehicle_status.vehicle_type, vehicle_status.is_vtol);
+			current_mode.standard_mode =
+				(uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state_display, vehicle_status.vehicle_type, vehicle_status.is_vtol);
 			mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), &current_mode);
 			return true;
 		}

src/modules/mavlink/streams/HEARTBEAT.hpp

@@ -100,8 +100,8 @@ private:
 			}
 
 
-			// uint32_t custom_mode - A bitfield for use for autopilot-specific flags
-			union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)};
+			// uint32_t custom_mode - Bitfield for autopilot-specific flags
+			union px4_custom_mode custom_mode = get_px4_custom_mode(vehicle_status.nav_state_display);
 
 
 			// uint8_t system_status (MAV_STATE) - System status flag.