diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp index 58b13635d2..34614ea899 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp @@ -17,7 +17,6 @@ #include #include "AP_RangeFinder_BLPing.h" -#define BLPING_TIMEOUT_MS 500 // sensor timeout after 0.5 sec #define BLPING_INIT_RATE_MS 1000 // initialise sensor at no more than 1hz #define BLPING_FRAME_HEADER1 0x42 // header first byte ('B') #define BLPING_FRAME_HEADER2 0x52 // header second byte ('R') @@ -53,16 +52,10 @@ void AP_RangeFinder_BLPing::update(void) if (uart == nullptr) { return; } + AP_RangeFinder_Backend_Serial::update(); - const uint32_t now = AP_HAL::millis(); - - if (get_reading(state.distance_cm)) { - // update range_valid state based on distance measured - state.last_reading_ms = now; - update_status(); - } else if (now - state.last_reading_ms > BLPING_TIMEOUT_MS) { - set_status(RangeFinder::Status::NoData); - + if (status() == RangeFinder::Status::NoData) { + const uint32_t now = AP_HAL::millis(); // initialise sensor if no distances recently if (now - last_init_ms > BLPING_INIT_RATE_MS) { last_init_ms = now; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h index 46ac8b8100..998dced221 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h @@ -29,6 +29,8 @@ private: // read a distance from the sensor bool get_reading(uint16_t &reading_cm) override; + uint16_t read_timeout_ms() const override { return 500; } + // process one byte received on serial port // returns true if a distance message has been successfully parsed // state is stored in msg structure diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index 6877aaba28..aa2bf2d05a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -132,17 +132,3 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm) // no readings so return false return false; } - -/* - update the state of the sensor -*/ -void AP_RangeFinder_Benewake::update(void) -{ - if (get_reading(state.distance_cm)) { - // 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 > 200) { - set_status(RangeFinder::Status::NoData); - } -} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h index 2d9b90df8e..53e0cf5851 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h @@ -10,9 +10,6 @@ public: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; - // update state - void update(void) override; - protected: virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp index 60f51346e7..1da6a423fe 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp @@ -80,17 +80,3 @@ bool AP_RangeFinder_Lanbao::get_reading(uint16_t &reading_cm) } return false; } - -/* - update the state of the sensor -*/ -void AP_RangeFinder_Lanbao::update(void) -{ - if (get_reading(state.distance_cm)) { - // 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 > 200) { - set_status(RangeFinder::Status::NoData); - } -} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h index 7e88d13a82..fc44bde204 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h @@ -15,9 +15,6 @@ public: return 115200; } - // update state - void update(void) override; - protected: virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index 0cf3fafb56..da79fe01db 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -96,20 +96,6 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm) return false; } -/* - update the state of the sensor -*/ -void AP_RangeFinder_LeddarOne::update(void) -{ - if (get_reading(state.distance_cm)) { - // 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 > 200) { - set_status(RangeFinder::Status::NoData); - } -} - /* CRC16 CRC-16-IBM(x16+x15+x2+1) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index d67cf4028c..64ab648fbc 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -44,9 +44,6 @@ public: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; - // update state - void update(void) override; - protected: virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index ed6e1a3597..97355b1af0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -85,17 +85,3 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm) // no readings so return false return false; } - -/* - update the state of the sensor -*/ -void AP_RangeFinder_LightWareSerial::update(void) -{ - if (get_reading(state.distance_cm)) { - // 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 > 200) { - set_status(RangeFinder::Status::NoData); - } -} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h index cb00f68204..70c4a6ced6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h @@ -10,9 +10,6 @@ public: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; - // update state - void update(void) override; - protected: virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index d89d611a6e..2b1acc25c3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp @@ -60,17 +60,3 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(uint16_t &reading_cm) return true; } - -/* - update the state of the sensor -*/ -void AP_RangeFinder_MaxsonarSerialLV::update(void) -{ - if (get_reading(state.distance_cm)) { - // 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 > 500) { - set_status(RangeFinder::Status::NoData); - } -} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index bface028d4..200a5dd341 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h @@ -10,9 +10,6 @@ public: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; - // update state - void update(void) override; - protected: MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { @@ -23,6 +20,8 @@ private: // get a reading bool get_reading(uint16_t &reading_cm) override; + uint16_t read_timeout_ms() const override { return 500; } + char linebuf[10]; uint8_t linebuf_len = 0; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index a13d3cfd8f..37788b968a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -20,19 +20,6 @@ extern const AP_HAL::HAL& hal; -// update the state of the sensor -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 - state.last_reading_ms = now; - update_status(); - } else if ((now - state.last_reading_ms) > 3000) { - set_status(RangeFinder::Status::NoData); - } -} - // return last value measured by sensor bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index af64a6ac86..e55879fec7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -25,9 +25,6 @@ public: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; - // update state - void update(void) override; - protected: virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { @@ -46,6 +43,8 @@ private: // get a reading bool get_reading(uint16_t &reading_cm) override; + uint16_t read_timeout_ms() const override { return 3000; } + // add a single character to the buffer and attempt to decode // returns true if a complete sentence was successfully decoded // distance should be pulled directly from _distance_m member diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp index dc2d13781b..333ccaedf8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp @@ -173,17 +173,3 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) return true; } - -/* - update the state of the sensor -*/ -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 - update_status(); - } else if (AP_HAL::millis() - state.last_reading_ms > 200) { - set_status(RangeFinder::Status::NoData); - } -} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h index 2cd6f6fd79..688d516dfa 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h @@ -10,9 +10,6 @@ public: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; - // update state - void update(void) override; - protected: MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp index 30bc07f3c1..3a9b3e0e6b 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.cpp @@ -62,7 +62,7 @@ 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 > 200) { + } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) { set_status(RangeFinder::Status::NoData); } } diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h index 371ef77175..fb731db22c 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend_Serial.h @@ -30,4 +30,7 @@ protected: // it is essential that anyone relying on the base-class update to // implement this: virtual bool get_reading(uint16_t &reading_cm) { return 0; } + + // maximum time between readings before we change state to NoData: + virtual uint16_t read_timeout_ms() const { return 200; } };