commander+mavlink: implement MAVLink standard modes

src/modules/commander/Commander.cpp

@@ -48,6 +48,8 @@
 #include "px4_custom_mode.h"
 #include "ModeUtil/control_mode.hpp"
 #include "ModeUtil/conversions.hpp"
+#include <lib/modes/ui.hpp>
+#include <lib/modes/standard_modes.hpp>
 
 /* PX4 headers */
 #include <drivers/drv_hrt.h>
@@ -1403,6 +1405,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			break;
 		}
 
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: {
+			mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1);
+			uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode);
+
+			if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) {
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
+
+			} else {
+				if (_user_mode_intention.change(nav_state, getSourceFromCommand(cmd))) {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				}
+			}
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
 		_health_and_arming_checks.update(true);
 		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -1864,6 +1884,7 @@ void Commander::run()
 			updateControlMode();
 
 			// vehicle_status publish (after prearm/preflight updates above)
+			_mode_management.getModeStatus(_vehicle_status.valid_nav_states_mask, _vehicle_status.can_set_nav_states_mask);
 			_vehicle_status.timestamp = hrt_absolute_time();
 			_vehicle_status_pub.publish(_vehicle_status);
 

src/modules/commander/ModeManagement.cpp

@@ -534,4 +534,30 @@ void ModeManagement::checkConfigOverrides()
 	}
 }
 
+void ModeManagement::getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const
+{
+	valid_nav_state_mask = mode_util::getValidNavStates();
+	can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION);
+
+	// Add external modes
+	for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
+		if (_modes.valid(i)) {
+			valid_nav_state_mask |= 1u << i;
+			can_set_nav_state_mask |= 1u << i;
+			const Modes::Mode &cur_mode = _modes.mode(i);
+
+			if (cur_mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) {
+				// Hide the internal mode if it's replaced
+				can_set_nav_state_mask &= ~(1u << cur_mode.replaces_nav_state);
+			}
+
+		} else {
+			// Still set the mode as valid but not as selectable. This is because an external mode could still
+			// be selected via RC when not yet running, so we make sure to display some mode label indicating it's not
+			// available.
+			valid_nav_state_mask |= 1u << i;
+		}
+	}
+}
+
 #endif /* CONSTRAINED_FLASH */

src/modules/commander/ModeManagement.hpp

@@ -42,7 +42,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/config_overrides.h>
 
-#include "ModeUtil/ui.hpp"
+#include <lib/modes/ui.hpp>
 #include "UserModeIntention.hpp"
 #include "HealthAndArmingChecks/checks/externalChecks.hpp"
 
@@ -155,6 +155,7 @@ public:
 
 	void printStatus() const;
 
+	void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const;
 
 	void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out);
 

src/modules/commander/ModeUtil/conversions.hpp

@@ -98,38 +98,4 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 	return navigation_mode_t::unknown;
 }
 
-const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
-	"MANUAL",
-	"ALTCTL",
-	"POSCTL",
-	"AUTO_MISSION",
-	"AUTO_LOITER",
-	"AUTO_RTL",
-	"6: unallocated",
-	"7: unallocated",
-	"AUTO_LANDENGFAIL",
-	"9: unallocated",
-	"ACRO",
-	"11: UNUSED",
-	"DESCEND",
-	"TERMINATION",
-	"OFFBOARD",
-	"STAB",
-	"16: UNUSED2",
-	"AUTO_TAKEOFF",
-	"AUTO_LAND",
-	"AUTO_FOLLOW_TARGET",
-	"AUTO_PRECLAND",
-	"ORBIT",
-	"AUTO_VTOL_TAKEOFF",
-	"EXTERNAL1",
-	"EXTERNAL2",
-	"EXTERNAL3",
-	"EXTERNAL4",
-	"EXTERNAL5",
-	"EXTERNAL6",
-	"EXTERNAL7",
-	"EXTERNAL8",
-};
-
 } // namespace mode_util

src/modules/commander/px4_custom_mode.h

@@ -50,7 +50,8 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 	PX4_CUSTOM_MAIN_MODE_STABILIZED,
 	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
-	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_TERMINATION
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -139,7 +140,7 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_TERMINATION;
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: