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

RangeFinder: allow up to 10 range finders to be used at once

This commit is contained in:
Dmitri Ranfft 2018-07-04 16:22:17 +02:00 committed by Randy Mackay
parent 1fcd7fac06
commit 5eff01a86f
40 changed files with 540 additions and 643 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp
View File

@ -39,8 +39,8 @@ volatile struct range *rangerpru;
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_BBB_PRU::AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_state)
AP_RangeFinder_BBB_PRU::AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend(_state, _params)
{
}

2
libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h
View File

@ -21,7 +21,7 @@ class AP_RangeFinder_BBB_PRU : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state);
AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
// static detection function
static bool detect();

3
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -49,10 +49,11 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_Benewake::AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance,
benewake_model_type model) :
AP_RangeFinder_Backend(_state),
AP_RangeFinder_Backend(_state, _params),
model_type(model)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);

1
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
View File

@ -15,6 +15,7 @@ public:
// constructor
AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance,
benewake_model_type model);

3
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -25,9 +25,10 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state)
AP_RangeFinder_Backend(_state, _params)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {

1
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -43,6 +43,7 @@ class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend
public:
// constructor
AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance);

10
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -26,8 +26,8 @@ extern const AP_HAL::HAL& hal;
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_RangeFinder_Backend(_state)
AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_RangeFinder_Backend(_state, _params)
, _dev(std::move(dev)) {}
/*
@ -35,14 +35,14 @@ AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinde
trying to take a reading on I2C. If we get a result the sensor is
there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
if (!dev) {
return nullptr;
}
AP_RangeFinder_LightWareI2C *sensor
= new AP_RangeFinder_LightWareI2C(_state, std::move(dev));
= new AP_RangeFinder_LightWareI2C(_state, _params, std::move(dev));
if (!sensor) {
delete sensor;
@ -76,7 +76,7 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
{
be16_t val;
if (state.address == 0) {
if (params.address == 0) {
return false;
}

3
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h
View File

@ -10,6 +10,7 @@ class AP_RangeFinder_LightWareI2C : public AP_RangeFinder_Backend
public:
// static detection function
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
// update state
@ -23,7 +24,7 @@ protected:
private:
// constructor
AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
void init();
void timer();

3
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -26,9 +26,10 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state)
AP_RangeFinder_Backend(_state, _params)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {

1
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
View File

@ -9,6 +9,7 @@ class AP_RangeFinder_LightWareSerial : public AP_RangeFinder_Backend
public:
// constructor
AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance);

4
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -25,8 +25,8 @@ extern const AP_HAL::HAL& hal;
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_state)
AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend(_state, _params)
{
state.last_reading_ms = AP_HAL::millis();
distance_cm = 0;

2
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
View File

@ -11,7 +11,7 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend
public:
// constructor
AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state);
AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
// static detection function
static bool detect();

6
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@ -37,8 +37,9 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_RangeFinder_Backend(_state)
: AP_RangeFinder_Backend(_state, _params)
, _dev(std::move(dev))
{
}
@ -49,6 +50,7 @@ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFin
there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
if (!dev) {
@ -56,7 +58,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeF
}
AP_RangeFinder_MaxsonarI2CXL *sensor
= new AP_RangeFinder_MaxsonarI2CXL(_state, std::move(dev));
= new AP_RangeFinder_MaxsonarI2CXL(_state, _params, std::move(dev));
if (!sensor) {
return nullptr;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h
View File

@ -12,6 +12,7 @@ class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend
public:
// static detection function
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
// update state
@ -26,6 +27,7 @@ protected:
private:
// constructor
AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
bool _init(void);

3
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp
View File

@ -30,9 +30,10 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state)
AP_RangeFinder_Backend(_state, _params)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {

1
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
View File

@ -9,6 +9,7 @@ class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend
public:
// constructor
AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance);

3
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp
View File

@ -25,9 +25,10 @@ extern const AP_HAL::HAL& hal;
// Note this is called after detect() returns true, so we
// already know that we should setup the rangefinder
AP_RangeFinder_NMEA::AP_RangeFinder_NMEA(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state),
AP_RangeFinder_Backend(_state, _params),
_distance_m(-1.0f)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);

1
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h
View File

@ -24,6 +24,7 @@ class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend
public:
// constructor
AP_RangeFinder_NMEA(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance);

33
libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp
View File

@ -24,10 +24,9 @@ extern const AP_HAL::HAL& hal;
The constructor also initialises the rangefinder.
*/
AP_RangeFinder_PWM::AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state,
AP_Int16 &_powersave_range,
AP_RangeFinder_Params &_params,
float &_estimated_terrain_height) :
AP_RangeFinder_Backend(_state),
powersave_range(_powersave_range),
AP_RangeFinder_Backend(_state, _params),
estimated_terrain_height(_estimated_terrain_height)
{
}
@ -74,7 +73,7 @@ bool AP_RangeFinder_PWM::get_reading(uint16_t &reading_cm)
void AP_RangeFinder_PWM::check_pin()
{
if (state.pin == last_pin) {
if (params.pin == last_pin) {
return;
}
@ -88,19 +87,19 @@ void AP_RangeFinder_PWM::check_pin()
}
}
// set last pin to state.pin so we don't continually try to attach
// set last pin to params.pin so we don't continually try to attach
// to it if the attach is failing
last_pin = state.pin;
last_pin = params.pin;
if (state.pin <= 0) {
if (params.pin <= 0) {
// don't need to install handler
return;
}
// install interrupt handler on rising and falling edge
hal.gpio->pinMode(state.pin, HAL_GPIO_INPUT);
hal.gpio->pinMode(params.pin, HAL_GPIO_INPUT);
if (!hal.gpio->attach_interrupt(
state.pin,
params.pin,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PWM::irq_handler,
void,
uint8_t,
@ -110,20 +109,20 @@ void AP_RangeFinder_PWM::check_pin()
// failed to attach interrupt
gcs().send_text(MAV_SEVERITY_WARNING,
"RangeFinder_PWM: Failed to attach to pin %u",
state.pin);
params.pin);
return;
}
}
void AP_RangeFinder_PWM::check_stop_pin()
{
if (state.stop_pin == last_stop_pin) {
if (params.stop_pin == last_stop_pin) {
return;
}
hal.gpio->pinMode(state.stop_pin, HAL_GPIO_OUTPUT);
hal.gpio->pinMode(params.stop_pin, HAL_GPIO_OUTPUT);
last_stop_pin = state.stop_pin;
last_stop_pin = params.stop_pin;
}
void AP_RangeFinder_PWM::check_pins()
@ -146,12 +145,12 @@ void AP_RangeFinder_PWM::update(void)
return;
}
if (state.stop_pin != -1) {
if (params.stop_pin != -1) {
const bool oor = out_of_range();
if (oor) {
if (!was_out_of_range) {
// we are above the power saving range. Disable the sensor
hal.gpio->write(state.stop_pin, false);
hal.gpio->write(params.stop_pin, false);
set_status(RangeFinder::RangeFinder_NoData);
state.distance_cm = 0;
state.voltage_mv = 0;
@ -161,7 +160,7 @@ void AP_RangeFinder_PWM::update(void)
}
// re-enable the sensor:
if (!oor && was_out_of_range) {
hal.gpio->write(state.stop_pin, true);
hal.gpio->write(params.stop_pin, true);
was_out_of_range = oor;
}
}
@ -182,5 +181,5 @@ void AP_RangeFinder_PWM::update(void)
// return true if we are beyond the power saving range
bool AP_RangeFinder_PWM::out_of_range(void) const {
return powersave_range > 0 && estimated_terrain_height > powersave_range;
return params.powersave_range > 0 && estimated_terrain_height > params.powersave_range;
}

3
libraries/AP_RangeFinder/AP_RangeFinder_PWM.h
View File

@ -22,7 +22,7 @@ class AP_RangeFinder_PWM : public AP_RangeFinder_Backend
public:
// constructor
AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state,
AP_Int16 &_powersave_range,
AP_RangeFinder_Params &_params,
float &_estimated_terrain_height);
// destructor
@ -58,7 +58,6 @@ private:
void check_pins();
uint8_t last_stop_pin = -1;
AP_Int16 &powersave_range;
float &estimated_terrain_height;
// return true if we are beyond the power saving range

139
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp Normal file
View File

@ -0,0 +1,139 @@
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "AP_RangeFinder_Params.h"
#include "AP_RangeFinder.h"
// table of user settable parameters
const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: TYPE
// @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini
// @User: Standard
AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0),
// @Param: PIN
// @DisplayName: Rangefinder pin
// @Description: Analog pin that rangefinder is connected to. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
// @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
// @User: Standard
AP_GROUPINFO("PIN", 2, AP_RangeFinder_Params, pin, -1),
// @Param: SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
// @Units: m/V
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("SCALING", 3, AP_RangeFinder_Params, scaling, 3.0f),
// @Param: OFFSET
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance for analog rangefinders. Offset added to distance in centimeters for PWM and I2C Lidars
// @Units: V
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("OFFSET", 4, AP_RangeFinder_Params, offset, 0.0f),
// @Param: FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
// @User: Standard
AP_GROUPINFO("FUNCTION", 5, AP_RangeFinder_Params, function, 0),
// @Param: MIN_CM
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MIN_CM", 6, AP_RangeFinder_Params, min_distance_cm, 20),
// @Param: MAX_CM
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MAX_CM", 7, AP_RangeFinder_Params, max_distance_cm, 700),
// @Param: STOP_PIN
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
// @User: Standard
AP_GROUPINFO("STOP_PIN", 8, AP_RangeFinder_Params, stop_pin, -1),
// @Param: SETTLE
// @DisplayName: Rangefinder settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
// @Units: ms
// @Increment: 1
// @User: Standard
AP_GROUPINFO("SETTLE", 9, AP_RangeFinder_Params, settle_time_ms, 0),
// @Param: RMETRIC
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
// @User: Standard
AP_GROUPINFO("RMETRIC", 10, AP_RangeFinder_Params, ratiometric, 1),
// @Param: PWRRNG
// @DisplayName: Powersave range
// @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled
// @Units: m
// @Range: 0 32767
// @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
// @User: Standard
AP_GROUPINFO("GNDCLEAR", 12, AP_RangeFinder_Params, ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT),
// @Param: ADDR
// @DisplayName: Bus address of sensor
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ADDR", 23, AP_RangeFinder_Params, address, 0),
// @Param: POS_X
// @DisplayName: X position offset
// @Description: X position of the first rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
// @Units: m
// @User: Advanced
// @Param: POS_Y
// @DisplayName: Y position offset
// @Description: Y position of the first rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
// @Units: m
// @User: Advanced
// @Param: POS_Z
// @DisplayName: Z position offset
// @Description: Z position of the first rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
// @Units: m
// @User: Advanced
AP_GROUPINFO("POS", 49, AP_RangeFinder_Params, pos_offset, 0.0f),
// @Param: ORIENT
// @DisplayName: Rangefinder orientation
// @Description: Orientation of rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("ORIENT", 53, AP_RangeFinder_Params, orientation, ROTATION_PITCH_270),
AP_GROUPEND
};
AP_RangeFinder_Params::AP_RangeFinder_Params(void) {
AP_Param::setup_object_defaults(this, var_info);
}

