From 0a86e02d1773247e3c53049fb5a2e71b83933113 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Tue, 16 Feb 2016 16:14:18 -0800
Subject: [PATCH] Fix ArduPlane FLTMODE_CH usage

---
 .../APM/APMFlightModesComponentController.cc         | 12 ++++++++++--
 .../APM/APMFlightModesComponentController.h          |  1 +
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc
index 31acdf1de..c7dad8eb0 100644
--- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc
+++ b/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 };
diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h
index 66b57d200..c8a13223e 100644
--- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h
+++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h
@@ -62,6 +62,7 @@ private:
     int             _channelCount;
     QVariantList    _rgChannelOptionEnabled;
     bool            _fixedWing;
+    int             _flightModeChannel;
 };
 
 #endif
-- 
2.22.0