Standard Modes: Fix various problems with implementation

[DonLakeFlyer]

• Was using cannotBeSet to filter things like Land/Takeoff from list. The goal was to filter the flight mode dropdown. But this in turn filters the list from all users. Including Joystick buttons actions which is incorrect.
• We should pay attention to the advanced attribute and use that to initially hide these modes from the flight mode dropdown. With the edit modes option allowing you to bring them back for display. This should not affect joystick actions which should always get the full list.
• Protocol should work with generic firmware. Need to remove the unnecessary ties to FirmwarePlugin. Related to Standard modes doesn't work for 'generic' autopilot #13472

[ikalnytskyi]

Hey @DonLakeFlyer,

I understand that implementing proper support for standard modes can be challenging, since the current codebase isn't structured for it yet. It's also not entirely clear how to report standard modes via HEARTBEAT, or when to use MAV_CMD_DO_SET_HOME versus MAV_CMD_DO_SET_STANDARD_MODE in autopilot agnostic way.

However, I found that it might be relatively straightforward to add support for custom modes through the Standard Modes protocol. We could reuse the implementation from the ArduPilot firmware plugin — it seems to work well with my firmware. QGC correctly shows the available modes in the dropdown and can set them using MAV_CMD_DO_SET_HOME. I've included the patch I tested below.

Would this be something you'd consider merging into the codebase? Or do you have alternative ideas on how to properly support standard modes? Or maybe this kind of logic should go to some Generic firmware plugin, not the base class?

diff --git src/FirmwarePlugin/FirmwarePlugin.cc src/FirmwarePlugin/FirmwarePlugin.cc
index 73aef0074..7322bdfdf 100644
--- src/FirmwarePlugin/FirmwarePlugin.cc
+++ src/FirmwarePlugin/FirmwarePlugin.cc
@@ -42,6 +42,18 @@ AutoPilotPlugin *FirmwarePlugin::autopilotPlugin(Vehicle *vehicle) const
     return new GenericAutoPilotPlugin(vehicle, vehicle);
 }

+QStringList FirmwarePlugin::flightModes(Vehicle *vehicle) const
+{
+    Q_UNUSED(vehicle)
+    QStringList flightModesList;
+    for (const FirmwareFlightMode &mode : _flightModeList) {
+        if (mode.canBeSet){
+            flightModesList += mode.mode_name;
+        }
+    }
+    return flightModesList;
+}
+
 QString FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
 {
     Q_UNUSED(custom_mode);
@@ -80,13 +92,21 @@ QString FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) cons

 bool FirmwarePlugin::setFlightMode(const QString &flightMode, uint8_t *base_mode, uint32_t *custom_mode) const
 {
-    Q_UNUSED(flightMode);
-    Q_UNUSED(base_mode);
-    Q_UNUSED(custom_mode);
+    *base_mode = 0;
+    *custom_mode = 0;

-    qCWarning(FirmwarePluginLog) << "FirmwarePlugin::setFlightMode called on base class, not supported";
+    bool found = false;

-    return false;
+    for (const FirmwareFlightMode &mode: _flightModeList){
+        if (flightMode.compare(mode.mode_name, Qt::CaseInsensitive) == 0){
+            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+            *custom_mode = mode.custom_mode;
+            found = true;
+            break;
+        }
+    }
+
+    return found;
 }

 QString FirmwarePlugin::missionCommandOverrides(QGCMAVLink::VehicleClass_t vehicleClass) const
diff --git src/FirmwarePlugin/FirmwarePlugin.h src/FirmwarePlugin/FirmwarePlugin.h
index a170d5174..aa5122dd4 100644
--- src/FirmwarePlugin/FirmwarePlugin.h
+++ src/FirmwarePlugin/FirmwarePlugin.h
@@ -117,7 +117,7 @@ class FirmwarePlugin : public QObject

     /// Returns the list of available flight modes for the Fly View dropdown. This may or may not be the full
     /// list available from the firmware. Call will be made again if advanced mode changes.
-    virtual QStringList flightModes(Vehicle* /*vehicle*/) const { return QStringList(); }
+    virtual QStringList flightModes(Vehicle* /*vehicle*/) const;

     /// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable.
     ///     @param base_mode Base mode from mavlink HEARTBEAT message
@@ -130,7 +130,7 @@ class FirmwarePlugin : public QObject
     virtual bool setFlightMode(const QString &flightMode, uint8_t *base_mode, uint32_t *custom_mode) const;

     /// returns true if this flight stack supports MAV_CMD_DO_SET_MODE
-    virtual bool MAV_CMD_DO_SET_MODE_is_supported() const { return false; }
+    virtual bool MAV_CMD_DO_SET_MODE_is_supported() const { return !_flightModeList.empty(); }

     /// Returns The flight mode which indicates the vehicle is paused
     virtual QString pauseFlightMode() const { return QString(); }

[DonLakeFlyer]

Would this be something you'd consider merging into the codebase?

I don't see this as a good change. There is already a mavlink protocol for this. No reason to hack in something vehicle side.

[DonLakeFlyer]

I don't see this as a good change. There is already a mavlink protocol for this. No reason to hack in something vehicle side.

Sorry, looking at wrong thing. My comment is wrong.

[DonLakeFlyer]

So your change is close but I think there are still problems with it. I'm going to have to get back to this in 5.2. It's been a long slog just getting the firware specific implementation to work correctly. I don't want to screw that up just be a new stable is getting readied. Sorry about that.