AP_RangeFinder: cleanup rangefinder timeout handling

we had inconsistent behaviour across different sensors. This PR moves
the timeout logic to the frontend so it is consistent

note also that some drivers set reported distance to zero on
timeout. Should we remove that? (eg. mavlink)

this PR was inspired by a log showing the LidarLitev3 (PulsedLight
driver) getting "stuck". I think it was giving oscillating values
which led the driver to never (or rarely) update. This caused
uncontrolled descent/ascent in a copter. That was with 4.3.7 but I
think the same vulnerablity is in master

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -240,6 +240,19 @@ void RangeFinder::update(void)
                 continue;
             }
             drivers[i]->update();
+
+            WITH_SEMAPHORE(drivers[i]->get_semaphore());
+            /*
+              ensure that all drivers timeout to NoData. If they are
+              currently in NotConnected state then leave it in that
+              state as they have never had any data
+             */
+            const uint32_t last_update_ms = drivers[i]->last_reading_ms();
+            const uint32_t now_ms = AP_HAL::millis();
+            if (state[i].status != Status::NotConnected &&
+                now_ms - last_update_ms > drivers[i]->read_timeout_ms()) {
+                state[i].status = Status::NoData;
+            }
         }
     }
 #if HAL_LOGGING_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h

@@ -180,7 +180,7 @@ class AP_RangeFinder_BLPing : public AP_RangeFinder_Backend_Serial
      *
      * @return uint16_t
      */
-    uint16_t read_timeout_ms() const override { return 1000; }
+    uint32_t read_timeout_ms() const override { return 1000; }
 
     /**
      * @brief system time that sensor was last initialised

libraries/AP_RangeFinder/AP_RangeFinder_Backend.h

@@ -79,6 +79,12 @@ class AP_RangeFinder_Backend
     // parameter value which may be changed at runtime.
     RangeFinder::Type allocated_type() const { return _backend_type; }
 
+    // maximum time between readings before we change state to NoData:
+    virtual uint32_t read_timeout_ms() const { return 200; }
+
+    // get reference to driver semaphore
+    HAL_Semaphore &get_semaphore(void) { return _sem; }
+
 protected:
 
     // update status based on distance measurement

libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp

@@ -58,8 +58,6 @@ void AP_RangeFinder_Backend_CAN::update(void)
         // update range_valid state based on distance measured
         state.last_reading_ms = AP_HAL::millis();
         update_status();
-    } else if (AP_HAL::millis() - state.last_reading_ms >= read_timeout_ms()) {
-        set_status(RangeFinder::Status::NoData);
     }
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h

@@ -32,9 +32,6 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend
     // it is essential that anyone relying on the base-class update to implement this
     virtual bool handle_frame(AP_HAL::CANFrame &frame) = 0;
 
-    // maximum time between readings before we change state to NoData:
-    virtual uint32_t read_timeout_ms() const { return 200; }
-
     virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
         return MAV_DISTANCE_SENSOR_RADAR;
     }

libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp

@@ -57,7 +57,5 @@ void AP_RangeFinder_Backend_Serial::update(void)
         // update range_valid state based on distance measured
         state.last_reading_ms = AP_HAL::millis();
         update_status();
-    } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
-        set_status(RangeFinder::Status::NoData);
     }
 }

libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h

@@ -34,7 +34,4 @@ class AP_RangeFinder_Backend_Serial : public AP_RangeFinder_Backend
     // Like get_reading() this method is called in the base-class update() method.
     virtual int8_t get_signal_quality_pct() const WARN_IF_UNUSED
     { return RangeFinder::SIGNAL_QUALITY_UNKNOWN; }
-
-    // maximum time between readings before we change state to NoData:
-    virtual uint16_t read_timeout_ms() const { return 200; }
 };

libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp

@@ -148,8 +148,6 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update()
         accum.sum = 0;
         accum.count = 0;
         update_status();
-    } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
-        set_status(RangeFinder::Status::NoData);
     }
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp

@@ -97,13 +97,7 @@ void AP_RangeFinder_HC_SR04::update(void)
     const uint32_t value_us = pwm_source.get_pwm_us();
 
     const uint32_t now = AP_HAL::millis();
-    if (value_us == 0) {
-        // no reading; check for timeout:
-        if (now - last_reading_ms > 1000) {
-            // no reading for a second - something is broken
-            state.distance_m = 0.0f;
-        }
-    } else {
+    if (value_us != 0) {
         // gcs().send_text(MAV_SEVERITY_WARNING, "Pong!");
         // a new reading - convert time to distance
         state.distance_m = (value_us * (1.0/58.0f)) * 0.01f;  // 58 is from datasheet, mult for performance
@@ -126,10 +120,10 @@ void AP_RangeFinder_HC_SR04::update(void)
         }
 
         last_reading_ms = now;
-    }
 
-    // update range_valid state based on distance measured
-    update_status();
+        // update range_valid state based on distance measured
+        update_status();
+    }
 
     // consider sending new ping
     if (now - last_ping_ms > 67) { // read ~@15Hz - recommended 60ms delay from datasheet

libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h

@@ -36,7 +36,7 @@ class AP_RangeFinder_LeddarVu8 : public AP_RangeFinder_Backend_Serial
     bool get_reading(float &reading_m) override;
 
     // maximum time between readings before we change state to NoData:
-    uint16_t read_timeout_ms() const override { return 500; }
+    uint32_t read_timeout_ms() const override { return 500; }
 
 private:
 

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -465,8 +465,11 @@ void AP_RangeFinder_LightWareI2C::update(void)
 
 void AP_RangeFinder_LightWareI2C::legacy_timer(void)
 {
-    if (legacy_get_reading(state.distance_m)) {
+    float distance_m = 0;
+    if (legacy_get_reading(distance_m)) {
         // update range_valid state based on distance measured
+        WITH_SEMAPHORE(_sem);
+        state.distance_m = distance_m;
         update_status();
     } else {
         set_status(RangeFinder::Status::NoData);
@@ -475,8 +478,11 @@ void AP_RangeFinder_LightWareI2C::legacy_timer(void)
 
 void AP_RangeFinder_LightWareI2C::sf20_timer(void)
 {
-    if (sf20_get_reading(state.distance_m)) {
+    float distance_m = 0;
+    if (sf20_get_reading(distance_m)) {
         // update range_valid state based on distance measured
+        WITH_SEMAPHORE(_sem);
+        state.distance_m = distance_m;
         update_status();
     } else {
         set_status(RangeFinder::Status::NoData);

libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp

@@ -42,6 +42,10 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
             // Map 1 to 0 as that is what ardupilot uses as the worst signal quality
             signal_quality = RangeFinder::SIGNAL_QUALITY_MIN;
         }
+        WITH_SEMAPHORE(_sem);
+        state.distance_m = distance_cm * 0.01f;
+        state.signal_quality_pct = signal_quality;
+        update_status();
     }
 }
 
@@ -69,22 +73,4 @@ int16_t AP_RangeFinder_MAVLink::min_distance_cm() const
     return _min_distance_cm;
 }
 
-/*
-   update the state of the sensor
-*/
-void AP_RangeFinder_MAVLink::update(void)
-{
-    //Time out on incoming data; if we don't get new
-    //data in 500ms, dump it
-    if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
-        set_status(RangeFinder::Status::NoData);
-        state.distance_m = 0.0f;
-        state.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
-    } else {
-        state.distance_m = distance_cm * 0.01f;
-        state.signal_quality_pct = signal_quality;
-        update_status();
-    }
-}
-
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h

