Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-11 10:23:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: allow for more than 327m range rangefinders

Browse Source
This commit is contained in:
Peter Barker 2024-10-11 14:46:48 +11:00 committed by Andrew Tridgell
parent 6ecbc07090
commit fb7925d029
22 changed files with 147 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -190,6 +190,14 @@ RangeFinder::RangeFinder()
_singleton = this;
}
void RangeFinder::convert_params(void)
{
// PARAMETER_CONVERSION - Added: Dec-2024 for 4.6->4.7
for (auto &p : params) {
p.convert_min_max_params();
}
}
/*
initialise the RangeFinder class. We do detection of attached range
finders here. For now we won't allow for hot-plugging of
@ -197,6 +205,8 @@ RangeFinder::RangeFinder()
*/
void RangeFinder::init(enum Rotation orientation_default)
{
convert_params();
if (num_instances != 0) {
// don't re-init if we've found some sensors already
return;
@ -702,11 +712,6 @@ float RangeFinder::distance_orient(enum Rotation orientation) const
return backend->distance();
}
uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
{
return distance_orient(orientation) * 100.0;
}
int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const
{
AP_RangeFinder_Backend *backend = find_instance(orientation);
@ -716,31 +721,31 @@ int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const
return backend->signal_quality_pct();
}
int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
float RangeFinder::min_distance_orient(enum Rotation orientation) const
{
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
return 0; // consider NaN
}
return backend->max_distance_cm();
return backend->min_distance();
}
int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const
float RangeFinder::max_distance_orient(enum Rotation orientation) const
{
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
return 0; // consider NaN
}
return backend->min_distance_cm();
return backend->max_distance();
}
int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
float RangeFinder::ground_clearance_orient(enum Rotation orientation) const
{
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
return 0; // consider NaN
}
return backend->ground_clearance_cm();
return backend->ground_clearance();
}
bool RangeFinder::has_data_orient(enum Rotation orientation) const
@ -821,7 +826,7 @@ void RangeFinder::Log_RFND() const
LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
time_us : AP_HAL::micros64(),
instance : i,
dist : s->distance_cm(),
dist : s->distance(),
status : (uint8_t)s->status(),
orient : s->orientation(),
quality : s->signal_quality_pct(),

31
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -35,7 +35,7 @@
#endif
#endif
#define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10
#define RANGEFINDER_GROUND_CLEARANCE_DEFAULT 0.10
#define RANGEFINDER_PREARM_ALT_MAX_CM 200
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 0
@ -268,15 +268,30 @@ public:
uint8_t get_address(uint8_t id) const {
return id >= RANGEFINDER_MAX_INSTANCES? 0 : uint8_t(params[id].address.get());
}
// 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;
int8_t signal_quality_pct_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 ground_clearance_cm_orient(enum Rotation orientation) const;
#if AP_SCRIPTING_ENABLED
// centimetre accessors - do not use, reduce use where possible
uint16_t distance_cm_orient(enum Rotation orientation) const {
return distance_orient(orientation) * 100.0;
}
int32_t max_distance_cm_orient(enum Rotation orientation) const {
return max_distance_orient(orientation) * 100;
}
int32_t min_distance_cm_orient(enum Rotation orientation) const {
return min_distance_orient(orientation) * 100;
}
int32_t ground_clearance_cm_orient(enum Rotation orientation) const {
return ground_clearance_orient(orientation) * 100;
}
#endif
// metre accessors - use these in preference to the cm accessors
float distance_orient(enum Rotation orientation) const;
float max_distance_orient(enum Rotation orientation) const;
float min_distance_orient(enum Rotation orientation) const;
float ground_clearance_orient(enum Rotation orientation) const;
MAV_DISTANCE_SENSOR get_mav_distance_sensor_type_orient(enum Rotation orientation) const;
RangeFinder::Status status_orient(enum Rotation orientation) const;
bool has_data_orient(enum Rotation orientation) const;
@ -310,6 +325,8 @@ private:
float estimated_terrain_height;
Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests
void convert_params(void);
void detect_instance(uint8_t instance, uint8_t& serial_instance);
bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance, uint8_t serial_instance=0);

2
libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp
View File

@ -95,7 +95,7 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
has_data = false;
if (snr == 0) {
state.status = RangeFinder::Status::OutOfRangeHigh;
reading_m = MAX(656, max_distance_cm() * 0.01 + 1);
reading_m = MAX(656, max_distance() + 1);
} else {
state.status = RangeFinder::Status::NoData;
}

4
libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp
View File

