AP_RangeFinder: support last_reading_ms

Benewake, LeddarOne, LightWareSerial, MAVLink, MaxsonarI2CXL, MaxsonarSerialLV, NMEA, PX4_PWM, uLanding and Wasp already stored the last read time so for these drivers, this change just moves that storage to the state structure
analog, BBB_PRU, Bebop, LightWareI2C, PulsedLightLRF, TeraRangerI2C, VL53L0X did not store the last read time so this was added

libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp

@@ -116,5 +116,6 @@ void AP_RangeFinder_BBB_PRU::update(void)
 {
     state.status = (RangeFinder::RangeFinder_Status)rangerpru->status;
     state.distance_cm = rangerpru->distance;
+    state.last_reading_ms = AP_HAL::millis();
 }
 #endif // CONFIG_HAL_BOARD_SUBTYPE

libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp

@@ -267,6 +267,7 @@ void AP_RangeFinder_Bebop::update(void)
     }
 
     state.distance_cm = (uint16_t) (_altitude * 100);
+    state.last_reading_ms = AP_HAL::millis();
     update_status();
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

@@ -152,14 +152,14 @@ void AP_RangeFinder_Benewake::update(void)
     bool signal_ok;
     if (get_reading(state.distance_cm, signal_ok)) {
         // update range_valid state based on distance measured
-        last_reading_ms = AP_HAL::millis();
+        state.last_reading_ms = AP_HAL::millis();
         if (signal_ok) {
             update_status();
         } else {
             // if signal is weak set status to out-of-range
             set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
         }
-    } else if (AP_HAL::millis() - last_reading_ms > 200) {
+    } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
 }

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h

@@ -39,7 +39,6 @@ private:
 
     AP_HAL::UARTDriver *uart = nullptr;
     benewake_model_type model_type;
-    uint32_t last_reading_ms;
     char linebuf[10];
     uint8_t linebuf_len;
 };

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp

@@ -128,9 +128,9 @@ void AP_RangeFinder_LeddarOne::update(void)
 {
     if (get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
-        last_reading_ms = AP_HAL::millis();
+        state.last_reading_ms = AP_HAL::millis();
         update_status();
-    } else if (AP_HAL::millis() - last_reading_ms > 200) {
+    } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
 }

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h

@@ -69,7 +69,6 @@ private:
     LeddarOne_Status parse_response(uint8_t &number_detections);
 
     AP_HAL::UARTDriver *uart = nullptr;
-    uint32_t last_reading_ms;
     uint32_t last_sending_request_ms;
     uint32_t last_available_ms;
 

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -97,6 +97,7 @@ void AP_RangeFinder_LightWareI2C::update(void)
 void AP_RangeFinder_LightWareI2C::timer(void)
 {
     if (get_reading(state.distance_cm)) {
+        state.last_reading_ms = AP_HAL::millis();
         // update range_valid state based on distance measured
         update_status();
     } else {

libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp

@@ -90,9 +90,9 @@ void AP_RangeFinder_LightWareSerial::update(void)
 {
     if (get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
-        last_reading_ms = AP_HAL::millis();
+        state.last_reading_ms = AP_HAL::millis();
         update_status();
-    } else if (AP_HAL::millis() - last_reading_ms > 200) {
+    } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
 }

libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h

@@ -29,7 +29,6 @@ private:
     bool get_reading(uint16_t &reading_cm);
 
     AP_HAL::UARTDriver *uart = nullptr;
-    uint32_t last_reading_ms = 0;
     char linebuf[10];
     uint8_t linebuf_len = 0;
 };

libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp

@@ -28,7 +28,7 @@ extern const AP_HAL::HAL& hal;
 AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state) :
     AP_RangeFinder_Backend(_state)
 {
-    last_update_ms = AP_HAL::millis();
+    state.last_reading_ms = AP_HAL::millis();
     distance_cm = 0;
 }
 
@@ -53,7 +53,7 @@ void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg)
 
     // only accept distances for downward facing sensors
     if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
-        last_update_ms = AP_HAL::millis();
+        state.last_reading_ms = AP_HAL::millis();
         distance_cm = packet.current_distance;
     }
     sensor_type = (MAV_DISTANCE_SENSOR)packet.type;
