From 00c512c7e229eb8457a0133b222d369bb7480c8f Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Fri, 11 May 2018 12:29:11 -0700
Subject: [PATCH] AP_Proximity: Small fixups to the rangefinder backend

---
 .../AP_Proximity/AP_Proximity_RangeFinder.cpp | 23 ++++++++++++++-----
 .../AP_Proximity/AP_Proximity_RangeFinder.h   |  4 ++--
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
index a543b89a3f..8d810bbcc5 100644
--- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
@@ -38,8 +38,10 @@ void AP_Proximity_RangeFinder::update(void)
         return;
     }
 
+    uint32_t now = AP_HAL::millis();
+
     // look through all rangefinders
-    for (uint8_t i=0; i<rngfnd->num_sensors(); i++) {
+    for (uint8_t i=0; i < RANGEFINDER_MAX_INSTANCES; i++) {
         AP_RangeFinder_Backend *sensor = rngfnd->get_backend(i);
         if (sensor == nullptr) {
             continue;
@@ -53,19 +55,26 @@ void AP_Proximity_RangeFinder::update(void)
                 _distance_min = sensor->min_distance_cm() / 100.0f;
                 _distance_max = sensor->max_distance_cm() / 100.0f;
                 _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
-                _last_update_ms = AP_HAL::millis();
+                _last_update_ms = now;
                 update_boundary_for_sector(sector);
             }
             // check upward facing range finder
             if (sensor->orientation() == ROTATION_PITCH_90) {
-                _distance_upward = sensor->distance_cm() / 100.0f;
-                _last_upward_update_ms = AP_HAL::millis();
+                int16_t distance_upward = sensor->distance_cm();
+                int16_t up_distance_min = sensor->min_distance_cm();
+                int16_t up_distance_max = sensor->max_distance_cm();
+                if ((distance_upward >= up_distance_min) && (distance_upward <= up_distance_max)) {
+                    _distance_upward = distance_upward * 1e2;
+                } else {
+                    _distance_upward = -1.0; // mark an valid reading
+                }
+                _last_upward_update_ms = now;
             }
         }
     }
 
     // check for timeout and set health status
-    if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) {
+    if ((_last_update_ms == 0) || (now - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) {
         set_status(AP_Proximity::Proximity_NoData);
     } else {
         set_status(AP_Proximity::Proximity_Good);
@@ -75,7 +84,9 @@ void AP_Proximity_RangeFinder::update(void)
 // get distance upwards in meters. returns true on success
 bool AP_Proximity_RangeFinder::get_upward_distance(float &distance) const
 {
-    if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_RANGEFIDER_TIMEOUT_MS)) {
+    if ((_last_upward_update_ms != 0) &&
+        (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_RANGEFIDER_TIMEOUT_MS) &&
+        is_positive(_distance_upward)) {
         distance = _distance_upward;
         return true;
     }
diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.h b/libraries/AP_Proximity/AP_Proximity_RangeFinder.h
index ab863107dd..15e47d7e6f 100644
--- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.h
+++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.h
@@ -3,7 +3,7 @@
 #include "AP_Proximity.h"
 #include "AP_Proximity_Backend.h"
 
-#define PROXIMITY_RANGEFIDER_TIMEOUT_MS 200 // requests timeout after 0.5 seconds
+#define PROXIMITY_RANGEFIDER_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
 
 class AP_Proximity_RangeFinder : public AP_Proximity_Backend
 {
@@ -31,5 +31,5 @@ private:
 
     // upward distance support
     uint32_t _last_upward_update_ms;    // system time of last update distance
-    float _distance_upward;             // upward distance in meters
+    float _distance_upward;             // upward distance in meters, negative if the last reading was out of range
 };