mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder: make rangefinder ranges m rather than cm
This commit is contained in:
parent
1fb1eb793f
commit
0f7c3e3964
|
@ -663,6 +663,15 @@ AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) co
|
||||||
return nullptr;
|
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
|
uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
|
||||||
{
|
{
|
||||||
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
||||||
|
|
|
@ -107,7 +107,7 @@ public:
|
||||||
|
|
||||||
// The RangeFinder_State structure is filled in by the backend driver
|
// The RangeFinder_State structure is filled in by the backend driver
|
||||||
struct RangeFinder_State {
|
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
|
uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0
|
||||||
enum RangeFinder::Status status; // sensor status
|
enum RangeFinder::Status status; // sensor status
|
||||||
uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10)
|
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
|
// methods to return a distance on a particular orientation from
|
||||||
// any sensor which can current supply it
|
// any sensor which can current supply it
|
||||||
|
float distance_orient(enum Rotation orientation) const;
|
||||||
uint16_t distance_cm_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 max_distance_cm_orient(enum Rotation orientation) const;
|
||||||
int16_t min_distance_cm_orient(enum Rotation orientation) const;
|
int16_t min_distance_cm_orient(enum Rotation orientation) const;
|
||||||
|
|
|
@ -111,7 +111,7 @@ bool AP_RangeFinder_BBB_PRU::detect()
|
||||||
void AP_RangeFinder_BBB_PRU::update(void)
|
void AP_RangeFinder_BBB_PRU::update(void)
|
||||||
{
|
{
|
||||||
state.status = (RangeFinder::Status)rangerpru->status;
|
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();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
}
|
}
|
||||||
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
||||||
|
|
|
@ -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));
|
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
|
// distance returned in reading_m, signal_ok is set to true if sensor reports a strong signal
|
||||||
bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm)
|
bool AP_RangeFinder_BLPing::get_reading(float &reading_m)
|
||||||
{
|
{
|
||||||
if (uart == nullptr) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -73,7 +73,7 @@ bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm)
|
||||||
|
|
||||||
if (averageStruct.count > 0) {
|
if (averageStruct.count > 0) {
|
||||||
// return average distance of readings
|
// return average distance of readings
|
||||||
reading_cm = averageStruct.mean();
|
reading_m = averageStruct.mean() * 0.01f;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -151,11 +151,11 @@ private:
|
||||||
/**
|
/**
|
||||||
* @brief Read serial interface and calculate new distance
|
* @brief Read serial interface and calculate new distance
|
||||||
*
|
*
|
||||||
* @param reading_cm
|
* @param reading_m
|
||||||
* @return true
|
* @return true
|
||||||
* @return false
|
* @return false
|
||||||
*/
|
*/
|
||||||
bool get_reading(uint16_t &reading_cm) override;
|
bool get_reading(float &reading_m) override;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Timeout between messages
|
* @brief Timeout between messages
|
||||||
|
|
|
@ -56,9 +56,9 @@ bool AP_RangeFinder_Backend::has_data() const {
|
||||||
void AP_RangeFinder_Backend::update_status()
|
void AP_RangeFinder_Backend::update_status()
|
||||||
{
|
{
|
||||||
// check distance
|
// 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);
|
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);
|
set_status(RangeFinder::Status::OutOfRangeLow);
|
||||||
} else {
|
} else {
|
||||||
set_status(RangeFinder::Status::Good);
|
set_status(RangeFinder::Status::Good);
|
||||||
|
|
|
@ -38,7 +38,8 @@ public:
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
|
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; }
|
uint16_t voltage_mv() const { return state.voltage_mv; }
|
||||||
virtual int16_t max_distance_cm() const { return params.max_distance_cm; }
|
virtual int16_t max_distance_cm() const { return params.max_distance_cm; }
|
||||||
virtual int16_t min_distance_cm() const { return params.min_distance_cm; }
|
virtual int16_t min_distance_cm() const { return params.min_distance_cm; }
|
||||||
|
|
|
@ -62,7 +62,7 @@ bool AP_RangeFinder_Backend_Serial::detect(uint8_t serial_instance)
|
||||||
*/
|
*/
|
||||||
void AP_RangeFinder_Backend_Serial::update(void)
|
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
|
// update range_valid state based on distance measured
|
||||||
state.last_reading_ms = AP_HAL::millis();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
update_status();
|
update_status();
|
||||||
|
|
|
@ -29,7 +29,7 @@ protected:
|
||||||
|
|
||||||
// it is essential that anyone relying on the base-class update to
|
// it is essential that anyone relying on the base-class update to
|
||||||
// implement this:
|
// 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:
|
// maximum time between readings before we change state to NoData:
|
||||||
virtual uint16_t read_timeout_ms() const { return 200; }
|
virtual uint16_t read_timeout_ms() const { return 200; }
|
||||||
|
|
|
@ -271,7 +271,7 @@ void AP_RangeFinder_Bebop::update(void)
|
||||||
first_call = false;
|
first_call = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
state.distance_cm = (uint16_t) (_altitude * 100);
|
state.distance_m = _altitude;
|
||||||
state.last_reading_ms = AP_HAL::millis();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
update_status();
|
update_status();
|
||||||
}
|
}
|
||||||
|
|
|
@ -45,8 +45,8 @@ extern const AP_HAL::HAL& hal;
|
||||||
// byte 7 (TF02 only) TIME Exposure time in two levels 0x03 and 0x06
|
// 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
|
// 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
|
// distance returned in reading_m, signal_ok is set to true if sensor reports a strong signal
|
||||||
bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
|
bool AP_RangeFinder_Benewake::get_reading(float &reading_m)
|
||||||
{
|
{
|
||||||
if (uart == nullptr) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -118,14 +118,14 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
|
||||||
|
|
||||||
if (count > 0) {
|
if (count > 0) {
|
||||||
// return average distance of readings
|
// return average distance of readings
|
||||||
reading_cm = sum_cm / count;
|
reading_m = (sum_cm * 0.01f) / count;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (count_out_of_range > 0) {
|
if (count_out_of_range > 0) {
|
||||||
// if only out of range readings return larger of
|
// if only out of range readings return larger of
|
||||||
// driver defined maximum range for the model and user defined max range + 1m
|
// 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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -22,8 +22,8 @@ protected:
|
||||||
private:
|
private:
|
||||||
|
|
||||||
// get a reading
|
// get a reading
|
||||||
// distance returned in reading_cm
|
// distance returned in reading_m
|
||||||
bool get_reading(uint16_t &reading_cm) override;
|
bool get_reading(float &reading_m) override;
|
||||||
|
|
||||||
uint8_t linebuf[10];
|
uint8_t linebuf[10];
|
||||||
uint8_t linebuf_len;
|
uint8_t linebuf_len;
|
||||||
|
|
|
@ -141,7 +141,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update()
|
||||||
WITH_SEMAPHORE(_sem);
|
WITH_SEMAPHORE(_sem);
|
||||||
|
|
||||||
if (accum.count > 0) {
|
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();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
accum.sum = 0;
|
accum.sum = 0;
|
||||||
accum.count = 0;
|
accum.count = 0;
|
||||||
|
|
|
@ -34,7 +34,7 @@ bool AP_RangeFinder_GYUS42v2::find_signature_in_buffer(uint8_t start)
|
||||||
}
|
}
|
||||||
|
|
||||||
// get_reading - read a value from the sensor
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -65,7 +65,7 @@ bool AP_RangeFinder_GYUS42v2::get_reading(uint16_t &reading_cm)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
reading_cm = buffer[4] << 8 | buffer[5];
|
reading_m = (buffer[4] << 8 | buffer[5]) * 0.01f;
|
||||||
buffer_used = 0;
|
buffer_used = 0;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -23,7 +23,7 @@ protected:
|
||||||
private:
|
private:
|
||||||
|
|
||||||
// get a reading
|
// 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
|
// find signature byte in buffer starting at start, moving that
|
||||||
// byte and following bytes to start of buffer.
|
// byte and following bytes to start of buffer.
|
||||||
|
|
|
@ -87,7 +87,7 @@ void AP_RangeFinder_HC_SR04::update(void)
|
||||||
// check if pin has changed and configure interrupt handlers if required:
|
// check if pin has changed and configure interrupt handlers if required:
|
||||||
if (!check_pins()) {
|
if (!check_pins()) {
|
||||||
// disabled (either by configuration or failure to attach interrupt)
|
// disabled (either by configuration or failure to attach interrupt)
|
||||||
state.distance_cm = 0.0f;
|
state.distance_m = 0.0f;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -98,27 +98,27 @@ void AP_RangeFinder_HC_SR04::update(void)
|
||||||
// no reading; check for timeout:
|
// no reading; check for timeout:
|
||||||
if (now - last_reading_ms > 1000) {
|
if (now - last_reading_ms > 1000) {
|
||||||
// no reading for a second - something is broken
|
// no reading for a second - something is broken
|
||||||
state.distance_cm = 0.0f;
|
state.distance_m = 0.0f;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// gcs().send_text(MAV_SEVERITY_WARNING, "Pong!");
|
// gcs().send_text(MAV_SEVERITY_WARNING, "Pong!");
|
||||||
// a new reading - convert time to distance
|
// 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.
|
// glitch remover: measurement is greater than .5m from last.
|
||||||
// the SR-04 seeems to suffer from single-measurement glitches
|
// the SR-04 seeems to suffer from single-measurement glitches
|
||||||
// which can be removed by a simple filter.
|
// 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,
|
// if greater for 5 readings then pass it as new height,
|
||||||
// otherwise use last reading
|
// otherwise use last reading
|
||||||
if (glitch_count++ > 4) {
|
if (glitch_count++ > 4) {
|
||||||
last_distance_cm = state.distance_cm;
|
last_distance_m = state.distance_m;
|
||||||
} else {
|
} else {
|
||||||
state.distance_cm = last_distance_cm;
|
state.distance_m = last_distance_m;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// is not greater 0.5m, pass on and reset glitch counter
|
// 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;
|
glitch_count = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,7 +30,7 @@ private:
|
||||||
|
|
||||||
int8_t trigger_pin;
|
int8_t trigger_pin;
|
||||||
uint32_t last_reading_ms; // system time of last read (used for health reporting)
|
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)
|
float last_distance_m; // last distance reported (used to prevent glitches in measurement)
|
||||||
uint8_t glitch_count; // glitch counter
|
uint8_t glitch_count; // glitch counter
|
||||||
|
|
||||||
AP_HAL::PWMSource pwm_source;
|
AP_HAL::PWMSource pwm_source;
|
||||||
|
|
|
@ -28,10 +28,10 @@ extern const AP_HAL::HAL& hal;
|
||||||
the sky. For this reason we limit the max range to 6 meters as
|
the sky. For this reason we limit the max range to 6 meters as
|
||||||
otherwise we may be giving false data
|
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
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -75,8 +75,8 @@ bool AP_RangeFinder_Lanbao::get_reading(uint16_t &reading_cm)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (count > 0) {
|
if (count > 0) {
|
||||||
reading_cm = (sum_range / count) * 100;
|
reading_m = (sum_range / count);
|
||||||
return reading_cm <= LANBAO_MAX_RANGE_CM?true:false;
|
return reading_m <= LANBAO_MAX_RANGE_M?true:false;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,7 +23,7 @@ protected:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// get a reading
|
// 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[6];
|
||||||
uint8_t buf_len = 0;
|
uint8_t buf_len = 0;
|
||||||
|
|
|
@ -21,7 +21,7 @@
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// read - return last value measured by sensor
|
// 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;
|
uint8_t number_detections;
|
||||||
LeddarOne_Status leddarone_status;
|
LeddarOne_Status leddarone_status;
|
||||||
|
@ -70,7 +70,7 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
|
||||||
leddarone_status = parse_response(number_detections);
|
leddarone_status = parse_response(number_detections);
|
||||||
|
|
||||||
if (leddarone_status == LEDDARONE_STATE_OK) {
|
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
|
// reset mod_bus status to read new buffer
|
||||||
modbus_status = LEDDARONE_MODBUS_STATE_INIT;
|
modbus_status = LEDDARONE_MODBUS_STATE_INIT;
|
||||||
|
|
|
@ -52,7 +52,7 @@ protected:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// get a reading
|
// get a reading
|
||||||
bool get_reading(uint16_t &reading_cm) override;
|
bool get_reading(float &reading_m) override;
|
||||||
|
|
||||||
// CRC16
|
// CRC16
|
||||||
bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck);
|
bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck);
|
||||||
|
|
|
@ -29,7 +29,7 @@ extern const AP_HAL::HAL& hal;
|
||||||
#define LEDDARVU8_TIMEOUT_MS 200 // timeout in milliseconds if no distance messages received
|
#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
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -73,11 +73,11 @@ bool AP_RangeFinder_LeddarVu8::get_reading(uint16_t &reading_cm)
|
||||||
|
|
||||||
if (count > 0) {
|
if (count > 0) {
|
||||||
// return average distance of readings
|
// return average distance of readings
|
||||||
reading_cm = sum_cm / count;
|
reading_m = (sum_cm * 0.01f) / count;
|
||||||
} else {
|
} else {
|
||||||
// if only out of range readings return larger of
|
// if only out of range readings return larger of
|
||||||
// driver defined maximum range for the model and user defined max range + 1m
|
// 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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -25,7 +25,7 @@ protected:
|
||||||
}
|
}
|
||||||
|
|
||||||
// get a reading, distance returned in reading_cm
|
// 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:
|
// maximum time between readings before we change state to NoData:
|
||||||
uint16_t read_timeout_ms() const override { return 500; }
|
uint16_t read_timeout_ms() const override { return 500; }
|
||||||
|
|
|
@ -337,7 +337,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_init()
|
||||||
}
|
}
|
||||||
|
|
||||||
// read - return last value measured by sensor
|
// 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;
|
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));
|
int16_t signed_val = int16_t(be16toh(val));
|
||||||
if (signed_val < 0) {
|
if (signed_val < 0) {
|
||||||
// some lidar firmwares will return 65436 for out of range
|
// 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 {
|
} else {
|
||||||
reading_cm = uint16_t(signed_val);
|
reading_m = uint16_t(signed_val) * 0.01f;
|
||||||
}
|
}
|
||||||
return true;
|
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
|
// 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.
|
// 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.
|
// 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) {
|
if (i==0) {
|
||||||
reading_cm = sf20_stream_val[0];
|
reading_m = sf20_stream_val[0] * 0.01f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Increment the stream sequence
|
// Increment the stream sequence
|
||||||
|
@ -462,7 +462,7 @@ void AP_RangeFinder_LightWareI2C::update(void)
|
||||||
|
|
||||||
void AP_RangeFinder_LightWareI2C::legacy_timer(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 range_valid state based on distance measured
|
||||||
update_status();
|
update_status();
|
||||||
} else {
|
} else {
|
||||||
|
@ -472,7 +472,7 @@ void AP_RangeFinder_LightWareI2C::legacy_timer(void)
|
||||||
|
|
||||||
void AP_RangeFinder_LightWareI2C::sf20_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 range_valid state based on distance measured
|
||||||
update_status();
|
update_status();
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -49,8 +49,8 @@ private:
|
||||||
void sf20_timer();
|
void sf20_timer();
|
||||||
|
|
||||||
// get a reading
|
// get a reading
|
||||||
bool legacy_get_reading(uint16_t &reading_cm);
|
bool legacy_get_reading(float &reading_m);
|
||||||
bool sf20_get_reading(uint16_t &reading_cm);
|
bool sf20_get_reading(float &reading_m);
|
||||||
bool sf20_parse_stream(uint8_t *stream_buf,
|
bool sf20_parse_stream(uint8_t *stream_buf,
|
||||||
size_t *p_num_processed_chars,
|
size_t *p_num_processed_chars,
|
||||||
const char *string_identifier,
|
const char *string_identifier,
|
||||||
|
|
|
@ -24,7 +24,7 @@ extern const AP_HAL::HAL& hal;
|
||||||
#define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100
|
#define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100
|
||||||
|
|
||||||
// read - return last value measured by sensor
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -115,13 +115,13 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
|
||||||
|
|
||||||
// return average of all valid readings
|
// return average of all valid readings
|
||||||
if (valid_count > 0) {
|
if (valid_count > 0) {
|
||||||
reading_cm = 100 * sum / valid_count;
|
reading_m = sum / valid_count;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// all readings were invalid so return out-of-range-high value
|
// all readings were invalid so return out-of-range-high value
|
||||||
if (invalid_count > 0) {
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -18,7 +18,7 @@ protected:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// get a reading
|
// get a reading
|
||||||
bool get_reading(uint16_t &reading_cm) override;
|
bool get_reading(float &reading_m) override;
|
||||||
|
|
||||||
char linebuf[10]; // legacy protocol buffer
|
char linebuf[10]; // legacy protocol buffer
|
||||||
uint8_t linebuf_len; // legacy protocol buffer length
|
uint8_t linebuf_len; // legacy protocol buffer length
|
||||||
|
|
|
@ -67,9 +67,9 @@ void AP_RangeFinder_MAVLink::update(void)
|
||||||
//data in 500ms, dump it
|
//data in 500ms, dump it
|
||||||
if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
|
if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
|
||||||
set_status(RangeFinder::Status::NoData);
|
set_status(RangeFinder::Status::NoData);
|
||||||
state.distance_cm = 0;
|
state.distance_m = 0.0f;
|
||||||
} else {
|
} else {
|
||||||
state.distance_cm = distance_cm;
|
state.distance_m = distance_cm * 0.01f;
|
||||||
update_status();
|
update_status();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -61,9 +61,9 @@ void AP_RangeFinder_MSP::update(void)
|
||||||
//data in 500ms, dump it
|
//data in 500ms, dump it
|
||||||
if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MSP_TIMEOUT_MS) {
|
if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MSP_TIMEOUT_MS) {
|
||||||
set_status(RangeFinder::Status::NoData);
|
set_status(RangeFinder::Status::NoData);
|
||||||
state.distance_cm = 0;
|
state.distance_m = 0.0f;
|
||||||
} else {
|
} else {
|
||||||
state.distance_cm = distance_cm;
|
state.distance_m = distance_cm * 0.01f;
|
||||||
update_status();
|
update_status();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -143,7 +143,7 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(_sem);
|
WITH_SEMAPHORE(_sem);
|
||||||
if (new_distance) {
|
if (new_distance) {
|
||||||
state.distance_cm = distance;
|
state.distance_m = distance * 0.01f;
|
||||||
new_distance = false;
|
new_distance = false;
|
||||||
update_status();
|
update_status();
|
||||||
} else if (AP_HAL::millis() - state.last_reading_ms > 300) {
|
} else if (AP_HAL::millis() - state.last_reading_ms > 300) {
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// read - return last value measured by sensor
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -55,8 +55,8 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(uint16_t &reading_cm)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// This sonar gives the metrics in inches, so we have to transform this to centimeters
|
// This sonar gives the metrics in inches, so we have to transform this to meters
|
||||||
reading_cm = 2.54f * sum / count;
|
reading_m = (2.54f * 0.01f) * (float(sum) / count);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -18,7 +18,7 @@ protected:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// get a reading
|
// 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; }
|
uint16_t read_timeout_ms() const override { return 500; }
|
||||||
|
|
||||||
|
|
|
@ -21,7 +21,7 @@
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// return last value measured by sensor
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -45,7 +45,7 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm)
|
||||||
}
|
}
|
||||||
|
|
||||||
// return average of all measurements
|
// return average of all measurements
|
||||||
reading_cm = 100.0f * sum / count;
|
reading_m = sum / count;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -43,7 +43,7 @@ private:
|
||||||
};
|
};
|
||||||
|
|
||||||
// get a distance reading
|
// 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
|
// get temperature reading in C. returns true on success and populates temp argument
|
||||||
bool get_temp(float &temp) const override;
|
bool get_temp(float &temp) const override;
|
||||||
|
|
|
@ -40,14 +40,14 @@ bool AP_RangeFinder_PWM::detect()
|
||||||
}
|
}
|
||||||
|
|
||||||
// read - return last value measured by sensor
|
// 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();
|
const uint32_t value_us = pwm_source.get_pwm_avg_us();
|
||||||
if (value_us == 0) {
|
if (value_us == 0) {
|
||||||
return false;
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -94,7 +94,7 @@ void AP_RangeFinder_PWM::update(void)
|
||||||
// we are above the power saving range. Disable the sensor
|
// we are above the power saving range. Disable the sensor
|
||||||
hal.gpio->write(params.stop_pin, false);
|
hal.gpio->write(params.stop_pin, false);
|
||||||
set_status(RangeFinder::Status::NoData);
|
set_status(RangeFinder::Status::NoData);
|
||||||
state.distance_cm = 0;
|
state.distance_m = 0.0f;
|
||||||
state.voltage_mv = 0;
|
state.voltage_mv = 0;
|
||||||
was_out_of_range = oor;
|
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
|
// failure; consider changing our state
|
||||||
if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||||
set_status(RangeFinder::Status::NoData);
|
set_status(RangeFinder::Status::NoData);
|
||||||
|
@ -115,7 +115,7 @@ void AP_RangeFinder_PWM::update(void)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// add offset
|
// add offset
|
||||||
state.distance_cm += params.offset;
|
state.distance_m += params.offset * 0.01f;
|
||||||
|
|
||||||
// update range_valid state based on distance measured
|
// update range_valid state based on distance measured
|
||||||
state.last_reading_ms = AP_HAL::millis();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
|
|
|
@ -36,7 +36,7 @@ public:
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
bool get_reading(uint16_t &reading_cm);
|
bool get_reading(float &reading_m);
|
||||||
|
|
||||||
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
|
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
|
||||||
return MAV_DISTANCE_SENSOR_UNKNOWN;
|
return MAV_DISTANCE_SENSOR_UNKNOWN;
|
||||||
|
|
|
@ -91,7 +91,7 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
|
||||||
uint16_t _distance_cm = be16toh(val);
|
uint16_t _distance_cm = be16toh(val);
|
||||||
// remove momentary spikes
|
// remove momentary spikes
|
||||||
if (abs(_distance_cm - last_distance_cm) < 100) {
|
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();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
update_status();
|
update_status();
|
||||||
}
|
}
|
||||||
|
|
|
@ -42,7 +42,7 @@ void AP_RangeFinder_SITL::update(void)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
state.distance_cm = dist * 100.0f;
|
state.distance_m = dist;
|
||||||
state.last_reading_ms = AP_HAL::millis();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
|
|
||||||
// update range_valid state based on distance measured
|
// update range_valid state based on distance measured
|
||||||
|
|
|
@ -180,7 +180,7 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
|
||||||
WITH_SEMAPHORE(_sem);
|
WITH_SEMAPHORE(_sem);
|
||||||
|
|
||||||
if (accum.count > 0) {
|
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();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
accum.sum = 0;
|
accum.sum = 0;
|
||||||
accum.count = 0;
|
accum.count = 0;
|
||||||
|
|
|
@ -108,7 +108,7 @@ void AP_RangeFinder_UAVCAN::update()
|
||||||
set_status(RangeFinder::Status::NoData);
|
set_status(RangeFinder::Status::NoData);
|
||||||
} else if (_status == RangeFinder::Status::Good && new_data) {
|
} else if (_status == RangeFinder::Status::Good && new_data) {
|
||||||
//copy over states
|
//copy over states
|
||||||
state.distance_cm = _distance_cm;
|
state.distance_m = _distance_cm * 0.01f;
|
||||||
state.last_reading_ms = _last_reading_ms;
|
state.last_reading_ms = _last_reading_ms;
|
||||||
update_status();
|
update_status();
|
||||||
new_data = false;
|
new_data = false;
|
||||||
|
|
|
@ -21,7 +21,7 @@ void AP_RangeFinder_USD1_CAN::update(void)
|
||||||
// if data is older than 500ms, report NoData
|
// if data is older than 500ms, report NoData
|
||||||
set_status(RangeFinder::Status::NoData);
|
set_status(RangeFinder::Status::NoData);
|
||||||
} else if (new_data) {
|
} else if (new_data) {
|
||||||
state.distance_cm = _distance_cm;
|
state.distance_m = _distance_cm * 0.01f;
|
||||||
state.last_reading_ms = _last_reading_ms;
|
state.last_reading_ms = _last_reading_ms;
|
||||||
update_status();
|
update_status();
|
||||||
new_data = false;
|
new_data = false;
|
||||||
|
|
|
@ -762,7 +762,7 @@ uint16_t AP_RangeFinder_VL53L0X::read_register16(uint8_t reg)
|
||||||
void AP_RangeFinder_VL53L0X::update(void)
|
void AP_RangeFinder_VL53L0X::update(void)
|
||||||
{
|
{
|
||||||
if (counter > 0) {
|
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();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
sum_mm = 0;
|
sum_mm = 0;
|
||||||
counter = 0;
|
counter = 0;
|
||||||
|
|
|
@ -563,7 +563,7 @@ void AP_RangeFinder_VL53L1X::update(void)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(_sem);
|
WITH_SEMAPHORE(_sem);
|
||||||
if (counter > 0) {
|
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();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
update_status();
|
update_status();
|
||||||
sum_mm = 0;
|
sum_mm = 0;
|
||||||
|
|
|
@ -76,7 +76,7 @@ AP_RangeFinder_Wasp::AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
|
||||||
}
|
}
|
||||||
|
|
||||||
// read - return last value measured by sensor
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -118,7 +118,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
reading_cm = 100 * sum / count;
|
reading_m = sum / count;
|
||||||
set_status(RangeFinder::Status::Good);
|
set_status(RangeFinder::Status::Good);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
@ -127,7 +127,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
|
||||||
#define COMMAND_BUFFER_LEN 15
|
#define COMMAND_BUFFER_LEN 15
|
||||||
|
|
||||||
void AP_RangeFinder_Wasp::update(void) {
|
void AP_RangeFinder_Wasp::update(void) {
|
||||||
if (!get_reading(state.distance_cm)) {
|
if (!get_reading(state.distance_m)) {
|
||||||
set_status(RangeFinder::Status::NoData);
|
set_status(RangeFinder::Status::NoData);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -46,7 +46,7 @@ private:
|
||||||
|
|
||||||
wasp_configuration_stage configuration_state = WASP_CFG_PROTOCOL;
|
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);
|
void parse_response(void);
|
||||||
|
|
||||||
|
|
|
@ -111,7 +111,7 @@ void AP_RangeFinder_analog::update(void)
|
||||||
if (dist_m < 0) {
|
if (dist_m < 0) {
|
||||||
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();
|
state.last_reading_ms = AP_HAL::millis();
|
||||||
|
|
||||||
// update range_valid state based on distance measured
|
// update range_valid state based on distance measured
|
||||||
|
|
|
@ -104,7 +104,7 @@ bool AP_RangeFinder_uLanding::detect_version(void)
|
||||||
|
|
||||||
|
|
||||||
// read - return last value measured by sensor
|
// 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) {
|
if (uart == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -165,10 +165,10 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
reading_cm = sum / count;
|
reading_m = (sum * 0.01f) / count;
|
||||||
|
|
||||||
if (_version == 0 && _header != ULANDING_HDR) {
|
if (_version == 0 && _header != ULANDING_HDR) {
|
||||||
reading_cm *= 2.5f;
|
reading_m *= 2.5f;
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -29,7 +29,7 @@ private:
|
||||||
bool detect_version(void);
|
bool detect_version(void);
|
||||||
|
|
||||||
// get a reading
|
// 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[6];
|
||||||
uint8_t _linebuf_len;
|
uint8_t _linebuf_len;
|
||||||
|
|
Loading…
Reference in New Issue