@@ -22,15 +22,17 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend
     // there is an attached MAVLink rangefinder
     static bool detect() { return true; }
 
-    // update state
-    void update(void) override;
+    // empty update
+    void update(void) override {}
 
     // Get update from mavlink
     void handle_msg(const mavlink_message_t &msg) override;
 
     int16_t max_distance_cm() const override;
     int16_t min_distance_cm() const override;
 
+    uint32_t read_timeout_ms() const override { return AP_RANGEFINDER_MAVLINK_TIMEOUT_MS; }
+
 protected:
 
     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {

libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp

@@ -29,7 +29,6 @@ AP_RangeFinder_MSP::AP_RangeFinder_MSP(RangeFinder::RangeFinder_State &_state, A
     AP_RangeFinder_Backend(_state, _params)
 {
     state.last_reading_ms = AP_HAL::millis();
-    distance_cm = 0;
 }
 
 /*
@@ -48,24 +47,10 @@ bool AP_RangeFinder_MSP::detect()
 */
 void AP_RangeFinder_MSP::handle_msp(const MSP::msp_rangefinder_data_message_t &pkt)
 {
+    WITH_SEMAPHORE(_sem);
+    state.distance_m = pkt.distance_mm * 0.001f;
     state.last_reading_ms = AP_HAL::millis();
-    distance_cm = pkt.distance_mm / 10;
-}
-
-/*
-   update the state of the sensor
-*/
-void AP_RangeFinder_MSP::update(void)
-{
-    //Time out on incoming data; if we don't get new
-    //data in 500ms, dump it
-    if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MSP_TIMEOUT_MS) {
-        set_status(RangeFinder::Status::NoData);
-        state.distance_m = 0.0f;
-    } else {
-        state.distance_m = distance_cm * 0.01f;
-        update_status();
-    }
+    update_status();
 }
 
 #endif //HAL_MSP_RANGEFINDER_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_MSP.h

@@ -20,21 +20,21 @@ class AP_RangeFinder_MSP : public AP_RangeFinder_Backend
     // static detection function
     static bool detect();
 
-    // update state
-    void update(void) override;
+    // empty update
+    void update(void) override {}
 
     // Get update from msp
     void handle_msp(const MSP::msp_rangefinder_data_message_t &pkt) override;
 
+    uint32_t read_timeout_ms() const override { return AP_RANGEFINDER_MSP_TIMEOUT_MS; }
+
 protected:
 
     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
         return MAV_DISTANCE_SENSOR_UNKNOWN;
     }
 
 private:
