Merge pull request #2825 from DonLakeFlyer/FLTMODE_CH

Fix ArduPlane FLTMODE_CH usage

src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc

@@ -32,6 +32,8 @@ APMFlightModesComponentController::APMFlightModesComponentController(void)
     : _activeFlightMode(0)
     , _channelCount(Vehicle::cMaxRcChannels)
     , _fixedWing(_vehicle->vehicleType() == MAV_TYPE_FIXED_WING)
+    , _flightModeChannel(4)
+
 {
     QStringList usedParams;
     usedParams << QStringLiteral("FLTMODE1") << QStringLiteral("FLTMODE2") << QStringLiteral("FLTMODE3")
@@ -40,6 +42,10 @@ APMFlightModesComponentController::APMFlightModesComponentController(void)
         return;
     }
 
+    if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("FLTMODE_CH"))) {
+        _flightModeChannel = getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FLTMODE_CH"))->rawValue().toInt();
+    }
+
     _rgChannelOptionEnabled << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false);
     
     connect(_vehicle, &Vehicle::rcChannelsChanged, this, &APMFlightModesComponentController::_rcChannelsChanged);
@@ -48,10 +54,12 @@ APMFlightModesComponentController::APMFlightModesComponentController(void)
 /// Connected to Vehicle::rcChannelsChanged signal
 void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
 {
-    Q_UNUSED(channelCount);
+    if (_flightModeChannel > channelCount) {
+        return;
+    }
 
     _activeFlightMode = 0;
-    int channelValue = pwmValues[4];
+    int channelValue = pwmValues[_flightModeChannel - 1];
     if (channelValue != -1) {
         bool found = false;
         int rgThreshold[] = { 1230, 1360, 1490, 1620, 1749 };

src/AutoPilotPlugins/APM/APMFlightModesComponentController.h

@@ -62,6 +62,7 @@ private:
     int             _channelCount;
     QVariantList    _rgChannelOptionEnabled;
     bool            _fixedWing;
+    int             _flightModeChannel;
 };
 
 #endif