@ -60,9 +60,9 @@ bool AP_RangeFinder_Backend::has_data() const {
void AP_RangeFinder_Backend::update_status(RangeFinder::RangeFinder_State &state_arg) const
{
// check distance
if (state_arg.distance_m > max_distance_cm() * 0.01f) {
if (state_arg.distance_m > max_distance()) {
set_status(state_arg, RangeFinder::Status::OutOfRangeHigh);
} else if (state_arg.distance_m < min_distance_cm() * 0.01f) {
} else if (state_arg.distance_m < min_distance()) {
set_status(state_arg, RangeFinder::Status::OutOfRangeLow);
} else {
set_status(state_arg, RangeFinder::Status::Good);

7
libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
View File

@ -53,12 +53,11 @@ public:
enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
float distance() const { return state.distance_m; }
uint16_t distance_cm() const { return state.distance_m*100.0f; }
int8_t signal_quality_pct() const WARN_IF_UNUSED { return state.signal_quality_pct; }
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; }
int16_t ground_clearance_cm() const { return params.ground_clearance_cm; }
virtual float max_distance() const { return params.max_distance; }
virtual float min_distance() const { return params.min_distance; }
float ground_clearance() const { return params.ground_clearance; }
MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const;
RangeFinder::Status status() const;
RangeFinder::Type type() const { return (RangeFinder::Type)params.type.get(); }

4
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h
View File

@ -141,8 +141,8 @@ private:
unsigned int _filtered_capture_size;
struct echo _echoes[RNFD_BEBOP_MAX_ECHOES];
unsigned int _filter_average = 4;
int16_t _last_max_distance_cm = 850;
int16_t _last_min_distance_cm = 32;
float _last_max_distance = 8.50;
float _last_min_distance = 0.32;
};

4
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal;
#define BENEWAKE_FRAME_HEADER 0x59
#define BENEWAKE_FRAME_LENGTH 9
#define BENEWAKE_DIST_MAX_CM 32768
#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
#define BENEWAKE_OUT_OF_RANGE_ADD 1.00 // metres
// format of serial packets received from benewake lidar
//
@ -129,7 +129,7 @@ bool AP_RangeFinder_Benewake::get_reading(float &reading_m)
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_m = MAX(model_dist_max_cm(), max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM) * 0.01f;
reading_m = MAX(model_dist_max_cm() * 0.01, max_distance() + BENEWAKE_OUT_OF_RANGE_ADD);
return true;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp
View File

@ -169,7 +169,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw
* value to 0." - force it to the max distance so status is set to OutOfRangeHigh
* rather than NoData.
*/
output_distance_cm = MAX(MAX_DIST_CM, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM);
output_distance_cm = MAX(MAX_DIST_CM, max_distance()*100 + BENEWAKE_OUT_OF_RANGE_ADD_CM);
} else {
output_distance_cm = constrain_int16(output_distance_cm, MIN_DIST_CM, MAX_DIST_CM);
}

6
libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp
View File

@ -25,8 +25,8 @@
#define FRAME_HEADER_2_B 'B' // 0x42
#define FRAME_HEADER_2_C 'C' // 0x43
#define DIST_MAX_CM 5000
#define OUT_OF_RANGE_ADD_CM 100
#define DIST_MAX 50.00
#define OUT_OF_RANGE_ADD 1.0 // metres
void AP_RangeFinder_JRE_Serial::move_preamble_in_buffer(uint8_t search_start_pos)
{
@ -134,7 +134,7 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
// all readings were invalid so return out-of-range-high value
if (invalid_count > 0) {
no_signal = true;
reading_m = MIN(MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
reading_m = MAX(DIST_MAX, max_distance() + OUT_OF_RANGE_ADD);
return true;
}

6
libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp
View File

@ -26,8 +26,8 @@ extern const AP_HAL::HAL& hal;
// https://autonomoustuff.com/product/leddartech-vu8/
#define LEDDARVU8_ADDR_DEFAULT 0x01 // modbus default device id
#define LEDDARVU8_DIST_MAX_CM 18500 // maximum possible distance reported by lidar
#define LEDDARVU8_OUT_OF_RANGE_ADD_CM 100 // add this many cm to out-of-range values
#define LEDDARVU8_DIST_MAX 185.00 // maximum possible distance reported by lidar
#define LEDDARVU8_OUT_OF_RANGE_ADD 1.00 // add this many metres to out-of-range values
#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
@ -78,7 +78,7 @@ bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m)
} else {
// if only out of range readings return larger of
// driver defined maximum range for the model and user defined max range + 1m
reading_m = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM)*0.01f;
reading_m = MAX(LEDDARVU8_DIST_MAX, max_distance() + LEDDARVU8_OUT_OF_RANGE_ADD);
}
return true;
}