-    uint16_t distance_cm;
-
     // start a reading
     static bool start_reading(void);
     static bool get_reading(uint16_t &reading_cm);

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp

@@ -148,9 +148,6 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void)
         state.distance_m = distance * 0.01f;
         new_distance = false;
         update_status();
-    } else if (AP_HAL::millis() - state.last_reading_ms > 300) {
-        // if no updates for 0.3 seconds set no-data
-        set_status(RangeFinder::Status::NoData);
     }
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h

@@ -23,6 +23,8 @@ class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend
     // update state
     void update(void) override;
 
+    uint32_t read_timeout_ms() const override { return 300; }
+
 protected:
 
     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h

@@ -31,7 +31,7 @@ class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend_Serial
     // get a reading
     bool get_reading(float &reading_m) override;
 
-    uint16_t read_timeout_ms() const override { return 500; }
+    uint32_t read_timeout_ms() const override { return 500; }
 
     char linebuf[10];
     uint8_t linebuf_len = 0;

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -58,7 +58,7 @@ class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend_Serial
     // get temperature reading in C.  returns true on success and populates temp argument
     bool get_temp(float &temp) const override;
 
-    uint16_t read_timeout_ms() const override { return 3000; }
+    uint32_t read_timeout_ms() const override { return 3000; }
 
     // add a single character to the buffer and attempt to decode
     // returns true if a distance was successfully decoded

libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp

@@ -14,15 +14,6 @@ void AP_RangeFinder_NRA24_CAN::update(void)
         // update range_valid state based on distance measured
         state.last_reading_ms = AP_HAL::millis();
         update_status();
-    } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
-        if (AP_HAL::millis() - last_heartbeat_ms > read_timeout_ms()) {
-            // no heartbeat, must be disconnected
-            set_status(RangeFinder::Status::NotConnected);
-        } else {
-            // Have heartbeat, just no data. Probably because this sensor doesn't output data when there is no relative motion infront of the radar.
-            // This case has special pre-arm check handling
-            set_status(RangeFinder::Status::NoData);
-        }
     }
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp

@@ -114,9 +114,6 @@ void AP_RangeFinder_PWM::update(void)
 
     if (!get_reading(state.distance_m)) {
         // failure; consider changing our state
-        if (AP_HAL::millis() - state.last_reading_ms > 200) {
-            set_status(RangeFinder::Status::NoData);
-        }
         return;
     }
     // add offset

libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp

@@ -96,8 +96,6 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
             state.distance_m = filter.apply(_distance_cm*0.01);
             state.last_reading_ms = AP_HAL::millis();
             update_status();
-        } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
-            set_status(RangeFinder::Status::NoData);
         }
         if (!v2_hardware) {
             // for v2 hw we use continuous mode

libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.cpp

@@ -150,9 +150,6 @@ void AP_RangeFinder_TOFSenseF_I2C::update(void)
         state.distance_m = distance_mm * 0.001f;
         new_distance = false;
         update_status();
-    } else if (AP_HAL::millis() - state.last_reading_ms > 300) {
-        // if no updates for 0.3 seconds set no-data
-        set_status(RangeFinder::Status::NoData);
     }
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.h

@@ -22,6 +22,8 @@ class AP_RangeFinder_TOFSenseF_I2C : public AP_RangeFinder_Backend
     // update state
     void update(void) override;
 
+    uint32_t read_timeout_ms() const override { return 300; }
+
 protected:
 
     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {

libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp

@@ -187,8 +187,6 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
         accum.sum = 0;
         accum.count = 0;
         update_status();        
-    } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
-        set_status(RangeFinder::Status::NoData);
     }
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp

@@ -763,21 +763,21 @@ uint16_t AP_RangeFinder_VL53L0X::read_register16(uint8_t reg)
 */
 void AP_RangeFinder_VL53L0X::update(void)
 {
+    WITH_SEMAPHORE(_sem);
     if (counter > 0) {
         state.distance_m = (sum_mm * 0.001f) / counter;
         state.last_reading_ms = AP_HAL::millis();
         sum_mm = 0;
         counter = 0;
         update_status();
-    } else {
-        set_status(RangeFinder::Status::NoData);
     }
 }
 
 void AP_RangeFinder_VL53L0X::timer(void)
 {
-    uint16_t range_mm;
+     uint16_t range_mm;
     if (get_reading(range_mm) && range_mm < 8000) {
+        WITH_SEMAPHORE(_sem);
         sum_mm += range_mm;
         counter++;
     }