Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: make rangefinder ranges m rather than cm

This commit is contained in:
Sebastian Quilter 2021-10-18 16:45:33 +11:00 committed by Peter Barker
parent 1fb1eb793f
commit 0f7c3e3964
48 changed files with 102 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -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);

3
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp
View File

@ -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

6
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp
View File

@ -45,8 +45,8 @@ void AP_RangeFinder_BLPing::init_sensor()
protocol.send_message(uart, PingProtocol::MessageId::CONTINUOUS_START, reinterpret_cast<uint8_t*>(&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;
}

4
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h
View File

@ -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

4
libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp
View File

@ -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);

3
libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
View File

@ -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; }

2
libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp
View File

@ -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();

2
libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h
View File

@ -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; }

2
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp
View File

@ -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();
}

8
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -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;
}

4
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp
View File

@ -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;

4
libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h
View File

@ -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.

14
libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp
View File

@ -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;
}

4
libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h
View File

@ -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;

8
libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp
View File

@ -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;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h
View File

@ -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;

4
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -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);

6
libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp
View File

@ -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;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h
View File

@ -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; }

14
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -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 {

4
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h
View File

@ -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,

6
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -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;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
View File

@ -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

4
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -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();
}
}

4
libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp
View File

@ -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();
}
}

2
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@ -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) {

6
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp
View File

@ -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;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
View File

@ -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; }

4
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp
View File

@ -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;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h
View File

@ -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;

10
libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp
View File

@ -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();

2
libraries/AP_RangeFinder/AP_RangeFinder_PWM.h
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -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();
}

2
libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp
View File

@ -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

2
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
View File

@ -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;

6
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp
View File

@ -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);
}

2
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h
View File

@ -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);

2
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -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

6
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp
View File

@ -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;

2
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h
View File

@ -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;