From 66a9093cd0386f27b5f67d006a00dae05300519a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 30 Nov 2016 15:54:55 +0900
Subject: [PATCH] AP_NavEKF2: getRangeBeaconDebug returns false on failure to
 get beacon data

---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp         |  6 ++++--
 libraries/AP_NavEKF2/AP_NavEKF2.h           |  3 ++-
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 17 ++++-------------
 libraries/AP_NavEKF2/AP_NavEKF2_core.h      |  2 +-
 4 files changed, 11 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 098de552ff..c2e6ef750e 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -1077,11 +1077,13 @@ void NavEKF2::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
 }
 
 // return data for debugging range beacon fusion
-void NavEKF2::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
+bool NavEKF2::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
 {
     if (instance < 0 || instance >= num_cores) instance = primary;
     if (core) {
-        core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow);
+        return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow);
+    } else {
+        return false;
     }
 }
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index fac9c34e9f..6dd151c27b 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -216,8 +216,9 @@ public:
         innovVar : innovation variance (m^2)
         testRatio : innovation consistency test ratio
         beaconPosNED : beacon NED position (m)
+        returns true if data could be found, false if it could not
     */
-    void getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
+    bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
 
     // called by vehicle code to specify that a takeoff is happening
     // causes the EKF to compensate for expected barometer errors due to ground effect
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index 5d5750e82e..a6c26ad6eb 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -71,21 +71,11 @@ void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
 }
 
 // return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
-void NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
+bool NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
 {
     // if the states have not been initialised or we have not received any beacon updates then return zeros
     if (!statesInitialised || N_beacons == 0) {
-        ID = 0;
-        rng = 0.0f;
-        innov = 0.0f;
-        innovVar = 0.0f;
-        testRatio = 0.0f;
-        Vector3f temp;
-        temp.zero();
-        beaconPosNED = temp;
-        offsetHigh = 0.0f;
-        offsetLow = 0.0f;
-        return;
+        return false;
     }
 
     // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
@@ -100,9 +90,10 @@ void NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
     innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar;          // innovation variance (m^2)
     testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio;        // innovation consistency test ratio
     beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED;  // beacon NED position
-    offsetHigh = bcnPosOffsetMax;                                              // beacon system vertical pos offset upper estimate
+    offsetHigh = bcnPosOffsetMax;                                               // beacon system vertical pos offset upper estimate
     offsetLow = bcnPosOffsetMin;                                                // beacon system vertical pos offset lower estimate
     rngBcnFuseDataReportIndex++;
+    return true;
 }
 
 // provides the height limit to be observed by the control loops
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index aed96e91cf..87edf0f0c0 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -203,7 +203,7 @@ public:
         testRatio : innovation consistency test ratio
         beaconPosNED : beacon NED position (m)
     */
-    void getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
+    bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
 
     // called by vehicle code to specify that a takeoff is happening
     // causes the EKF to compensate for expected barometer errors due to ground effect