From 861a2d071165118967027b693ecd087caccdd358 Mon Sep 17 00:00:00 2001
From: Nate Weibley <nate.weibley@prioria.com>
Date: Wed, 13 Apr 2016 15:33:05 -0400
Subject: [PATCH] Fix HIL mode not being properly detected for #3226

---
 src/Vehicle/Vehicle.h |  2 ++
 src/uas/UAS.cc        | 11 +++++------
 src/uas/UAS.h         |  2 --
 3 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index f0066eefd..7e3089f83 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -494,6 +494,8 @@ public:
     uint            messagesLost        () { return _messagesLost; }
     bool            flying              () const { return _flying; }
     bool            guidedMode          () const;
+    uint8_t         baseMode            () const { return _base_mode; }
+    uint32_t        customMode          () const { return _custom_mode; }
 
     Fact* roll              (void) { return &_rollFact; }
     Fact* heading           (void) { return &_headingFact; }
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 1dfa1b48f..ddfc7831f 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -58,8 +58,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
     receiveDropRate(0),
     sendDropRate(0),
 
-    base_mode(0),
-    custom_mode(0),
     status(-1),
 
     startTime(QGC::groundTimeMilliseconds()),
@@ -1440,6 +1438,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
     if (!_vehicle) {
         return;
     }
+    const uint8_t base_mode = _vehicle->baseMode();
 
    // If system has manual inputs enabled and is armed
     if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
@@ -1686,7 +1685,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
         return;
     }
 
-    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
+    if (_vehicle->hilMode())
     {
         float q[4];
 
@@ -1766,7 +1765,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
         return;
     }
 
-    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
+    if (_vehicle->hilMode())
     {
         float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
         float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var);
@@ -1817,7 +1816,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
     Q_UNUSED(quality);
     Q_UNUSED(ground_distance);
 
-    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
+    if (_vehicle->hilMode())
     {
 #if 0
         mavlink_message_t msg;
@@ -1849,7 +1848,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
     if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
         return;
 
-    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
+    if (_vehicle->hilMode())
     {
         float course = cog;
         // map to 0..2pi
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 1a859f6ff..c527f5eda 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -370,8 +370,6 @@ protected: //COMMENTS FOR TEST UNIT
     float sendDropRate;           ///< Percentage of packets that were not received from the MAV by the GCS
 
     /// BASIC UAS TYPE, NAME AND STATE
-    uint8_t base_mode;                 ///< The current mode of the MAV
-    uint32_t custom_mode;         ///< The current mode of the MAV
     int status;                   ///< The current status of the MAV
 
     /// TIMEKEEPING
-- 
2.22.0