From f037629fc3114b21fe4ed43f68756f788e8f1773 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 9 Jan 2019 14:08:08 +1100
Subject: [PATCH] RangeFinder: understand stop pin for AP_RangeFinder_PWM
 backend

---
 .../AP_RangeFinder/AP_RangeFinder_PWM.cpp     | 54 +++++++++++++++++--
 libraries/AP_RangeFinder/AP_RangeFinder_PWM.h | 14 ++++-
 .../AP_RangeFinder/AP_RangeFinder_PX4_PWM.h   |  5 --
 libraries/AP_RangeFinder/RangeFinder.cpp      |  4 +-
 4 files changed, 66 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp
index 01feb14090..f14e5c6113 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp
@@ -23,8 +23,12 @@ extern const AP_HAL::HAL& hal;
 /*
    The constructor also initialises the rangefinder.
 */
-AP_RangeFinder_PWM::AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state) :
-    AP_RangeFinder_Backend(_state)
+AP_RangeFinder_PWM::AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state,
+                                       AP_Int16 &_powersave_range,
+                                       float &_estimated_terrain_height) :
+    AP_RangeFinder_Backend(_state),
+    powersave_range(_powersave_range),
+    estimated_terrain_height(_estimated_terrain_height)
 {
 }
 
@@ -111,19 +115,57 @@ void AP_RangeFinder_PWM::check_pin()
     }
 }
 
+void AP_RangeFinder_PWM::check_stop_pin()
+{
+    if (state.stop_pin == last_stop_pin) {
+        return;
+    }
+
+    hal.gpio->pinMode(state.stop_pin, HAL_GPIO_OUTPUT);
+
+    last_stop_pin = state.stop_pin;
+}
+
+void AP_RangeFinder_PWM::check_pins()
+{
+    check_pin();
+    check_stop_pin();
+}
+
+
 /*
    update the state of the sensor
 */
 void AP_RangeFinder_PWM::update(void)
 {
     // check if pin has changed and configure interrupt handlers if required:
-    check_pin();
+    check_pins();
 
     if (last_pin <= 0) {
         // disabled (by configuration)
         return;
     }
 
+    if (state.stop_pin != -1) {
+        const bool oor = out_of_range();
+        if (oor) {
+            if (!was_out_of_range) {
+                // we are above the power saving range. Disable the sensor
+                hal.gpio->write(state.stop_pin, false);
+                set_status(RangeFinder::RangeFinder_NoData);
+                state.distance_cm = 0;
+                state.voltage_mv = 0;
+                was_out_of_range = oor;
+            }
+            return;
+        }
+        // re-enable the sensor:
+        if (!oor && was_out_of_range) {
+            hal.gpio->write(state.stop_pin, true);
+            was_out_of_range = oor;
+        }
+    }
+
     if (!get_reading(state.distance_cm)) {
         // failure; consider changing our state
         if (AP_HAL::millis() - state.last_reading_ms > 200) {
@@ -136,3 +178,9 @@ void AP_RangeFinder_PWM::update(void)
     state.last_reading_ms = AP_HAL::millis();
     update_status();
 }
+
+
+// return true if we are beyond the power saving range
+bool AP_RangeFinder_PWM::out_of_range(void) const {
+    return powersave_range > 0 && estimated_terrain_height > powersave_range;
+}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h
index 6deaf87c33..622d5ce6e5 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h
@@ -21,7 +21,9 @@ class AP_RangeFinder_PWM : public AP_RangeFinder_Backend
 {
 public:
     // constructor
-    AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state);
+    AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state,
+                       AP_Int16 &_powersave_range,
+                       float &_estimated_terrain_height);
 
     // destructor
     ~AP_RangeFinder_PWM(void) {};
@@ -52,5 +54,15 @@ private:
     void irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us);
 
     void check_pin();
+    void check_stop_pin();
+    void check_pins();
+    uint8_t last_stop_pin = -1;
+
+    AP_Int16 &powersave_range;
+    float &estimated_terrain_height;
+
+    // return true if we are beyond the power saving range
+    bool out_of_range(void) const;
+    bool was_out_of_range = -1; // this odd initialisation ensures we transition to new state
 
 };
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h
index 7abfc577cd..e8c0c9fde0 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h
@@ -48,9 +48,4 @@ private:
     AP_Int16 &_powersave_range;
     float &estimated_terrain_height;
 
-    // return true if we are beyond the power saving range
-    bool out_of_range(void) const {
-        return _powersave_range > 0 && estimated_terrain_height > _powersave_range;
-    }
-
 };
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 701f678820..69de403331 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -714,7 +714,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         // to ease moving from PX4 to ChibiOS we'll lie a little about
         // the backend driver...
         if (AP_RangeFinder_PWM::detect()) {
-            drivers[instance] = new AP_RangeFinder_PWM(state[instance]);
+            drivers[instance] = new AP_RangeFinder_PWM(state[instance], _powersave_range, estimated_terrain_height);
         }
         break;
 #endif
@@ -786,7 +786,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         break;
     case RangeFinder_TYPE_PWM:
         if (AP_RangeFinder_PWM::detect()) {
-            drivers[instance] = new AP_RangeFinder_PWM(state[instance]);
+            drivers[instance] = new AP_RangeFinder_PWM(state[instance], _powersave_range, estimated_terrain_height);
         }
         break;
     default: