From 86e8c7ed2f1969b10dcbf21f64956835d9600230 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 16 Sep 2015 11:14:26 +1000
Subject: [PATCH] Plane: add some safety to detect bad lidar readings

we only accept a lidar if it changes by 5% of its full range, and we
reject a lidar again if the correction between barometric and lidar
range changes by more than 30m

This allows us to cope with faulty lidars which may give a constant
reading
---
 ArduPlane/Plane.h            |  6 +++++-
 ArduPlane/altitude.cpp       | 31 +++++++++++++++++++++++++++----
 ArduPlane/commands_logic.cpp |  5 +++++
 ArduPlane/landing.cpp        |  6 ++++--
 4 files changed, 41 insertions(+), 7 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 3e3b5f22a6..6f056514f3 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -194,8 +194,12 @@ private:
     RangeFinder rangefinder {serial_manager};
 
     struct {
-        bool in_range;
+        bool in_range:1;
+        bool have_initial_reading:1;
+        bool in_use:1;
+        float initial_range;
         float correction;
+        float initial_correction;
         uint32_t last_correction_time_ms;
         uint8_t in_range_count;
     } rangefinder_state;
diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
index 5fdc398c39..5699f48cf8 100644
--- a/ArduPlane/altitude.cpp
+++ b/ArduPlane/altitude.cpp
@@ -552,19 +552,34 @@ float Plane::rangefinder_correction(void)
  */
 void Plane::rangefinder_height_update(void)
 {
-    uint16_t distance_cm = rangefinder.distance_cm();
+    float distance = rangefinder.distance_cm()*0.01f;
     float height_estimate = 0;
     if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
+        if (!rangefinder_state.have_initial_reading) {
+            rangefinder_state.have_initial_reading = true;
+            rangefinder_state.initial_range = distance;
+        }
         // correct the range for attitude (multiply by DCM.c.z, which
         // is cos(roll)*cos(pitch))
-        height_estimate = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
+        height_estimate = distance * ahrs.get_dcm_matrix().c.z;
 
         // we consider ourselves to be fully in range when we have 10
-        // good samples (0.2s)
+        // good samples (0.2s) that are different by 5% of the maximum
+        // range from the initial range we see. The 5% change is to
+        // catch Lidars that are giving a constant range, either due
+        // to misconfiguration or a faulty sensor
         if (rangefinder_state.in_range_count < 10) {
-            rangefinder_state.in_range_count++;
+            if (fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) {
+                rangefinder_state.in_range_count++;
+            }
         } else {
             rangefinder_state.in_range = true;
+            if (!rangefinder_state.in_use &&
+                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
+                g.rangefinder_landing) {
+                rangefinder_state.in_use = true;
+                gcs_send_text_fmt(PSTR("Rangefinder engaged at %.2fm"), height_estimate);
+            }
         }
     } else {
         rangefinder_state.in_range_count = 0;
@@ -589,8 +604,16 @@ void Plane::rangefinder_height_update(void)
         // the old data is more than 5 seconds old
         if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
             rangefinder_state.correction = correction;
+            rangefinder_state.initial_correction = correction;
         } else {
             rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
+            if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) {
+                // the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar
+                if (rangefinder_state.in_use) {
+                    gcs_send_text_fmt(PSTR("Rangefinder disengaged at %.2fm"), height_estimate);
+                }
+                memset(&rangefinder_state, 0, sizeof(rangefinder_state));
+            }
         }
         rangefinder_state.last_correction_time_ms = millis();    
     }
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 45f1d083ac..cd0431cf41 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -358,6 +358,11 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
         // If no takeoff command has ever been used, default to a conservative 10deg
         auto_state.takeoff_pitch_cd = 1000;
     }
+
+#if RANGEFINDER_ENABLED == ENABLED
+    // zero rangefinder state, start to accumulate good samples now
+    memset(&rangefinder_state, 0, sizeof(rangefinder_state));
+#endif
 }
 
 void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp
index 2a7447c760..cc317458d2 100644
--- a/ArduPlane/landing.cpp
+++ b/ArduPlane/landing.cpp
@@ -76,8 +76,10 @@ bool Plane::verify_land()
             if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
                 gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
             } else {
-                gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), 
-                        (double)height, (double)auto_state.sink_rate, (double)gps.ground_speed());
+                gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f dist=%.1f"), 
+                                  (double)height, (double)auto_state.sink_rate,
+                                  (double)gps.ground_speed(),
+                                  (double)get_distance(current_loc, next_WP_loc));
             }
         }
         auto_state.land_complete = true;