31
libraries/AP_RangeFinder/AP_RangeFinder_Params.h Normal file
View File

@ -0,0 +1,31 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
class AP_RangeFinder_Params {
public:
static const struct AP_Param::GroupInfo var_info[];
AP_RangeFinder_Params(void);
/* Do not allow copies */
AP_RangeFinder_Params(const AP_RangeFinder_Params &other) = delete;
AP_RangeFinder_Params &operator=(const AP_RangeFinder_Params&) = delete;
AP_Int8 type;
AP_Int8 pin;
AP_Int8 ratiometric;
AP_Int16 powersave_range;
AP_Int8 stop_pin;
AP_Int16 settle_time_ms;
AP_Float scaling;
AP_Float offset;
AP_Int8 function;
AP_Int16 min_distance_cm;
AP_Int16 max_distance_cm;
AP_Int8 ground_clearance_cm;
AP_Int8 address;
AP_Vector3f pos_offset; // position offset in body frame
AP_Int8 orientation;
};

6
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -47,8 +47,9 @@ extern const AP_HAL::HAL& hal;
*/
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus,
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
RangeFinder::RangeFinder_Type _rftype)
: AP_RangeFinder_Backend(_state)
: AP_RangeFinder_Backend(_state, _params)
, _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
, rftype(_rftype)
{
@ -60,10 +61,11 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus,
*/
AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus,
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
RangeFinder::RangeFinder_Type rftype)
{
AP_RangeFinder_PulsedLightLRF *sensor
= new AP_RangeFinder_PulsedLightLRF(bus, _state, rftype);
= new AP_RangeFinder_PulsedLightLRF(bus, _state, _params, rftype);
if (!sensor ||
!sensor->init()) {
delete sensor;

2
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
View File

@ -21,6 +21,7 @@ public:
// static detection function
static AP_RangeFinder_Backend *detect(uint8_t bus,
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
RangeFinder::RangeFinder_Type rftype);
// update state
@ -36,6 +37,7 @@ private:
// constructor
AP_RangeFinder_PulsedLightLRF(uint8_t bus,
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
RangeFinder::RangeFinder_Type rftype);
// start a reading

8
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp
View File

@ -35,8 +35,9 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev)
: AP_RangeFinder_Backend(_state)
: AP_RangeFinder_Backend(_state, _params)
, dev(std::move(i2c_dev))
{
}
@ -47,13 +48,14 @@ AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFin
there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_TeraRangerI2C::detect(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev)
{
if (!i2c_dev) {
return nullptr;
}
AP_RangeFinder_TeraRangerI2C *sensor = new AP_RangeFinder_TeraRangerI2C(_state, std::move(i2c_dev));
AP_RangeFinder_TeraRangerI2C *sensor = new AP_RangeFinder_TeraRangerI2C(_state, _params, std::move(i2c_dev));
if (!sensor) {
return nullptr;
}
@ -144,7 +146,7 @@ bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, ui
return false;
} else if (raw_distance == 0x0000) {
// Too close
output_distance_cm = state.min_distance_cm;
output_distance_cm = params.min_distance_cm;
return true;
} else if (raw_distance == 0x0001) {
// Unable to measure

2
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h
View File

@ -9,6 +9,7 @@ class AP_RangeFinder_TeraRangerI2C : public AP_RangeFinder_Backend
public:
// static detection function
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev);
// update state
@ -23,6 +24,7 @@ protected:
private:
// constructor
AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev);
bool measure(void);

