From 629a5fd7143c8eae74c69449744ace5ea8ae6133 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 19 Aug 2015 12:19:47 +0900
Subject: [PATCH] NavEKF: IMUSwitchState enum

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 41 ++++++++++++++++++-------------
 libraries/AP_NavEKF/AP_NavEKF.h   |  7 +++++-
 2 files changed, 30 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 1e243c455a..a27780cde5 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -1130,9 +1130,9 @@ void NavEKF::UpdateStrapdownEquationsNED()
 
     // use weighted average of both IMU units for delta velocities
     // Over-ride accelerometer blend weighting using a hard switch based on the IMU consistency and vibration monitoring checks
-    if (lastImuSwitchState == 1) {
+    if (lastImuSwitchState == IMUSWITCH_IMU0) {
         IMU1_weighting = 1.0f;
-    } else if (lastImuSwitchState == 2) {
+    } else if (lastImuSwitchState == IMUSWITCH_IMU1) {
         IMU1_weighting = 0.0f;
     }
     correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting);
@@ -4150,29 +4150,29 @@ void NavEKF::readIMUData()
 
         // Check the difference for excessive error and use the IMU with less noise
         // Apply hysteresis to prevent rapid switching
-        if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != 0)) {
-            if (lastImuSwitchState == 0) {
+        if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != IMUSWITCH_MIXED)) {
+            if (lastImuSwitchState == IMUSWITCH_MIXED) {
                 // no previous fail so switch to the IMU with least noise
                 if (imuNoiseFiltState1 < imuNoiseFiltState2) {
-                    lastImuSwitchState = 1;
+                    lastImuSwitchState = IMUSWITCH_IMU0;
                 } else {
-                    lastImuSwitchState = 2;
+                    lastImuSwitchState = IMUSWITCH_IMU1;
                 }
-            } else if (lastImuSwitchState == 1) {
+            } else if (lastImuSwitchState == IMUSWITCH_IMU0) {
                 // IMU1 previously failed so require 5 m/s/s less noise on IMU2 to switch across
                 if (imuNoiseFiltState1 - imuNoiseFiltState2 > 5.0f) {
                     // IMU2 is significantly less noisy, so switch
-                    lastImuSwitchState = 2;
+                    lastImuSwitchState = IMUSWITCH_IMU1;
                 }
             } else {
                 // IMU2 previously failed so require 5 m/s/s less noise on IMU1 to switch across
                 if (imuNoiseFiltState2 - imuNoiseFiltState1 > 5.0f) {
                     // IMU1 is significantly less noisy, so switch
-                    lastImuSwitchState = 1;
+                    lastImuSwitchState = IMUSWITCH_IMU0;
                 }
             }
         } else {
-            lastImuSwitchState = 0;
+            lastImuSwitchState = IMUSWITCH_MIXED;
         }
 
     } else {
@@ -4181,16 +4181,23 @@ void NavEKF::readIMUData()
         // set the switch state based on the IMU we are using to make the data source selection visible
         if (ins.use_accel(0)) {
             readDeltaVelocity(0, dVelIMU1, dtDelVel1);
-            lastImuSwitchState = 1;
+            lastImuSwitchState = IMUSWITCH_IMU0;
         } else if (ins.use_accel(1)) {
             readDeltaVelocity(1, dVelIMU1, dtDelVel1);
-            lastImuSwitchState = 2;
+            lastImuSwitchState = IMUSWITCH_IMU1;
         } else {
             readDeltaVelocity(ins.get_primary_accel(), dVelIMU1, dtDelVel1);
-            if (ins.get_primary_accel() < 2) {
-                lastImuSwitchState = ins.get_primary_accel() + 1;
-            } else {
-                lastImuSwitchState = 0;
+            switch (ins.get_primary_accel()) {
+                case 0:
+                    lastImuSwitchState = IMUSWITCH_IMU0;
+                    break;
+                case 1:
+                    lastImuSwitchState = IMUSWITCH_IMU1;
+                    break;
+                default:
+                    // we must be using IMU2 which can't be properly represented so we set to "mixed"
+                    lastImuSwitchState = IMUSWITCH_MIXED;
+                    break;
             }
         }
         dtDelVel2 = dtDelVel1;
@@ -4770,7 +4777,7 @@ void NavEKF::InitialiseVariables()
     yawResetAngleWaiting = false;
     imuNoiseFiltState1 = 0.0f;
     imuNoiseFiltState2 = 0.0f;
-    lastImuSwitchState = 0;
+    lastImuSwitchState = IMUSWITCH_MIXED;
 }
 
 // return true if we should use the airspeed sensor
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index d405275b2a..071439881d 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -773,7 +773,12 @@ private:
     float imuNoiseFiltState1;       // peak hold noise estimate for IMU 1
     float imuNoiseFiltState2;       // peak hold noise estimate for IMU 2
     Vector3f accelDiffFilt;         // filtered difference between IMU 1 and 2
-    uint8_t lastImuSwitchState;     // last switch state, 0=normal, 1 = use IMU1, 2 = use IMU2
+    enum ImuSwitchState {
+        IMUSWITCH_MIXED=0,          // IMU 0 & 1 are mixed
+        IMUSWITCH_IMU0,             // only IMU 0 is used
+        IMUSWITCH_IMU1              // only IMU 1 is used
+    };
+    ImuSwitchState lastImuSwitchState;  // last switch state (see imuSwitchState enum)
 
     // states held by optical flow fusion across time steps
     // optical flow X,Y motion compensated rate measurements are fused across two time steps