12
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal;
#define LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG 23
#define LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE 20 // number of lost signal confirmations for legacy protocol only
#define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100
#define LIGHTWARE_OUT_OF_RANGE_ADD 1.00 // metres
static const size_t lx20_max_reply_len_bytes = 32;
static const size_t lx20_max_expected_stream_reply_len_bytes = 14;
@ -351,7 +351,7 @@ bool AP_RangeFinder_LightWareI2C::legacy_get_reading(float &reading_m)
int16_t signed_val = int16_t(be16toh(val));
if (signed_val < 0) {
// some lidar firmwares will return 65436 for out of range
reading_m = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM) * 0.01f;
reading_m = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD;
} else {
reading_m = uint16_t(signed_val) * 0.01f;
}
@ -381,7 +381,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m)
}
if (i==0) {
reading_m = sf20_stream_val[0] * 0.01f;
reading_m = sf20_stream_val[0];
}
// Increment the stream sequence
@ -400,7 +400,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m)
bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf,
size_t *p_num_processed_chars,
const char *string_identifier,
uint16_t &val)
float &val)
{
size_t string_identifier_len = strlen(string_identifier);
for (uint32_t i = 0 ; i < string_identifier_len ; i++) {
@ -416,7 +416,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf,
we will return max distance
*/
if (strncmp((const char *)&stream_buf[*p_num_processed_chars], "-1.00", 5) == 0) {
val = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
val = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD;
(*p_num_processed_chars) += 5;
return true;
}
@ -451,7 +451,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf,
}
accumulator *= final_multiplier;
val = accumulator;
val = accumulator * 0.01;
return number_found;
}

5
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h
View File

@ -32,7 +32,7 @@ protected:
private:
uint16_t sf20_stream_val[NUM_SF20_DATA_STREAMS];
float sf20_stream_val[NUM_SF20_DATA_STREAMS];
int currentStreamSequenceIndex = 0;
// constructor
@ -59,8 +59,7 @@ private:
bool sf20_parse_stream(uint8_t *stream_buf,
size_t *p_num_processed_chars,
const char *string_identifier,
uint16_t &val);
void data_log(uint16_t *val);
float &val);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
};

