RangeFinder: replace healthy with status and no_data methods

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -207,6 +207,10 @@ void RangeFinder::init(void)
         state[i].pre_arm_check = false;
         state[i].pre_arm_distance_min = 9999;  // initialise to an arbitrary large value
         state[i].pre_arm_distance_max = 0;
+
+        // initialise status
+        state[i].status = RangeFinder_NotConnected;
+        state[i].range_valid_count = 0;
     }
 }
 
@@ -220,7 +224,8 @@ void RangeFinder::update(void)
         if (drivers[i] != NULL) {
             if (_type[i] == RangeFinder_TYPE_NONE) {
                 // allow user to disable a rangefinder at runtime
-                state[i].healthy = false;
+                state[i].status = RangeFinder_NotConnected;
+                state[i].range_valid_count = 0;
                 continue;
             }
             drivers[i]->update();
@@ -228,9 +233,9 @@ void RangeFinder::update(void)
         }
     }
 
-    // work out primary instance - first healthy sensor
+    // work out primary instance - first sensor returning good data
     for (int8_t i=num_instances-1; i>=0; i--) {
-        if (drivers[i] != NULL && state[i].healthy) {
+        if (drivers[i] != NULL && (state[i].status == RangeFinder_Good)) {
             primary_instance = i;
         }
     }
@@ -290,6 +295,31 @@ void RangeFinder::detect_instance(uint8_t instance)
     }
 }
 
+// query status
+RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const
+{
+    // sanity check instance
+    if (instance > RANGEFINDER_MAX_INSTANCES) {
+        return RangeFinder_NotConnected;
+    }
+
+    if (drivers[instance] == NULL || _type[instance] == RangeFinder_TYPE_NONE) {
+        return RangeFinder_NotConnected;
+    }
+
+    return state[instance].status;
+}
+
+// true if sensor is returning data
+bool RangeFinder::has_data(uint8_t instance) const
+{
+    // sanity check instance
+    if (instance > RANGEFINDER_MAX_INSTANCES) {
+        return RangeFinder_NotConnected;
+    }
+    return ((state[instance].status != RangeFinder_NotConnected) && (state[instance].status != RangeFinder_NoData));
+}
+
 /*
   returns true if pre-arm checks have passed for all range finders
   these checks involve the user lifting or rotating the vehicle so that sensor readings between
@@ -314,8 +344,8 @@ bool RangeFinder::pre_arm_check() const
  */
 void RangeFinder::update_pre_arm_check(uint8_t instance)
 {
-    // return immediately if already passed or unhealty
+    // return immediately if already passed or no sensor data
-    if (state[instance].pre_arm_check || !state[instance].healthy) {
+    if (state[instance].pre_arm_check || state[instance].status == RangeFinder_NotConnected || state[instance].status == RangeFinder_NoData) {
         return;
     }
 

libraries/AP_RangeFinder/RangeFinder.h

@@ -58,6 +58,13 @@ public:
         FUNCTION_HYPERBOLA = 2
     };
 
+    enum RangeFinder_Status {
+        RangeFinder_NotConnected = 0,
+        RangeFinder_NoData,
+        RangeFinder_OutOfRangeLow,
+        RangeFinder_OutOfRangeHigh,
+        RangeFinder_Good
+    };
 
     // The RangeFinder_State structure is filled in by the backend driver
     struct RangeFinder_State {
@@ -65,7 +72,8 @@ public:
         uint16_t               distance_cm; // distance: in cm
         uint16_t               voltage_mv;  // voltage in millivolts,
                                             // if applicable, otherwise 0
-        bool                   healthy;     // sensor is communicating correctly
+        enum RangeFinder_Status status;     // sensor status
+        uint8_t                range_valid_count;   // number of consecutive valid readings (maxes out at 10)
         bool                   pre_arm_check;   // true if sensor has passed pre-arm checks
         uint16_t               pre_arm_distance_min;    // min distance captured during pre-arm checks
         uint16_t               pre_arm_distance_max;    // max distance captured during pre-arm checks
@@ -135,11 +143,24 @@ public:
         return _ground_clearance_cm[primary_instance];
     }
 
-    bool healthy(uint8_t instance) const {
+    // query status
-        return instance < num_instances && _RangeFinder_STATE(instance).healthy;
+    RangeFinder_Status status(uint8_t instance) const;
+    RangeFinder_Status status(void) const {
+        return status(primary_instance);
     }
-    bool healthy() const {
+
-        return healthy(primary_instance);
+    // true if sensor is returning data
+    bool has_data(uint8_t instance) const;
+    bool has_data() const {
+        return has_data(primary_instance);
+    }
+
+    // returns count of consecutive good readings
+    uint8_t range_valid_count() const {
+        return range_valid_count(primary_instance);
+    }
+    uint8_t range_valid_count(uint8_t instance) const {
+        return _RangeFinder_STATE(instance).range_valid_count;
     }
 
     /*

libraries/AP_RangeFinder/RangeFinder_Backend.cpp

@@ -28,3 +28,31 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t ins
         state(_state) 
 {
 }
+
+// update status based on distance measurement
+void AP_RangeFinder_Backend::update_status()
+{
+    // check distance
+    if (state.distance_cm > ranger._max_distance_cm[state.instance]) {
+        set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
+    } else if (state.distance_cm < ranger._min_distance_cm[state.instance]) {
+        set_status(RangeFinder::RangeFinder_OutOfRangeLow);
+    } else {
+        set_status(RangeFinder::RangeFinder_Good);
+    }
+}
+
+// set status and update valid count
+void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status status)
+{
+    state.status = status;
+
+    // update valid count
+    if (status == RangeFinder::RangeFinder_Good) {
+        if (state.range_valid_count < 10) {
+            state.range_valid_count++;
+        }
+    } else {
+        state.range_valid_count = 0;
+    }
+}

libraries/AP_RangeFinder/RangeFinder_Backend.h

@@ -40,6 +40,13 @@ public:
     }
 
 protected:
+
+    // update status based on distance measurement
+    void update_status();
+
+    // set status and update valid_count
+    void set_status(RangeFinder::RangeFinder_Status status);
+
     RangeFinder &ranger;
     RangeFinder::RangeFinder_State &state;
 };