8
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp
View File

@ -216,8 +216,8 @@ const AP_RangeFinder_VL53L0X::RegData AP_RangeFinder_VL53L0X::tuning_data[] =
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev)
: AP_RangeFinder_Backend(_state)
AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev)
: AP_RangeFinder_Backend(_state, _params)
, dev(std::move(_dev)) {}
@ -226,13 +226,13 @@ AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_
trying to take a reading on I2C. If we get a result the sensor is
there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
if(!dev){
return nullptr;
}
AP_RangeFinder_VL53L0X *sensor
= new AP_RangeFinder_VL53L0X(_state, std::move(dev));
= new AP_RangeFinder_VL53L0X(_state, _params, std::move(dev));
if (!sensor) {
delete sensor;

4
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h
View File

@ -9,7 +9,7 @@ class AP_RangeFinder_VL53L0X : public AP_RangeFinder_Backend
public:
// static detection function
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
// update state
void update(void) override;
@ -22,7 +22,7 @@ protected:
private:
// constructor
AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
bool init();
void timer();

8
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
View File

@ -34,8 +34,8 @@ extern const AP_HAL::HAL& hal;
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev)
: AP_RangeFinder_Backend(_state)
AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev)
: AP_RangeFinder_Backend(_state, _params)
, dev(std::move(_dev)) {}
/*
@ -43,14 +43,14 @@ AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_
trying to take a reading on I2C. If we get a result the sensor is
there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
if (!dev) {
return nullptr;
}
AP_RangeFinder_VL53L1X *sensor
= new AP_RangeFinder_VL53L1X(_state, std::move(dev));
= new AP_RangeFinder_VL53L1X(_state, _params, std::move(dev));
if (!sensor) {
delete sensor;

4
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
View File

@ -9,7 +9,7 @@ class AP_RangeFinder_VL53L1X : public AP_RangeFinder_Backend
public:
// static detection function
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev);
// update state
void update(void) override;
@ -1213,7 +1213,7 @@ private:
};
// constructor
AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
bool init();
void timer();

3
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp
View File

@ -67,9 +67,10 @@ const AP_Param::GroupInfo AP_RangeFinder_Wasp::var_info[] = {
};
AP_RangeFinder_Wasp::AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state) {
AP_RangeFinder_Backend(_state, _params) {
AP_Param::setup_object_defaults(this, var_info);
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);

1
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h
View File

@ -10,6 +10,7 @@ class AP_RangeFinder_Wasp : public AP_RangeFinder_Backend {
public:
AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance);

31
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -22,6 +22,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include "RangeFinder.h"
#include "AP_RangeFinder_Params.h"
#include "AP_RangeFinder_analog.h"
extern const AP_HAL::HAL& hal;
@ -31,17 +32,17 @@ extern const AP_HAL::HAL& hal;
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_state)
AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend(_state, _params)
{
source = hal.analogin->channel(_state.pin);
source = hal.analogin->channel(_params.pin);
if (source == nullptr) {
// failed to allocate a ADC channel? This shouldn't happen
set_status(RangeFinder::RangeFinder_NotConnected);
return;
}
source->set_stop_pin((uint8_t)_state.stop_pin);
source->set_settle_time((uint16_t)_state.settle_time_ms);
source->set_stop_pin((uint8_t)_params.stop_pin);
source->set_settle_time((uint16_t)_params.settle_time_ms);
set_status(RangeFinder::RangeFinder_NoData);
}
@ -50,9 +51,9 @@ AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_st
can do is check if the pin number is valid. If it is, then assume
that the device is connected
*/
bool AP_RangeFinder_analog::detect(RangeFinder::RangeFinder_State &_state)
bool AP_RangeFinder_analog::detect(AP_RangeFinder_Params &_params)
{
if (_state.pin != -1) {
if (_params.pin != -1) {
return true;
}
return false;
@ -69,10 +70,10 @@ void AP_RangeFinder_analog::update_voltage(void)
return;
}
// cope with changed settings
source->set_pin(state.pin);
source->set_stop_pin((uint8_t)state.stop_pin);
source->set_settle_time((uint16_t)state.settle_time_ms);
if (state.ratiometric) {
source->set_pin(params.pin);
source->set_stop_pin((uint8_t)params.stop_pin);
source->set_settle_time((uint16_t)params.settle_time_ms);
if (params.ratiometric) {
state.voltage_mv = source->voltage_average_ratiometric() * 1000U;
} else {
state.voltage_mv = source->voltage_average() * 1000U;
@ -87,10 +88,10 @@ void AP_RangeFinder_analog::update(void)
update_voltage();
float v = state.voltage_mv * 0.001f;
float dist_m = 0;
float scaling = state.scaling;
float offset = state.offset;
RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)state.function.get();
int16_t _max_distance_cm = state.max_distance_cm;
float scaling = params.scaling;
float offset = params.offset;
RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)params.function.get();
int16_t _max_distance_cm = params.max_distance_cm;
switch (function) {
case RangeFinder::FUNCTION_LINEAR:

5
libraries/AP_RangeFinder/AP_RangeFinder_analog.h
View File

@ -2,15 +2,16 @@
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#include "AP_RangeFinder_Params.h"
class AP_RangeFinder_analog : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state);
AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
// static detection function
static bool detect(RangeFinder::RangeFinder_State &_state);
static bool detect(AP_RangeFinder_Params &_params);
// update state
void update(void) override;

3
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp
View File

@ -33,9 +33,10 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state)
AP_RangeFinder_Backend(_state, _params)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {

1
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h
View File

@ -9,6 +9,7 @@ class AP_RangeFinder_uLanding : public AP_RangeFinder_Backend
public:
// constructor
AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance);