4
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -37,7 +37,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
uint16_t invalid_count = 0; // number of invalid readings
// max distance the sensor can reliably measure - read from parameters
const int16_t distance_cm_max = max_distance_cm();
const auto distance_cm_max = max_distance()*100;
// read any available lines from the lidar
for (auto i=0; i<8192; i++) {
@ -128,7 +128,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
// all readings were invalid so return out-of-range-high value
if (invalid_count > 0) {
reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
reading_m = MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
no_signal = true;
return true;
}

39
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -30,9 +30,9 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
// only accept distances for the configured orientation
if (packet.orientation == orientation()) {
state.last_reading_ms = AP_HAL::millis();
distance_cm = packet.current_distance;
_max_distance_cm = packet.max_distance;
_min_distance_cm = packet.min_distance;
distance = packet.current_distance * 0.01;
_max_distance = packet.max_distance * 0.01;
_min_distance = packet.min_distance * 0.01;
sensor_type = (MAV_DISTANCE_SENSOR)packet.type;
signal_quality = packet.signal_quality;
if (signal_quality == 0) {
@ -45,28 +45,31 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
}
}
int16_t AP_RangeFinder_MAVLink::max_distance_cm() const
float AP_RangeFinder_MAVLink::max_distance() const
{
if (_max_distance_cm == 0 && _min_distance_cm == 0) {
const auto baseclass_max_distance = AP_RangeFinder_Backend::max_distance();
if (is_zero(_max_distance) && is_zero(_min_distance)) {
// we assume if both of these are zero that we ignore both
return params.max_distance_cm;
return baseclass_max_distance;
}
if (params.max_distance_cm < _max_distance_cm) {
return params.max_distance_cm;
}
return _max_distance_cm;
// return the smaller of the base class's distance and what we
// receive from the network:
return MIN(baseclass_max_distance, _max_distance);
}
int16_t AP_RangeFinder_MAVLink::min_distance_cm() const
float AP_RangeFinder_MAVLink::min_distance() const
{
if (_max_distance_cm == 0 && _min_distance_cm == 0) {
const auto baseclass_min_distance = AP_RangeFinder_Backend::min_distance();
if (is_zero(_max_distance) && is_zero(_min_distance)) {
// we assume if both of these are zero that we ignore both
return params.min_distance_cm;
return baseclass_min_distance;
}
if (params.min_distance_cm > _min_distance_cm) {
return params.min_distance_cm;
}
return _min_distance_cm;
// return the smaller of the base class's distance and what we
// receive from the network:
return MIN(baseclass_min_distance, _min_distance);
}
/*
@ -81,7 +84,7 @@ void AP_RangeFinder_MAVLink::update(void)
state.distance_m = 0.0f;
state.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
} else {
state.distance_m = distance_cm * 0.01f;
state.distance_m = distance;
state.signal_quality_pct = signal_quality;
update_status();
}

10
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
View File

@ -28,8 +28,8 @@ public:
// Get update from mavlink
void handle_msg(const mavlink_message_t &msg) override;
int16_t max_distance_cm() const override;
int16_t min_distance_cm() const override;
float max_distance() const override;
float min_distance() const override;
protected:
@ -40,9 +40,9 @@ protected:
private:
// stored data from packet:
uint16_t distance_cm;
uint16_t _max_distance_cm;
uint16_t _min_distance_cm;
float distance;
float _max_distance;
float _min_distance;
int8_t signal_quality;
// start a reading

44
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
View File

@ -49,21 +49,21 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @User: Standard
AP_GROUPINFO("FUNCTION", 5, AP_RangeFinder_Params, function, 0),
// @Param: MIN_CM
// @Param: MIN
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @Description: Minimum distance in metres that rangefinder can reliably read
// @Units: m
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("MIN_CM", 6, AP_RangeFinder_Params, min_distance_cm, 20),
AP_GROUPINFO("MIN", 6, AP_RangeFinder_Params, min_distance, 0.20),
// @Param: MAX_CM
// @Param: MAX
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @Description: Maximum distance in metres that rangefinder can reliably read
// @Units: m
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("MAX_CM", 7, AP_RangeFinder_Params, max_distance_cm, 700),
AP_GROUPINFO("MAX", 7, AP_RangeFinder_Params, max_distance, 7.00),
// @Param: STOP_PIN
// @DisplayName: Rangefinder stop pin
@ -89,14 +89,14 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @User: Standard
AP_GROUPINFO("PWRRNG", 11, AP_RangeFinder_Params, powersave_range, 0),
// @Param: GNDCLEAR
// @DisplayName: Distance (in cm) from the range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground.
// @Units: cm
// @Range: 5 127
// @Increment: 1
// @Param: GNDCLR
// @DisplayName: Distance from the range finder to the ground
// @Description: This parameter sets the expected range measurement that the range finder should return when the vehicle is on the ground.
// @Units: m
// @Range: 0.05 1.5
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("GNDCLEAR", 12, AP_RangeFinder_Params, ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT),
AP_GROUPINFO("GNDCLR", 12, AP_RangeFinder_Params, ground_clearance, RANGEFINDER_GROUND_CLEARANCE_DEFAULT),
// @Param: ADDR
// @DisplayName: Bus address of sensor
@ -141,6 +141,16 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
AP_GROUPEND
};
// PARAMETER_CONVERSION - Added: Dec-2024 for 4.7
void AP_RangeFinder_Params::convert_min_max_params(void)
{
// ./Tools/autotest/test_param_upgrade.py --vehicle=arducopter --param "RNGFND1_MAX_CM=300->RNGFND1_MAX=3.00" --param "RNGFND2_MIN_CM=678->RNGFND2_MIN=6.78" --param "RNGFNDA_MIN_CM=1->RNGFNDA_MIN=0.01" --param "RNGFND5_GNDCLEAR=103->RNGFND5_GNDCLR=1.03"
max_distance.convert_parameter_width(AP_PARAM_INT16, 0.01);
min_distance.convert_parameter_width(AP_PARAM_INT16, 0.01);
ground_clearance.convert_parameter_width(AP_PARAM_INT8, 0.01);
}
AP_RangeFinder_Params::AP_RangeFinder_Params(void) {
AP_Param::setup_object_defaults(this, var_info);
}

7
libraries/AP_RangeFinder/AP_RangeFinder_Params.h
View File

@ -12,6 +12,7 @@ public:
static const struct AP_Param::GroupInfo var_info[];
AP_RangeFinder_Params(void);
void convert_min_max_params();
/* Do not allow copies */
CLASS_NO_COPY(AP_RangeFinder_Params);
@ -20,14 +21,14 @@ public:
AP_Float scaling;
AP_Float offset;
AP_Int16 powersave_range;
AP_Int16 min_distance_cm;
AP_Int16 max_distance_cm;
AP_Float min_distance;
AP_Float max_distance;
AP_Int8 type;
AP_Int8 pin;
AP_Int8 ratiometric;
AP_Int8 stop_pin;
AP_Int8 function;
AP_Int8 ground_clearance_cm;
AP_Float ground_clearance;
AP_Int8 address;
AP_Int8 orientation;
};

12
libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h
View File

@ -23,8 +23,8 @@
#include "AP_RangeFinder_Backend_Serial.h"
#define RDS02_BUFFER_SIZE 50
#define RDS02UF_DIST_MAX_CM 2000
#define RDS02UF_DIST_MIN_CM 150
#define RDS02UF_DIST_MAX 20.00
#define RDS02UF_DIST_MIN 1.50
#define RDS02UF_DATA_LEN 10
class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial
@ -57,8 +57,12 @@ private:
uint16_t read_timeout_ms() const override { return 500; }
// make sure readings go out-of-range when necessary
int16_t max_distance_cm()const override { return MIN(params.max_distance_cm, RDS02UF_DIST_MAX_CM); }
int16_t min_distance_cm() const override { return MAX(params.min_distance_cm, RDS02UF_DIST_MIN_CM); }
float max_distance() const override {
return MIN(AP_RangeFinder_Backend::max_distance(), RDS02UF_DIST_MAX);
}
float min_distance() const override {
return MAX(AP_RangeFinder_Backend::min_distance(), RDS02UF_DIST_MIN);
}
// Data Format for Benewake Rds02UF
// ===============================

4
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp
View File

@ -139,14 +139,14 @@ bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, ui
// Check for error codes
if (raw_distance == 0xFFFF) {
// Too far away
output_distance_cm = max_distance_cm() + TR_OUT_OF_RANGE_ADD_CM;
output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM;
} else if (raw_distance == 0x0000) {
// Too close
output_distance_cm = 0;
} else if (raw_distance == 0x0001) {
// Unable to measure
// This can also include the sensor pointing to the horizon when used as a proximity sensor
output_distance_cm = max_distance_cm() + TR_OUT_OF_RANGE_ADD_CM;
output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM;
} else {
output_distance_cm = raw_distance/10; // Conversion to centimeters
}

11
libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp
View File

@ -18,17 +18,14 @@
#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>
#include <ctype.h>
extern const AP_HAL::HAL& hal;
#define FRAME_HEADER 0x54
#define FRAME_LENGTH 5
#define DIST_MAX_CM 3000
#define OUT_OF_RANGE_ADD_CM 1000
#define DIST_MAX 30.00
#define OUT_OF_RANGE_ADD 10.00
#define STATUS_MASK 0x1F
#define DISTANCE_ERROR 0x0001
@ -77,7 +74,7 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m)
if (crc == linebuf[FRAME_LENGTH-1]) {
// calculate distance
uint16_t dist = ((uint16_t)linebuf[1] << 8) | linebuf[2];
if (dist >= DIST_MAX_CM *10) {
if (dist >= DIST_MAX *1000) {
// this reading is out of range and a bad read
bad_read++;
} else {
@ -107,7 +104,7 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m)
if (bad_read > 0) {
// if a bad read has occurred this update overwrite return with larger of
// driver defined maximum range for the model and user defined max range + 1m
reading_m = MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM) * 0.01f;
reading_m = MAX(DIST_MAX, max_distance() + OUT_OF_RANGE_ADD);
return true;
}

6
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -89,7 +89,7 @@ void AP_RangeFinder_analog::update(void)
float scaling = params.scaling;
float offset = params.offset;
RangeFinder::Function function = (RangeFinder::Function)params.function.get();
int16_t _max_distance_cm = params.max_distance_cm;
const float _max_distance = max_distance();
switch (function) {
case RangeFinder::Function::LINEAR:
@ -106,8 +106,8 @@ void AP_RangeFinder_analog::update(void)
} else {
dist_m = scaling / (v - offset);
}
if (dist_m > _max_distance_cm * 0.01f) {
dist_m = _max_distance_cm * 0.01f;
if (dist_m > _max_distance) {
dist_m = _max_distance;
}
break;
}

4
libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp
View File

@ -50,11 +50,11 @@ void loop()
if (!sensor->has_data()) {
continue;
}
hal.console->printf("All: device_%u type %d status %d distance_cm %d\n",
hal.console->printf("All: device_%u type=%d status=%d distance=%f\n",
i,
(int)sensor->type(),
(int)sensor->status(),
sensor->distance_cm());
sensor->distance());
had_data = true;
}
if (!had_data) {