From 0f7c3e39640911fd19677eda5674cfe116c7a6c8 Mon Sep 17 00:00:00 2001 From: Sebastian Quilter Date: Mon, 18 Oct 2021 16:45:33 +1100 Subject: [PATCH] AP_RangeFinder: make rangefinder ranges m rather than cm --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 9 +++++++++ libraries/AP_RangeFinder/AP_RangeFinder.h | 3 ++- .../AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp | 6 +++--- libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h | 4 ++-- .../AP_RangeFinder/AP_RangeFinder_Backend.cpp | 4 ++-- libraries/AP_RangeFinder/AP_RangeFinder_Backend.h | 3 ++- .../AP_RangeFinder_Backend_Serial.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_Backend_Serial.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_Benewake.cpp | 8 ++++---- libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h | 4 ++-- .../AP_RangeFinder_Benewake_TFMiniPlus.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp | 4 ++-- libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp | 14 +++++++------- libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h | 4 ++-- libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp | 8 ++++---- libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp | 4 ++-- .../AP_RangeFinder/AP_RangeFinder_LeddarOne.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp | 6 +++--- .../AP_RangeFinder/AP_RangeFinder_LeddarVu8.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp | 14 +++++++------- .../AP_RangeFinder/AP_RangeFinder_LightWareI2C.h | 4 ++-- .../AP_RangeFinder_LightWareSerial.cpp | 6 +++--- .../AP_RangeFinder_LightWareSerial.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_MAVLink.cpp | 4 ++-- libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp | 4 ++-- .../AP_RangeFinder_MaxsonarI2CXL.cpp | 2 +- .../AP_RangeFinder_MaxsonarSerialLV.cpp | 6 +++--- .../AP_RangeFinder_MaxsonarSerialLV.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp | 4 ++-- libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp | 10 +++++----- libraries/AP_RangeFinder/AP_RangeFinder_PWM.h | 2 +- .../AP_RangeFinder_PulsedLightLRF.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp | 2 +- .../AP_RangeFinder_TeraRangerI2C.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp | 6 +++--- libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_uLanding.cpp | 6 +++--- libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h | 2 +- 48 files changed, 102 insertions(+), 91 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index c00641a596..fd50f43727 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -663,6 +663,15 @@ AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) co return nullptr; } +float RangeFinder::distance_orient(enum Rotation orientation) const +{ + AP_RangeFinder_Backend *backend = find_instance(orientation); + if (backend == nullptr) { + return 0; + } + return backend->distance(); +} + uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 471a1016c0..68cd421ab7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -107,7 +107,7 @@ public: // The RangeFinder_State structure is filled in by the backend driver struct RangeFinder_State { - uint16_t distance_cm; // distance: in cm + float distance_m; // distance in meters uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0 enum RangeFinder::Status status; // sensor status uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10) @@ -170,6 +170,7 @@ public: // methods to return a distance on a particular orientation from // any sensor which can current supply it + float distance_orient(enum Rotation orientation) const; uint16_t distance_cm_orient(enum Rotation orientation) const; int16_t max_distance_cm_orient(enum Rotation orientation) const; int16_t min_distance_cm_orient(enum Rotation orientation) const; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp index 60ef18f6e0..372bab31de 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp @@ -111,7 +111,7 @@ bool AP_RangeFinder_BBB_PRU::detect() void AP_RangeFinder_BBB_PRU::update(void) { state.status = (RangeFinder::Status)rangerpru->status; - state.distance_cm = rangerpru->distance; + state.distance_m = rangerpru->distance * 0.01f; state.last_reading_ms = AP_HAL::millis(); } #endif // CONFIG_HAL_BOARD_SUBTYPE diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp index ea3e28593b..bfc87f00d6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp @@ -45,8 +45,8 @@ void AP_RangeFinder_BLPing::init_sensor() protocol.send_message(uart, PingProtocol::MessageId::CONTINUOUS_START, reinterpret_cast(&continuous_message), sizeof(continuous_message)); } -// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal -bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm) +// distance returned in reading_m, signal_ok is set to true if sensor reports a strong signal +bool AP_RangeFinder_BLPing::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -73,7 +73,7 @@ bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm) if (averageStruct.count > 0) { // return average distance of readings - reading_cm = averageStruct.mean(); + reading_m = averageStruct.mean() * 0.01f; return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h index ed906113dd..92d9fed435 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h @@ -151,11 +151,11 @@ private: /** * @brief Read serial interface and calculate new distance * - * @param reading_cm + * @param reading_m * @return true * @return false */ - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; /** * @brief Timeout between messages diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp index 17ccb035a5..0809cd516a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp @@ -56,9 +56,9 @@ bool AP_RangeFinder_Backend::has_data() const { void AP_RangeFinder_Backend::update_status() { // check distance - if ((int16_t)state.distance_cm > max_distance_cm()) { + if (state.distance_m > max_distance_cm() * 0.01f) { set_status(RangeFinder::Status::OutOfRangeHigh); - } else if ((int16_t)state.distance_cm < min_distance_cm()) { + } else if (state.distance_m < min_distance_cm() * 0.01f) { set_status(RangeFinder::Status::OutOfRangeLow); } else { set_status(RangeFinder::Status::Good); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index 2567f1a7b8..6751180d6c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -38,7 +38,8 @@ public: #endif enum Rotation orientation() const { return (Rotation)params.orientation.get(); } - uint16_t distance_cm() const { return state.distance_cm; } + float distance() const { return state.distance_m; } + uint16_t distance_cm() const { return state.distance_m*100.0f; } uint16_t voltage_mv() const { return state.voltage_mv; } virtual int16_t max_distance_cm() const { return params.max_distance_cm; } virtual int16_t min_distance_cm() const { return params.min_distance_cm; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp index 50cc913dd0..fc618c6d32 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp @@ -62,7 +62,7 @@ bool AP_RangeFinder_Backend_Serial::detect(uint8_t serial_instance) */ void AP_RangeFinder_Backend_Serial::update(void) { - if (get_reading(state.distance_cm)) { + if (get_reading(state.distance_m)) { // update range_valid state based on distance measured state.last_reading_ms = AP_HAL::millis(); update_status(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h index 8fb67c2ff8..ce117f8e5a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h @@ -29,7 +29,7 @@ protected: // it is essential that anyone relying on the base-class update to // implement this: - virtual bool get_reading(uint16_t &reading_cm) = 0; + virtual bool get_reading(float &reading_m) = 0; // maximum time between readings before we change state to NoData: virtual uint16_t read_timeout_ms() const { return 200; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp index a43529c695..a9028ef2c9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp @@ -271,7 +271,7 @@ void AP_RangeFinder_Bebop::update(void) first_call = false; } - state.distance_cm = (uint16_t) (_altitude * 100); + state.distance_m = _altitude; state.last_reading_ms = AP_HAL::millis(); update_status(); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index aa2bf2d05a..fed7778c48 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -45,8 +45,8 @@ extern const AP_HAL::HAL& hal; // byte 7 (TF02 only) TIME Exposure time in two levels 0x03 and 0x06 // byte 8 Checksum Checksum byte, sum of bytes 0 to bytes 7 -// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal -bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm) +// distance returned in reading_m, signal_ok is set to true if sensor reports a strong signal +bool AP_RangeFinder_Benewake::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -118,14 +118,14 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm) if (count > 0) { // return average distance of readings - reading_cm = sum_cm / count; + reading_m = (sum_cm * 0.01f) / count; return true; } if (count_out_of_range > 0) { // if only out of range readings return larger of // driver defined maximum range for the model and user defined max range + 1m - reading_cm = MAX(model_dist_max_cm(), max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM); + reading_m = MAX(model_dist_max_cm(), max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM) * 0.01f; return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h index 121949ca58..4dbe430d07 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h @@ -22,8 +22,8 @@ protected: private: // get a reading - // distance returned in reading_cm - bool get_reading(uint16_t &reading_cm) override; + // distance returned in reading_m + bool get_reading(float &reading_m) override; uint8_t linebuf[10]; uint8_t linebuf_len; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp index fd8536423b..6b0cd40cab 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp @@ -141,7 +141,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update() WITH_SEMAPHORE(_sem); if (accum.count > 0) { - state.distance_cm = accum.sum / accum.count; + state.distance_m = (accum.sum * 0.01f) / accum.count; state.last_reading_ms = AP_HAL::millis(); accum.sum = 0; accum.count = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp index c67584c064..8132235bb9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp @@ -34,7 +34,7 @@ bool AP_RangeFinder_GYUS42v2::find_signature_in_buffer(uint8_t start) } // get_reading - read a value from the sensor -bool AP_RangeFinder_GYUS42v2::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_GYUS42v2::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -65,7 +65,7 @@ bool AP_RangeFinder_GYUS42v2::get_reading(uint16_t &reading_cm) return false; } - reading_cm = buffer[4] << 8 | buffer[5]; + reading_m = (buffer[4] << 8 | buffer[5]) * 0.01f; buffer_used = 0; return true; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h index 4a7da24130..3403b86d59 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h @@ -23,7 +23,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; // find signature byte in buffer starting at start, moving that // byte and following bytes to start of buffer. diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp index 35094aeee6..877fba0d92 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp @@ -87,7 +87,7 @@ void AP_RangeFinder_HC_SR04::update(void) // check if pin has changed and configure interrupt handlers if required: if (!check_pins()) { // disabled (either by configuration or failure to attach interrupt) - state.distance_cm = 0.0f; + state.distance_m = 0.0f; return; } @@ -98,27 +98,27 @@ void AP_RangeFinder_HC_SR04::update(void) // no reading; check for timeout: if (now - last_reading_ms > 1000) { // no reading for a second - something is broken - state.distance_cm = 0.0f; + state.distance_m = 0.0f; } } else { // gcs().send_text(MAV_SEVERITY_WARNING, "Pong!"); // a new reading - convert time to distance - state.distance_cm = value_us * (1.0/58.0f); // 58 is from datasheet, mult for performance + state.distance_m = (value_us * (1.0/58.0f)) * 0.01f; // 58 is from datasheet, mult for performance // glitch remover: measurement is greater than .5m from last. // the SR-04 seeems to suffer from single-measurement glitches // which can be removed by a simple filter. - if (labs(int32_t(uint32_t(state.distance_cm) - last_distance_cm)) > 50) { + if (fabsf(state.distance_m - last_distance_m) > 0.5f) { // if greater for 5 readings then pass it as new height, // otherwise use last reading if (glitch_count++ > 4) { - last_distance_cm = state.distance_cm; + last_distance_m = state.distance_m; } else { - state.distance_cm = last_distance_cm; + state.distance_m = last_distance_m; } } else { // is not greater 0.5m, pass on and reset glitch counter - last_distance_cm = state.distance_cm; + last_distance_m = state.distance_m; glitch_count = 0; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h index 3d80f43d24..5c8abe4589 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h @@ -30,8 +30,8 @@ private: int8_t trigger_pin; uint32_t last_reading_ms; // system time of last read (used for health reporting) - uint32_t last_distance_cm; // last distance reported (used to prevent glitches in measurement) - uint8_t glitch_count; // glitch counter + float last_distance_m; // last distance reported (used to prevent glitches in measurement) + uint8_t glitch_count; // glitch counter AP_HAL::PWMSource pwm_source; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp index 1da6a423fe..6fcfc85116 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp @@ -28,10 +28,10 @@ extern const AP_HAL::HAL& hal; the sky. For this reason we limit the max range to 6 meters as otherwise we may be giving false data */ -#define LANBAO_MAX_RANGE_CM 600 +#define LANBAO_MAX_RANGE_M 6 // read - return last value measured by sensor -bool AP_RangeFinder_Lanbao::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_Lanbao::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -75,8 +75,8 @@ bool AP_RangeFinder_Lanbao::get_reading(uint16_t &reading_cm) } } if (count > 0) { - reading_cm = (sum_range / count) * 100; - return reading_cm <= LANBAO_MAX_RANGE_CM?true:false; + reading_m = (sum_range / count); + return reading_m <= LANBAO_MAX_RANGE_M?true:false; } return false; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h index 593ca2b030..86c85ff867 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h @@ -23,7 +23,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; uint8_t buf[6]; uint8_t buf_len = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index dc25564dfb..ce9ca93989 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -21,7 +21,7 @@ extern const AP_HAL::HAL& hal; // read - return last value measured by sensor -bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_LeddarOne::get_reading(float &reading_m) { uint8_t number_detections; LeddarOne_Status leddarone_status; @@ -70,7 +70,7 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm) leddarone_status = parse_response(number_detections); if (leddarone_status == LEDDARONE_STATE_OK) { - reading_cm = sum_distance / number_detections; + reading_m = (sum_distance * 0.01f) / number_detections; // reset mod_bus status to read new buffer modbus_status = LEDDARONE_MODBUS_STATE_INIT; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index b2b8dddee3..082a46f3f2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -52,7 +52,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; // CRC16 bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp index 4ad4f7da0f..1f791066fc 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -29,7 +29,7 @@ extern const AP_HAL::HAL& hal; #define LEDDARVU8_TIMEOUT_MS 200 // timeout in milliseconds if no distance messages received // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal -bool AP_RangeFinder_LeddarVu8::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -73,11 +73,11 @@ bool AP_RangeFinder_LeddarVu8::get_reading(uint16_t &reading_cm) if (count > 0) { // return average distance of readings - reading_cm = sum_cm / count; + reading_m = (sum_cm * 0.01f) / count; } else { // if only out of range readings return larger of // driver defined maximum range for the model and user defined max range + 1m - reading_cm = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM); + reading_m = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM)/100.0f; } return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h index 39b46afa5a..75fa8dd67f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h @@ -25,7 +25,7 @@ protected: } // get a reading, distance returned in reading_cm - bool get_reading(uint16_t &reading_cm) override; + 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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index 1acc9e404e..8ae8e50e3b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -337,7 +337,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_init() } // read - return last value measured by sensor -bool AP_RangeFinder_LightWareI2C::legacy_get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_LightWareI2C::legacy_get_reading(float &reading_m) { be16_t val; @@ -348,9 +348,9 @@ bool AP_RangeFinder_LightWareI2C::legacy_get_reading(uint16_t &reading_cm) int16_t signed_val = int16_t(be16toh(val)); if (signed_val < 0) { // some lidar firmwares will return 65436 for out of range - reading_cm = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM); + reading_m = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM) * 0.01f; } else { - reading_cm = uint16_t(signed_val); + reading_m = uint16_t(signed_val) * 0.01f; } return true; } @@ -358,7 +358,7 @@ bool AP_RangeFinder_LightWareI2C::legacy_get_reading(uint16_t &reading_cm) } // read - return last value measured by sf20 sensor -bool AP_RangeFinder_LightWareI2C::sf20_get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m) { // Parses up to 5 ASCII streams for LiDAR data. // If a parse fails, the stream measurement is not updated until it is successfully read in the future. @@ -378,7 +378,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_get_reading(uint16_t &reading_cm) } if (i==0) { - reading_cm = sf20_stream_val[0]; + reading_m = sf20_stream_val[0] * 0.01f; } // Increment the stream sequence @@ -462,7 +462,7 @@ void AP_RangeFinder_LightWareI2C::update(void) void AP_RangeFinder_LightWareI2C::legacy_timer(void) { - if (legacy_get_reading(state.distance_cm)) { + if (legacy_get_reading(state.distance_m)) { // update range_valid state based on distance measured update_status(); } else { @@ -472,7 +472,7 @@ void AP_RangeFinder_LightWareI2C::legacy_timer(void) void AP_RangeFinder_LightWareI2C::sf20_timer(void) { - if (sf20_get_reading(state.distance_cm)) { + if (sf20_get_reading(state.distance_m)) { // update range_valid state based on distance measured update_status(); } else { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h index 873b289ef2..fd75e483c2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h @@ -49,8 +49,8 @@ private: void sf20_timer(); // get a reading - bool legacy_get_reading(uint16_t &reading_cm); - bool sf20_get_reading(uint16_t &reading_cm); + bool legacy_get_reading(float &reading_m); + bool sf20_get_reading(float &reading_m); bool sf20_parse_stream(uint8_t *stream_buf, size_t *p_num_processed_chars, const char *string_identifier, diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index ef015441d4..9e823dcaa8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -24,7 +24,7 @@ extern const AP_HAL::HAL& hal; #define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100 // read - return last value measured by sensor -bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -115,13 +115,13 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm) // return average of all valid readings if (valid_count > 0) { - reading_cm = 100 * sum / valid_count; + reading_m = sum / valid_count; return true; } // all readings were invalid so return out-of-range-high value if (invalid_count > 0) { - reading_cm = MIN(MAX(LIGHTWARE_DIST_MAX_CM, max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX); + reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f; return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h index e8e74b0871..d98391f314 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h @@ -18,7 +18,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; char linebuf[10]; // legacy protocol buffer uint8_t linebuf_len; // legacy protocol buffer length diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index cbae721d22..a441e017b9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -67,9 +67,9 @@ void AP_RangeFinder_MAVLink::update(void) //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_cm = 0; + state.distance_m = 0.0f; } else { - state.distance_cm = distance_cm; + state.distance_m = distance_cm * 0.01f; update_status(); } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp index 3754f7025b..e5ee32bdc5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp @@ -61,9 +61,9 @@ void AP_RangeFinder_MSP::update(void) //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_cm = 0; + state.distance_m = 0.0f; } else { - state.distance_cm = distance_cm; + state.distance_m = distance_cm * 0.01f; update_status(); } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index d4812776ae..020c0e6e71 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -143,7 +143,7 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void) { WITH_SEMAPHORE(_sem); if (new_distance) { - state.distance_cm = distance; + state.distance_m = distance * 0.01f; new_distance = false; update_status(); } else if (AP_HAL::millis() - state.last_reading_ms > 300) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index 2b1acc25c3..62f412ffd0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp @@ -25,7 +25,7 @@ extern const AP_HAL::HAL& hal; // read - return last value measured by sensor -bool AP_RangeFinder_MaxsonarSerialLV::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_MaxsonarSerialLV::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -55,8 +55,8 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(uint16_t &reading_cm) return false; } - // This sonar gives the metrics in inches, so we have to transform this to centimeters - reading_cm = 2.54f * sum / count; + // This sonar gives the metrics in inches, so we have to transform this to meters + reading_m = (2.54f * 0.01f) * (float(sum) / count); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index 679d62cd84..916cc079d5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h @@ -18,7 +18,7 @@ protected: private: // get a reading - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; uint16_t read_timeout_ms() const override { return 500; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index c36ab22c18..02f5f18c54 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -21,7 +21,7 @@ extern const AP_HAL::HAL& hal; // return last value measured by sensor -bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_NMEA::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -45,7 +45,7 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) } // return average of all measurements - reading_cm = 100.0f * sum / count; + reading_m = sum / count; return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index 063d7e3301..ccb1220320 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -43,7 +43,7 @@ private: }; // get a distance reading - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; // get temperature reading in C. returns true on success and populates temp argument bool get_temp(float &temp) const override; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp index 5a11fb2e6d..c30c93c418 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp @@ -40,14 +40,14 @@ bool AP_RangeFinder_PWM::detect() } // read - return last value measured by sensor -bool AP_RangeFinder_PWM::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_PWM::get_reading(float &reading_m) { const uint32_t value_us = pwm_source.get_pwm_avg_us(); if (value_us == 0) { return false; } - reading_cm = value_us/10; // correct for LidarLite. Parameter needed? Converts from decimetres -> cm here + reading_m = value_us * 10.0f; // correct for LidarLite. Parameter needed? Converts from decimetres -> m here return true; } @@ -94,7 +94,7 @@ void AP_RangeFinder_PWM::update(void) // we are above the power saving range. Disable the sensor hal.gpio->write(params.stop_pin, false); set_status(RangeFinder::Status::NoData); - state.distance_cm = 0; + state.distance_m = 0.0f; state.voltage_mv = 0; was_out_of_range = oor; } @@ -107,7 +107,7 @@ void AP_RangeFinder_PWM::update(void) } } - if (!get_reading(state.distance_cm)) { + 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); @@ -115,7 +115,7 @@ void AP_RangeFinder_PWM::update(void) return; } // add offset - state.distance_cm += params.offset; + state.distance_m += params.offset * 0.01f; // update range_valid state based on distance measured state.last_reading_ms = AP_HAL::millis(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h index e6dfd04653..7e64a54ae3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h @@ -36,7 +36,7 @@ public: protected: - bool get_reading(uint16_t &reading_cm); + bool get_reading(float &reading_m); MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { return MAV_DISTANCE_SENSOR_UNKNOWN; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index cf500a6bb0..89009f6775 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -91,7 +91,7 @@ void AP_RangeFinder_PulsedLightLRF::timer(void) uint16_t _distance_cm = be16toh(val); // remove momentary spikes if (abs(_distance_cm - last_distance_cm) < 100) { - state.distance_cm = _distance_cm; + state.distance_m = _distance_cm * 0.01f; state.last_reading_ms = AP_HAL::millis(); update_status(); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp index f8c2d5b64f..a2903a90a7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp @@ -42,7 +42,7 @@ void AP_RangeFinder_SITL::update(void) return; } - state.distance_cm = dist * 100.0f; + state.distance_m = dist; state.last_reading_ms = AP_HAL::millis(); // update range_valid state based on distance measured diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp index 5f4e6c4871..312e9fd97d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp @@ -180,7 +180,7 @@ void AP_RangeFinder_TeraRangerI2C::update(void) WITH_SEMAPHORE(_sem); if (accum.count > 0) { - state.distance_cm = accum.sum / accum.count; + state.distance_m = (accum.sum * 0.01f) / accum.count; state.last_reading_ms = AP_HAL::millis(); accum.sum = 0; accum.count = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp index decdbaf206..41a396374e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp @@ -108,7 +108,7 @@ void AP_RangeFinder_UAVCAN::update() set_status(RangeFinder::Status::NoData); } else if (_status == RangeFinder::Status::Good && new_data) { //copy over states - state.distance_cm = _distance_cm; + state.distance_m = _distance_cm * 0.01f; state.last_reading_ms = _last_reading_ms; update_status(); new_data = false; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp index 578f8f559b..07307cfc1e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp @@ -21,7 +21,7 @@ void AP_RangeFinder_USD1_CAN::update(void) // if data is older than 500ms, report NoData set_status(RangeFinder::Status::NoData); } else if (new_data) { - state.distance_cm = _distance_cm; + state.distance_m = _distance_cm * 0.01f; state.last_reading_ms = _last_reading_ms; update_status(); new_data = false; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp index 4bd032d4b4..05b748f2fd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp @@ -762,7 +762,7 @@ uint16_t AP_RangeFinder_VL53L0X::read_register16(uint8_t reg) void AP_RangeFinder_VL53L0X::update(void) { if (counter > 0) { - state.distance_cm = sum_mm / (10*counter); + state.distance_m = (sum_mm * 0.001f) / counter; state.last_reading_ms = AP_HAL::millis(); sum_mm = 0; counter = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index 17f43c0554..1b9df36344 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp @@ -563,7 +563,7 @@ void AP_RangeFinder_VL53L1X::update(void) { WITH_SEMAPHORE(_sem); if (counter > 0) { - state.distance_cm = sum_mm / (10*counter); + state.distance_m = (sum_mm * 0.001f) / counter; state.last_reading_ms = AP_HAL::millis(); update_status(); sum_mm = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp index 6da29e7d3c..36fed34a8a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp @@ -76,7 +76,7 @@ AP_RangeFinder_Wasp::AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state, } // read - return last value measured by sensor -bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) { +bool AP_RangeFinder_Wasp::get_reading(float &reading_m) { if (uart == nullptr) { return false; } @@ -118,7 +118,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) { return false; } - reading_cm = 100 * sum / count; + reading_m = sum / count; set_status(RangeFinder::Status::Good); return true; @@ -127,7 +127,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) { #define COMMAND_BUFFER_LEN 15 void AP_RangeFinder_Wasp::update(void) { - if (!get_reading(state.distance_cm)) { + if (!get_reading(state.distance_m)) { set_status(RangeFinder::Status::NoData); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h index 2ccc2bd33f..a58f779ff6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h @@ -46,7 +46,7 @@ private: wasp_configuration_stage configuration_state = WASP_CFG_PROTOCOL; - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; void parse_response(void); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index 980bfe7c54..33cd9156a0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -111,7 +111,7 @@ void AP_RangeFinder_analog::update(void) if (dist_m < 0) { dist_m = 0; } - state.distance_cm = dist_m * 100.0f; + state.distance_m = dist_m; state.last_reading_ms = AP_HAL::millis(); // update range_valid state based on distance measured diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp index 333ccaedf8..72aed803f3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp @@ -104,7 +104,7 @@ bool AP_RangeFinder_uLanding::detect_version(void) // read - return last value measured by sensor -bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) +bool AP_RangeFinder_uLanding::get_reading(float &reading_m) { if (uart == nullptr) { return false; @@ -165,10 +165,10 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) return false; } - reading_cm = sum / count; + reading_m = (sum * 0.01f) / count; if (_version == 0 && _header != ULANDING_HDR) { - reading_cm *= 2.5f; + reading_m *= 2.5f; } return true; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h index 2ae37df152..cfe7170046 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h @@ -29,7 +29,7 @@ private: bool detect_version(void); // get a reading - bool get_reading(uint16_t &reading_cm) override; + bool get_reading(float &reading_m) override; uint8_t _linebuf[6]; uint8_t _linebuf_len;