@@ -66,7 +66,7 @@ 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() - last_update_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
+    if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
         set_status(RangeFinder::RangeFinder_NoData);
         state.distance_cm = 0;
     } else {

libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h

@@ -30,7 +30,6 @@ protected:
 
 private:
     uint16_t distance_cm;
-    uint32_t last_update_ms;
 
     // start a reading
     static bool start_reading(void);

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp

@@ -133,7 +133,7 @@ void AP_RangeFinder_MaxsonarI2CXL::_timer(void)
         if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
             distance = d;
             new_distance = true;
-            last_update_ms = AP_HAL::millis();
+            state.last_reading_ms = AP_HAL::millis();
             _sem->give();
         }
     }
@@ -149,7 +149,7 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void)
             state.distance_cm = distance;
             new_distance = false;
             update_status();
-        } else if (AP_HAL::millis() - last_update_ms > 300) {
+        } else if (AP_HAL::millis() - state.last_reading_ms > 300) {
             // if no updates for 0.3 seconds set no-data
             set_status(RangeFinder::RangeFinder_NoData);
         }

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h

@@ -33,7 +33,6 @@ private:
 
     uint16_t distance;
     bool new_distance;
-    uint32_t last_update_ms;
     
     // start a reading
     bool start_reading(void);

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp

@@ -94,9 +94,9 @@ void AP_RangeFinder_MaxsonarSerialLV::update(void)
 {
     if (get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
-        last_reading_ms = AP_HAL::millis();
+        state.last_reading_ms = AP_HAL::millis();
         update_status();
-    } else if (AP_HAL::millis() - last_reading_ms > 500) {
+    } else if (AP_HAL::millis() - state.last_reading_ms > 500) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
 }

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h

@@ -29,7 +29,6 @@ private:
     bool get_reading(uint16_t &reading_cm);
 
     AP_HAL::UARTDriver *uart = nullptr;
-    uint32_t last_reading_ms = 0;
     char linebuf[10];
     uint8_t linebuf_len = 0;
 };

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@@ -48,9 +48,9 @@ void AP_RangeFinder_NMEA::update(void)
     uint32_t now = AP_HAL::millis();
     if (get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
-        _last_reading_ms = now;
+        state.last_reading_ms = now;
         update_status();
-    } else if ((now - _last_reading_ms) > 3000) {
+    } else if ((now - state.last_reading_ms) > 3000) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
 }

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -64,7 +64,6 @@ private:
     static int16_t char_to_hex(char a);
 
     AP_HAL::UARTDriver *uart = nullptr;     // pointer to serial uart
-    uint32_t _last_reading_ms;              // system time of last successful reading
 
     // message decoding related members
     char _term[15];                         // buffer for the current term within the current sentence

libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp

@@ -114,7 +114,7 @@ void AP_RangeFinder_PX4_PWM::update(void)
         // pulse widths in the log
         state.voltage_mv = pwm.pulse_width;
 
-        _last_pulse_time_ms = now;
+        state.last_reading_ms = now;
 
         // setup for scaling in meters per millisecond
         float _distance_cm = pwm.pulse_width * 0.1f * scaling + state.offset;
@@ -162,9 +162,9 @@ void AP_RangeFinder_PX4_PWM::update(void)
        probably dead. Try resetting it. Tests show the sensor takes
        about 0.2s to boot, so 500ms offers some safety margin
     */
