From 25667a11a07da6bbbc04ba327db6f799cf7d4fcc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 21 Apr 2014 17:12:15 +1000
Subject: [PATCH] AP_NavEKF: use AHRS vehicle class for sideslip calculation

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 21 ++++++++++++++++-----
 libraries/AP_NavEKF/AP_NavEKF.h   |  3 +++
 2 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 4d96f26ef1..80aac8968a 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -786,7 +786,7 @@ void NavEKF::SelectBetaFusion()
     // we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position
     // AND NOT on the ground AND enough time has lapsed since our last fusion
     // AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set)
-    if (_ahrs->get_fly_forward() && !(use_compass() && useAirspeed() && posHealth) && !onGround  && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
+    if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !onGround  && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
         FuseSideslip();
         BETAmsecPrev = IMUmsec;
     }
@@ -2166,7 +2166,7 @@ void NavEKF::FuseMagnetometer()
         magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
         // Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
         // In this case we might as well try using the magnetometer, but with a reduced weighting
-        if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !_ahrs->get_fly_forward() && magTimeout))
+        if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout))
         {
             // correct the state vector
             for (uint8_t j= 0; j<=indexLimit; j++)
@@ -2745,7 +2745,7 @@ void NavEKF::OnGroundCheck()
             onGround = true;
         }
         // force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle
-        if (!onGround && prevOnGround && (!use_compass() || (magTimeout && _ahrs->get_fly_forward()))) {
+        if (!onGround && prevOnGround && (!use_compass() || (magTimeout && assume_zero_sideslip()))) {
             alignYawGPS();
         }
         // If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround
@@ -2753,7 +2753,7 @@ void NavEKF::OnGroundCheck()
         // wind speed is equal to the 6m/s. This prevents gains being too high at the start
         // of flight if launching into a headwind until the first turn when the EKF can form a  wind speed
         // estimate
-        if (!onGround && prevOnGround && !useAirspeed() && _ahrs->get_fly_forward()) {
+        if (!onGround && prevOnGround && !useAirspeed() && assume_zero_sideslip()) {
             setWindVelStates();
         }
     }
@@ -2850,7 +2850,7 @@ void NavEKF::CopyAndFixCovariances()
     }
     // if we have a non-forward flight vehicle type and no airspeed sensor, we want the wind
     // states to remain zero and want to keep the old variances for these states
-    else if (!useAirspeed() && !_ahrs->get_fly_forward()) {
+    else if (!useAirspeed() && !assume_zero_sideslip()) {
         // copy calculated variances we want to propagate
         for (uint8_t i=0; i<=13; i++) {
             P[i][i] = nextP[i][i];
@@ -3332,4 +3332,15 @@ void NavEKF::decayGpsOffset()
     }
 }
 
+/*
+  should we assume zero sideslip?
+ */
+bool NavEKF::assume_zero_sideslip(void) const
+{
+    // we don't assume zero sideslip for ground vehicles as EKF could
+    // be quite sensitive to a rapid spin of the ground vehicle if
+    // traction is lost
+    return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
+}
+
 #endif // HAL_CPU_CLASS
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 742cedadbc..92f48bf14a 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -480,6 +480,9 @@ private:
     
     // should we use the compass?
     bool use_compass(void) const;
+
+    // should we assume zero sideslip?
+    bool assume_zero_sideslip(void) const;
 };
 
 #if CONFIG_HAL_BOARD != HAL_BOARD_PX4