782
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -37,527 +37,112 @@
#include "AP_RangeFinder_Benewake.h"
#include "AP_RangeFinder_PWM.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <stdio.h>
extern const AP_HAL::HAL &hal;
// table of user settable parameters
const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: _TYPE
// @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
// @User: Standard
AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0),
// @Param: _PIN
// @DisplayName: Rangefinder pin
// @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
// @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 50:GPIO1,51:GPIO2,52:GPIO3,53:GPIO4,54:GPIO5,55:GPIO6
// @User: Standard
AP_GROUPINFO("_PIN", 1, RangeFinder, state[0].pin, -1),
// @Group: 1_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO_FLAGS(params[0], "1_", 25, RangeFinder, AP_RangeFinder_Params, AP_PARAM_FLAG_IGNORE_ENABLE),
// @Param: _SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
// @Units: m/V
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("_SCALING", 2, RangeFinder, state[0].scaling, 3.0f),
// @Param: _OFFSET
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance for analog rangefinders. Offset added to distance in centimeters for PWM and I2C Lidars
// @Units: V
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("_OFFSET", 3, RangeFinder, state[0].offset, 0.0f),
// @Param: _FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
// @User: Standard
AP_GROUPINFO("_FUNCTION", 4, RangeFinder, state[0].function, 0),
// @Param: _MIN_CM
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_MIN_CM", 5, RangeFinder, state[0].min_distance_cm, 20),
// @Param: _MAX_CM
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_MAX_CM", 6, RangeFinder, state[0].max_distance_cm, 700),
// @Param: _STOP_PIN
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
// @User: Standard
AP_GROUPINFO("_STOP_PIN", 7, RangeFinder, state[0].stop_pin, -1),
// @Param: _SETTLE
// @DisplayName: Rangefinder settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
// @Units: ms
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_SETTLE", 8, RangeFinder, state[0].settle_time_ms, 0),
// @Param: _RMETRIC
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
// @User: Standard
AP_GROUPINFO("_RMETRIC", 9, RangeFinder, state[0].ratiometric, 1),
// @Param: _PWRRNG
// @DisplayName: Powersave range
// @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled
// @Units: m
// @Range: 0 32767
// @User: Standard
AP_GROUPINFO("_PWRRNG", 10, RangeFinder, _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
// @User: Standard
AP_GROUPINFO("_GNDCLEAR", 11, RangeFinder, state[0].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT),
// @Param: _ADDR
// @DisplayName: Bus address of sensor
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_ADDR", 23, RangeFinder, state[0].address, 0),
// @Param: _POS_X
// @DisplayName: X position offset
// @Description: X position of the first rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: _POS_Y
// @DisplayName: Y position offset
// @Description: Y position of the first rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: _POS_Z
// @DisplayName: Z position offset
// @Description: Z position of the first rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("_POS", 49, RangeFinder, state[0].pos_offset, 0.0f),
// @Param: _ORIENT
// @DisplayName: Rangefinder orientation
// @Description: Orientation of rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("_ORIENT", 53, RangeFinder, state[0].orientation, ROTATION_PITCH_270),
// @Group: _
// @Group: 1_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[0], "_", 57, RangeFinder, backend_var_info[0]),
AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]),
#if RANGEFINDER_MAX_INSTANCES > 1
// @Param: 2_TYPE
// @DisplayName: Second Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
// @User: Advanced
AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0),
// @Param: 2_PIN
// @DisplayName: Rangefinder pin
// @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
// @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
// @User: Advanced
AP_GROUPINFO("2_PIN", 13, RangeFinder, state[1].pin, -1),
// @Param: 2_SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
// @Units: m/V
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("2_SCALING", 14, RangeFinder, state[1].scaling, 3.0f),
// @Param: 2_OFFSET
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance
// @Units: V
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("2_OFFSET", 15, RangeFinder, state[1].offset, 0.0f),
// @Param: 2_FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
// @User: Advanced
AP_GROUPINFO("2_FUNCTION", 16, RangeFinder, state[1].function, 0),
// @Param: 2_MIN_CM
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2_MIN_CM", 17, RangeFinder, state[1].min_distance_cm, 20),
// @Param: 2_MAX_CM
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2_MAX_CM", 18, RangeFinder, state[1].max_distance_cm, 700),
// @Param: 2_STOP_PIN
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
// @User: Advanced
AP_GROUPINFO("2_STOP_PIN", 19, RangeFinder, state[1].stop_pin, -1),
// @Param: 2_SETTLE
// @DisplayName: Sonar settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
// @Units: ms
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2_SETTLE", 20, RangeFinder, state[1].settle_time_ms, 0),
// @Param: 2_RMETRIC
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
// @User: Advanced
AP_GROUPINFO("2_RMETRIC", 21, RangeFinder, state[1].ratiometric, 1),
// @Param: 2_GNDCLEAR
// @DisplayName: Distance (in cm) from the second range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the second range finder should return when the vehicle is on the ground.
// @Units: cm
// @Range: 0 127
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2_GNDCLEAR", 22, RangeFinder, state[1].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT),
// @Param: 2_ADDR
// @DisplayName: Bus address of second rangefinder
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("2_ADDR", 24, RangeFinder, state[1].address, 0),
// @Param: 2_POS_X
// @DisplayName: X position offset
// @Description: X position of the second rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: 2_POS_Y
// @DisplayName: Y position offset
// @Description: Y position of the second rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: 2_POS_Z
// @DisplayName: Z position offset
// @Description: Z position of the second rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("2_POS", 50, RangeFinder, state[1].pos_offset, 0.0f),
// @Param: 2_ORIENT
// @DisplayName: Rangefinder 2 orientation
// @Description: Orientation of 2nd rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("2_ORIENT", 54, RangeFinder, state[1].orientation, ROTATION_PITCH_270),
// @Group: 2_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[1], "2_", 27, RangeFinder, AP_RangeFinder_Params),
// @Group: 2_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]),
AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 2
// @Param: 3_TYPE
// @DisplayName: Third Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
// @User: Advanced
AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0),
// @Param: 3_PIN
// @DisplayName: Rangefinder pin
// @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
// @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
// @User: Advanced
AP_GROUPINFO("3_PIN", 26, RangeFinder, state[2].pin, -1),
// @Param: 3_SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
// @Units: m/V
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("3_SCALING", 27, RangeFinder, state[2].scaling, 3.0f),
// @Param: 3_OFFSET
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance
// @Units: V
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("3_OFFSET", 28, RangeFinder, state[2].offset, 0.0f),
// @Param: 3_FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
// @User: Advanced
AP_GROUPINFO("3_FUNCTION", 29, RangeFinder, state[2].function, 0),
// @Param: 3_MIN_CM
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("3_MIN_CM", 30, RangeFinder, state[2].min_distance_cm, 20),
// @Param: 3_MAX_CM
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("3_MAX_CM", 31, RangeFinder, state[2].max_distance_cm, 700),
// @Param: 3_STOP_PIN
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
// @User: Advanced
AP_GROUPINFO("3_STOP_PIN", 32, RangeFinder, state[2].stop_pin, -1),
// @Param: 3_SETTLE
// @DisplayName: Sonar settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
// @Units: ms
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("3_SETTLE", 33, RangeFinder, state[2].settle_time_ms, 0),
// @Param: 3_RMETRIC
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
// @User: Advanced
AP_GROUPINFO("3_RMETRIC", 34, RangeFinder, state[2].ratiometric, 1),
// @Param: 3_GNDCLEAR
// @DisplayName: Distance (in cm) from the third range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the third range finder should return when the vehicle is on the ground.
// @Units: cm
// @Range: 0 127
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("3_GNDCLEAR", 35, RangeFinder, state[2].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT),
// @Param: 3_ADDR
// @DisplayName: Bus address of third rangefinder
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("3_ADDR", 36, RangeFinder, state[2].address, 0),
// @Param: 3_POS_X
// @DisplayName: X position offset
// @Description: X position of the third rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: 3_POS_Y
// @DisplayName: Y position offset
// @Description: Y position of the third rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: 3_POS_Z
// @DisplayName: Z position offset
// @Description: Z position of the third rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("3_POS", 51, RangeFinder, state[2].pos_offset, 0.0f),
// @Param: 3_ORIENT
// @DisplayName: Rangefinder 3 orientation
// @Description: Orientation of 3rd rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("3_ORIENT", 55, RangeFinder, state[2].orientation, ROTATION_PITCH_270),
// @Group: 3_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[2], "3_", 29, RangeFinder, AP_RangeFinder_Params),
// @Group: 3_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]),
AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 3
// @Param: 4_TYPE
// @DisplayName: Fourth Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
// @User: Advanced
AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0),
// @Param: 4_PIN
// @DisplayName: Rangefinder pin
// @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
// @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
// @User: Advanced
AP_GROUPINFO("4_PIN", 38, RangeFinder, state[3].pin, -1),
// @Param: 4_SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
// @Units: m/V
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("4_SCALING", 39, RangeFinder, state[3].scaling, 3.0f),
// @Param: 4_OFFSET
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance
// @Units: V
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("4_OFFSET", 40, RangeFinder, state[3].offset, 0.0f),
// @Param: 4_FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
// @User: Advanced
AP_GROUPINFO("4_FUNCTION", 41, RangeFinder, state[3].function, 0),
// @Param: 4_MIN_CM
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("4_MIN_CM", 42, RangeFinder, state[3].min_distance_cm, 20),
// @Param: 4_MAX_CM
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("4_MAX_CM", 43, RangeFinder, state[3].max_distance_cm, 700),
// @Param: 4_STOP_PIN
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
// @User: Advanced
AP_GROUPINFO("4_STOP_PIN", 44, RangeFinder, state[3].stop_pin, -1),
// @Param: 4_SETTLE
// @DisplayName: Sonar settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
// @Units: ms
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("4_SETTLE", 45, RangeFinder, state[3].settle_time_ms, 0),
// @Param: 4_RMETRIC
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
// @User: Advanced
AP_GROUPINFO("4_RMETRIC", 46, RangeFinder, state[3].ratiometric, 1),
// @Param: 4_GNDCLEAR
// @DisplayName: Distance (in cm) from the fourth range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the fourth range finder should return when the vehicle is on the ground.
// @Units: cm
// @Range: 0 127
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("4_GNDCLEAR", 47, RangeFinder, state[3].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT),
// @Param: 4_ADDR
// @DisplayName: Bus address of fourth rangefinder
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("4_ADDR", 48, RangeFinder, state[3].address, 0),
// @Param: 4_POS_X
// @DisplayName: X position offset
// @Description: X position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: 4_POS_Y
// @DisplayName: Y position offset
// @Description: Y position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
// @Param: 4_POS_Z
// @DisplayName: Z position offset
// @Description: Z position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
// @Units: m
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("4_POS", 52, RangeFinder, state[3].pos_offset, 0.0f),
// @Param: 4_ORIENT
// @DisplayName: Rangefinder 4 orientation
// @Description: Orientation of 4th range finder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("4_ORIENT", 56, RangeFinder, state[3].orientation, ROTATION_PITCH_270),
// @Group: 4_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[3], "4_", 31, RangeFinder, AP_RangeFinder_Params),
// @Group: 4_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]),
AP_SUBGROUPVARPTR(drivers[0], "4_", 60, RangeFinder, backend_var_info[3]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 4
// @Group: 5_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[4], "5_", 33, RangeFinder, AP_RangeFinder_Params),
// @Group: 5_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 5
// @Group: 6_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[5], "6_", 35, RangeFinder, AP_RangeFinder_Params),
// @Group: 6_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 6
// @Group: 7_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[6], "7_", 37, RangeFinder, AP_RangeFinder_Params),
// @Group: 7_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 7
// @Group: 8_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[7], "8_", 39, RangeFinder, AP_RangeFinder_Params),
// @Group: 8_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 8
// @Group: 9_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[8], "9_", 41, RangeFinder, AP_RangeFinder_Params),
// @Group: 9_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 9
// @Group: A_
// @Path: AP_RangeFinder_Params.cpp
AP_SUBGROUPINFO(params[9], "A_", 43, RangeFinder, AP_RangeFinder_Params),
// @Group: A_
// @Path: AP_RangeFinder_Wasp.cpp
AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]),
#endif
AP_GROUPEND
};
@ -570,7 +155,7 @@ RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orient
// set orientation defaults
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
state[i].orientation.set_default(orientation_default);
params[i].orientation.set_default(orientation_default);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -581,6 +166,126 @@ RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orient
_singleton = this;
}
void RangeFinder::convert_params(void) {
if (params[0].type.configured_in_storage()) {
// _params[0]._type will always be configured in storage after conversion is done the first time
return;
}
struct ConversionTable {
uint8_t old_element;
uint8_t new_index;
uint8_t instance;
};
const struct ConversionTable conversionTable[] = {
{0, 0, 0}, //0, TYPE 1
{1, 1, 0}, //1, PIN 1
{2, 2, 0}, //2, SCALING 1
{3, 3, 0}, //3, OFFSET 1
{4, 4, 0}, //4, FUNCTION 1
{5, 5, 0}, //5, MIN_CM 1
{6, 6, 0}, //6, MAX_CM 1
{7, 7, 0}, //7, STOP_PIN 1
{8, 8, 0}, //8, SETTLE 1
{9, 9, 0}, //9, RMETRIC 1
{10, 10, 0}, //10, PWRRNG 1 (previously existed only once for all sensors)
{11, 11, 0}, //11, GNDCLEAR 1
{23, 12, 0}, //23, ADDR 1
{49, 13, 0}, //49, POS 1
{53, 14, 0}, //53, ORIENT 1
//{57, 1, 0}, //57, backend 1
{12, 0, 1}, //12, TYPE 2
{13, 1, 1}, //13, PIN 2
{14, 2, 1}, //14, SCALING 2
{15, 3, 1}, //15, OFFSET 2
{16, 4, 1}, //16, FUNCTION 2
{17, 5, 1}, //17, MIN_CM 2
{18, 6, 1}, //18, MAX_CM 2
{19, 7, 1}, //19, STOP_PIN 2
{20, 8, 1}, //20, SETTLE 2
{21, 9, 1}, //21, RMETRIC 2
//{, 10, 1}, //PWRRNG 2 (previously existed only once for all sensors)
{22, 11, 1}, //22, GNDCLEAR 2
{24, 12, 1}, //24, ADDR 2
{50, 13, 1}, //50, POS 2
{54, 14, 1}, //54, ORIENT 2
//{58, 3, 1}, //58, backend 2
{25, 0, 2}, //25, TYPE 3
{26, 1, 2}, //26, PIN 3
{27, 2, 2}, //27, SCALING 3
{28, 3, 2}, //28, OFFSET 3
{29, 4, 2}, //29, FUNCTION 3
{30, 5, 2}, //30, MIN_CM 3
{31, 6, 2}, //31, MAX_CM 3
{32, 7, 2}, //32, STOP_PIN 3
{33, 8, 2}, //33, SETTLE 3
{34, 9, 2}, //34, RMETRIC 3
//{, 10, 2}, //PWRRNG 3 (previously existed only once for all sensors)
{35, 11, 2}, //35, GNDCLEAR 3
{36, 12, 2}, //36, ADDR 3
{51, 13, 2}, //51, POS 3
{55, 14, 2}, //55, ORIENT 3
//{59, 5, 2}, //59, backend 3
{37, 0, 3}, //37, TYPE 4
{38, 1, 3}, //38, PIN 4
{39, 2, 3}, //39, SCALING 4
{40, 3, 3}, //40, OFFSET 4
{41, 4, 3}, //41, FUNCTION 4
{42, 5, 3}, //42, MIN_CM 4
{43, 6, 3}, //43, MAX_CM 4
{44, 7, 3}, //44, STOP_PIN 4
{45, 8, 3}, //45, SETTLE 4
{46, 9, 3}, //46, RMETRIC 4
//{, 10, 3}, //PWRRNG 4 (previously existed only once for all sensors)
{47, 11, 3}, //47, GNDCLEAR 4
{48, 12, 3}, //48, ADDR 4
{52, 13, 3}, //52, POS 4
{56, 14, 3}, //56, ORIENT 4
//{60, 7, 3}, //60, backend 4
};
char param_name[17] = {0};
AP_Param::ConversionInfo info;
info.new_name = param_name;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
info.old_key = 71;
#elif APM_BUILD_TYPE(APM_BUILD_ArduCopter)
info.old_key = 53;
#elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
info.old_key = 35;
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
info.old_key = 197;
#else
params[0].type.save(true);
return; // no conversion is supported on this platform
#endif
for (uint8_t i = 0; i < ARRAY_SIZE(conversionTable); i++) {
uint8_t param_instance = conversionTable[i].instance + 1;
uint8_t destination_index = conversionTable[i].new_index;
info.old_group_element = conversionTable[i].old_element;
info.type = (ap_var_type)AP_RangeFinder_Params::var_info[destination_index].type;
hal.util->snprintf(param_name, sizeof(param_name), "RNGFND%X_%s", param_instance, AP_RangeFinder_Params::var_info[destination_index].name);
param_name[sizeof(param_name)-1] = '\0';
AP_Param::convert_old_parameter(&info, 1.0f, 0);
}
// force _params[0]._type into storage to flag that conversion has been done
params[0].type.save(true);
}
/*
initialise the RangeFinder class. We do detection of attached range
finders here. For now we won't allow for hot-plugging of
@ -592,6 +297,9 @@ void RangeFinder::init(void)
// init called a 2nd time?
return;
}
convert_params();
for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
// serial_instance will be increased inside detect_instance
// if a serial driver is loaded for this instance
@ -620,7 +328,7 @@ void RangeFinder::update(void)
{
for (uint8_t i=0; i<num_instances; i++) {
if (drivers[i] != nullptr) {
if (state[i].type == RangeFinder_TYPE_NONE) {
if (params[i].type == RangeFinder_TYPE_NONE) {
// allow user to disable a rangefinder at runtime
state[i].status = RangeFinder_NotConnected;
state[i].range_valid_count = 0;
@ -650,54 +358,54 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
*/
void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
{
enum RangeFinder_Type _type = (enum RangeFinder_Type)state[instance].type.get();
enum RangeFinder_Type _type = (enum RangeFinder_Type)params[instance].type.get();
switch (_type) {
case RangeFinder_TYPE_PLI2C:
case RangeFinder_TYPE_PLI2CV3:
case RangeFinder_TYPE_PLI2CV3HP:
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, state[instance], _type))) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, state[instance], _type));
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, state[instance], params[instance], _type))) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, state[instance], params[instance], _type));
}
break;
case RangeFinder_TYPE_MBI2C:
if (!_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance],
if (!_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) {
_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance],
_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)));
}
break;
case RangeFinder_TYPE_LWI2C:
if (state[instance].address) {
if (params[instance].address) {
#ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, state[instance].address)));
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address)));
#else
if (!_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
hal.i2c_mgr->get_device(1, state[instance].address)))) {
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
hal.i2c_mgr->get_device(0, state[instance].address)));
if (!_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(1, params[instance].address)))) {
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(0, params[instance].address)));
}
#endif
}
break;
case RangeFinder_TYPE_TRI2C:
if (state[instance].address) {
if (!_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance],
hal.i2c_mgr->get_device(1, state[instance].address)))) {
_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance],
hal.i2c_mgr->get_device(0, state[instance].address)));
if (params[instance].address) {
if (!_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(1, params[instance].address)))) {
_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(0, params[instance].address)));
}
}
break;
case RangeFinder_TYPE_VL53L0X:
if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance],
hal.i2c_mgr->get_device(1, 0x29)))) {
if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance],
hal.i2c_mgr->get_device(0, 0x29)))) {
if (!_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance],
hal.i2c_mgr->get_device(1, 0x29)))) {
_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance],
hal.i2c_mgr->get_device(0, 0x29)));
if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(1, params[instance].address)))) {
if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(0, params[instance].address)))) {
if (!_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(1, params[instance].address)))) {
_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(0, params[instance].address)));
}
}
}
@ -707,30 +415,30 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
// to ease moving from PX4 to ChibiOS we'll lie a little about
// the backend driver...
if (AP_RangeFinder_PWM::detect()) {
drivers[instance] = new AP_RangeFinder_PWM(state[instance], _powersave_range, estimated_terrain_height);
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height);
}
break;
#endif
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
case RangeFinder_TYPE_BBB_PRU:
if (AP_RangeFinder_BBB_PRU::detect()) {
drivers[instance] = new AP_RangeFinder_BBB_PRU(state[instance]);
drivers[instance] = new AP_RangeFinder_BBB_PRU(state[instance], params[instance]);
}
break;
#endif
case RangeFinder_TYPE_LWSER:
if (AP_RangeFinder_LightWareSerial::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], serial_manager, serial_instance++);
drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], params[instance], serial_manager, serial_instance++);
}
break;
case RangeFinder_TYPE_LEDDARONE:
if (AP_RangeFinder_LeddarOne::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], serial_manager, serial_instance++);
drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], params[instance], serial_manager, serial_instance++);
}
break;
case RangeFinder_TYPE_ULANDING:
if (AP_RangeFinder_uLanding::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_uLanding(state[instance], serial_manager, serial_instance++);
drivers[instance] = new AP_RangeFinder_uLanding(state[instance], params[instance], serial_manager, serial_instance++);
}
break;
#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
@ -743,43 +451,43 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
#endif
case RangeFinder_TYPE_MAVLink:
if (AP_RangeFinder_MAVLink::detect()) {
drivers[instance] = new AP_RangeFinder_MAVLink(state[instance]);
drivers[instance] = new AP_RangeFinder_MAVLink(state[instance], params[instance]);
}
break;
case RangeFinder_TYPE_MBSER:
if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], serial_manager, serial_instance++);
drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance], serial_manager, serial_instance++);
}
break;
case RangeFinder_TYPE_ANALOG:
// note that analog will always come back as present if the pin is valid
if (AP_RangeFinder_analog::detect(state[instance])) {
drivers[instance] = new AP_RangeFinder_analog(state[instance]);
if (AP_RangeFinder_analog::detect(params[instance])) {
drivers[instance] = new AP_RangeFinder_analog(state[instance], params[instance]);
}
break;
case RangeFinder_TYPE_NMEA:
if (AP_RangeFinder_NMEA::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_NMEA(state[instance], serial_manager, serial_instance++);
drivers[instance] = new AP_RangeFinder_NMEA(state[instance], params[instance], serial_manager, serial_instance++);
}
break;
case RangeFinder_TYPE_WASP:
if (AP_RangeFinder_Wasp::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_Wasp(state[instance], serial_manager, serial_instance++);
drivers[instance] = new AP_RangeFinder_Wasp(state[instance], params[instance], serial_manager, serial_instance++);
}
break;
case RangeFinder_TYPE_BenewakeTF02:
if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02);
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02);
}
break;
case RangeFinder_TYPE_BenewakeTFmini:
if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
}
break;
case RangeFinder_TYPE_PWM:
if (AP_RangeFinder_PWM::detect()) {
drivers[instance] = new AP_RangeFinder_PWM(state[instance], _powersave_range, estimated_terrain_height);
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height);
}
break;
default:
@ -819,7 +527,7 @@ void RangeFinder::handle_msg(mavlink_message_t *msg)
{
uint8_t i;
for (i=0; i<num_instances; i++) {
if ((drivers[i] != nullptr) && (state[i].type != RangeFinder_TYPE_NONE)) {
if ((drivers[i] != nullptr) && (params[i].type != RangeFinder_TYPE_NONE)) {
drivers[i]->handle_msg(msg);
}
}
@ -919,7 +627,7 @@ bool RangeFinder::pre_arm_check() const
{
for (uint8_t i=0; i<num_instances; i++) {
// if driver is valid but pre_arm_check is false, return false
if ((drivers[i] != nullptr) && (state[i].type != RangeFinder_TYPE_NONE) && !state[i].pre_arm_check) {
if ((drivers[i] != nullptr) && (params[i].type != RangeFinder_TYPE_NONE) && !state[i].pre_arm_check) {
return false;
}
}

23
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -19,9 +19,10 @@
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include "AP_RangeFinder_Params.h"
// Maximum number of range finder instances available on this platform
#define RANGEFINDER_MAX_INSTANCES 2
#define RANGEFINDER_MAX_INSTANCES 10
#define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10
#define RANGEFINDER_PREARM_ALT_MAX_CM 200
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -95,26 +96,10 @@ public:
uint16_t pre_arm_distance_max; // max distance captured during pre-arm checks
uint32_t last_reading_ms; // system time of last successful update from sensor
AP_Int8 type;
AP_Int8 pin;
AP_Int8 ratiometric;
AP_Int8 stop_pin;
AP_Int16 settle_time_ms;
AP_Float scaling;
AP_Float offset;
AP_Int8 function;
AP_Int16 min_distance_cm;
AP_Int16 max_distance_cm;
AP_Int8 ground_clearance_cm;
AP_Int8 address;
AP_Vector3f pos_offset; // position offset in body frame
AP_Int8 orientation;
const struct AP_Param::GroupInfo *var_info;
};
static const struct AP_Param::GroupInfo *backend_var_info[RANGEFINDER_MAX_INSTANCES];
AP_Int16 _powersave_range;
// parameters for each instance
static const struct AP_Param::GroupInfo var_info[];
@ -173,6 +158,8 @@ public:
static RangeFinder *get_singleton(void) { return _singleton; }
protected:
AP_RangeFinder_Params params[RANGEFINDER_MAX_INSTANCES];
private:
static RangeFinder *_singleton;
@ -184,6 +171,8 @@ private:
AP_SerialManager &serial_manager;
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);
void update_instance(uint8_t instance);

17
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
View File

@ -24,20 +24,21 @@ extern const AP_HAL::HAL& hal;
base class constructor.
This incorporates initialisation as well.
*/
AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state) :
state(_state)
AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
state(_state),
params(_params)
{
}
MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const {
if (state.type == RangeFinder::RangeFinder_TYPE_NONE) {
if (params.type == RangeFinder::RangeFinder_TYPE_NONE) {
return MAV_DISTANCE_SENSOR_UNKNOWN;
}
return _get_mav_distance_sensor_type();
}
RangeFinder::RangeFinder_Status AP_RangeFinder_Backend::status() const {
if (state.type == RangeFinder::RangeFinder_TYPE_NONE) {
if (params.type == RangeFinder::RangeFinder_TYPE_NONE) {
// turned off at runtime?
return RangeFinder::RangeFinder_NotConnected;
}
@ -54,9 +55,9 @@ bool AP_RangeFinder_Backend::has_data() const {
void AP_RangeFinder_Backend::update_status()
{
// check distance
if ((int16_t)state.distance_cm > state.max_distance_cm) {
if ((int16_t)state.distance_cm > params.max_distance_cm) {
set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
} else if ((int16_t)state.distance_cm < state.min_distance_cm) {
} else if ((int16_t)state.distance_cm < params.min_distance_cm) {
set_status(RangeFinder::RangeFinder_OutOfRangeLow);
} else {
set_status(RangeFinder::RangeFinder_Good);
@ -98,8 +99,8 @@ void AP_RangeFinder_Backend::update_pre_arm_check()
// Check that the range finder has been exercised through a realistic range of movement
if (((state.pre_arm_distance_max - state.pre_arm_distance_min) >= RANGEFINDER_PREARM_REQUIRED_CHANGE_CM) &&
(state.pre_arm_distance_max < RANGEFINDER_PREARM_ALT_MAX_CM) &&
((int16_t)state.pre_arm_distance_min < (MAX(state.ground_clearance_cm,state.min_distance_cm) + 10)) &&
((int16_t)state.pre_arm_distance_min > (MIN(state.ground_clearance_cm,state.min_distance_cm) - 10))) {
((int16_t)state.pre_arm_distance_min < (MAX(params.ground_clearance_cm,params.min_distance_cm) + 10)) &&
((int16_t)state.pre_arm_distance_min > (MIN(params.ground_clearance_cm,params.min_distance_cm) - 10))) {
state.pre_arm_check = true;
}
}

16
libraries/AP_RangeFinder/RangeFinder_Backend.h
View File

@ -22,7 +22,8 @@ class AP_RangeFinder_Backend
{
public:
// constructor. This incorporates initialisation as well.
AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state);
//AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state);
AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
// we declare a virtual destructor so that RangeFinder drivers can
// override with a custom destructor if need be
@ -35,15 +36,15 @@ public:
void update_pre_arm_check();
enum Rotation orientation() const { return (Rotation)state.orientation.get(); }
enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
uint16_t distance_cm() const { return state.distance_cm; }
uint16_t voltage_mv() const { return state.voltage_mv; }
int16_t max_distance_cm() const { return state.max_distance_cm; }
int16_t min_distance_cm() const { return state.min_distance_cm; }
int16_t ground_clearance_cm() const { return state.ground_clearance_cm; }
int16_t max_distance_cm() const { return params.max_distance_cm; }
int16_t min_distance_cm() const { return params.min_distance_cm; }
int16_t ground_clearance_cm() const { return params.ground_clearance_cm; }
MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const;
RangeFinder::RangeFinder_Status status() const;
RangeFinder::RangeFinder_Type type() const { return (RangeFinder::RangeFinder_Type)state.type.get(); }
RangeFinder::RangeFinder_Type type() const { return (RangeFinder::RangeFinder_Type)params.type.get(); }
// true if sensor is returning data
bool has_data() const;
@ -53,7 +54,7 @@ public:
// return a 3D vector defining the position offset of the sensor
// in metres relative to the body frame origin
const Vector3f &get_pos_offset() const { return state.pos_offset; }
const Vector3f &get_pos_offset() const { return params.pos_offset; }
// return system time of last successful read from the sensor
uint32_t last_reading_ms() const { return state.last_reading_ms; }
@ -67,6 +68,7 @@ protected:
void set_status(RangeFinder::RangeFinder_Status status);
RangeFinder::RangeFinder_State &state;
AP_RangeFinder_Params &params;
// semaphore for access to shared frontend data
HAL_Semaphore _sem;