-    if (now - _last_pulse_time_ms > 500U && _disable_time_ms == 0) {
+    if (now - state.last_reading_ms > 500U && _disable_time_ms == 0) {
         ioctl(_fd, SENSORIOCRESET, 0);
-        _last_pulse_time_ms = now;
+        state.last_reading_ms = now;
 
         // if a stop pin is configured then disable the sensor for the
         // settle time
@@ -185,7 +185,7 @@ void AP_RangeFinder_PX4_PWM::update(void)
         (now - _disable_time_ms > settle_time_ms)) {
         hal.gpio->write(stop_pin, true);        
         _disable_time_ms = 0;
-        _last_pulse_time_ms = now;
+        state.last_reading_ms = now;
     }
 
     if (count != 0) {

libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h

@@ -41,7 +41,6 @@ protected:
 private:
     int _fd;
     uint64_t _last_timestamp;
-    uint64_t _last_pulse_time_ms;
     uint32_t _disable_time_ms;
     uint32_t _good_sample_count;
     float _last_sample_distance_cm;

libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp

@@ -100,6 +100,7 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
             // remove momentary spikes
             if (abs(_distance_cm - last_distance_cm) < 100) {
                 state.distance_cm = _distance_cm;
+                state.last_reading_ms = AP_HAL::millis();
                 update_status();                
             }
             last_distance_cm = _distance_cm;

libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp

@@ -160,7 +160,7 @@ void AP_RangeFinder_TeraRangerI2C::timer(void)
     uint16_t _distance_cm = 0;
 
     if (collect_raw(_raw_distance) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        if(process_raw_measure(_raw_distance, _distance_cm)){
+        if (process_raw_measure(_raw_distance, _distance_cm)){
             accum.sum += _distance_cm;
             accum.count++;
         }
@@ -178,6 +178,7 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
     if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         if (accum.count > 0) {
             state.distance_cm = accum.sum / accum.count;
+            state.last_reading_ms = AP_HAL::millis();
             accum.sum = 0;
             accum.count = 0;
             update_status();

libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp

@@ -768,6 +768,7 @@ void AP_RangeFinder_VL53L0X::update(void)
 {
     if (counter > 0) {
         state.distance_cm = sum_mm / (10*counter);
+        state.last_reading_ms = AP_HAL::millis();
         sum_mm = 0;
         counter = 0;
         update_status();

libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp

@@ -103,7 +103,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
         if (c == '\n') {
             linebuf[linebuf_len] = 0;
             linebuf_len = 0;
-            last_reading_ms = AP_HAL::millis();
+            state.last_reading_ms = AP_HAL::millis();
             if (isalpha(linebuf[0])) {
                 parse_response();
             } else {
@@ -144,7 +144,7 @@ void AP_RangeFinder_Wasp::update(void) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
 
-    if (AP_HAL::millis() - last_reading_ms > 500) {
+    if (AP_HAL::millis() - state.last_reading_ms > 500) {
         // attempt to reconfigure on the assumption this was a bad baud setting
         configuration_state = WASP_CFG_RATE;
     }

libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h

@@ -49,7 +49,6 @@ private:
     void parse_response(void);
 
     AP_HAL::UARTDriver *uart;
-    uint32_t last_reading_ms;
     char linebuf[10];
     uint8_t linebuf_len;
     AP_Int16 mavg;

libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp

@@ -116,6 +116,7 @@ void AP_RangeFinder_analog::update(void)
         dist_m = 0;
     }
     state.distance_cm = dist_m * 100.0f;
+    state.last_reading_ms = AP_HAL::millis();
 
     // update range_valid state based on distance measured
     update_status();

libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp

@@ -211,10 +211,10 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm)
 void AP_RangeFinder_uLanding::update(void)
 {
     if (get_reading(state.distance_cm)) {
+        state.last_reading_ms = AP_HAL::millis();
         // update range_valid state based on distance measured
-        _last_reading_ms = AP_HAL::millis();
         update_status();
-    } else if (AP_HAL::millis() - _last_reading_ms > 200) {
+    } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
 }

libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h

@@ -34,7 +34,6 @@ private:
     AP_HAL::UARTDriver *uart;
     uint8_t  _linebuf[6];
     uint8_t  _linebuf_len;
-    uint32_t _last_reading_ms;
     bool     _version_known;
     uint8_t  _header;
     uint8_t  _version;

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -910,6 +910,15 @@ const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) co
     return backend->get_pos_offset();
 }
 
+uint32_t RangeFinder::last_reading_ms(enum Rotation orientation) const
+{
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
+    if (backend == nullptr) {
+        return 0;
+    }
+    return backend->last_reading_ms();
+}
+
 MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotation orientation) const
 {
     AP_RangeFinder_Backend *backend = find_instance(orientation);

libraries/AP_RangeFinder/RangeFinder.h

@@ -87,6 +87,7 @@ public:
         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
+        uint32_t last_reading_ms;       // system time of last successful update from sensor
 
         AP_Int8  type;
         AP_Int8  pin;
@@ -147,6 +148,7 @@ public:
     bool has_data_orient(enum Rotation orientation) const;
     uint8_t range_valid_count_orient(enum Rotation orientation) const;
     const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
+    uint32_t last_reading_ms(enum Rotation orientation) const;
 
     /*
       set an externally estimated terrain height. Used to enable power

libraries/AP_RangeFinder/RangeFinder_Backend.h

@@ -55,6 +55,9 @@ public:
     // in metres relative to the body frame origin
     const Vector3f &get_pos_offset() const { return state.pos_offset; }
 
+    // return system time of last successful read from the sensor (used for DPTH logging)
+    uint32_t last_reading_ms() const { return state.last_reading_ms; }
+
 protected:
 
     // update status based on distance measurement