AP_RangeFinder: new rangefinder API ready for its first backend
the backends are setup to have just the minimum functionality needed for a rangefinder, with all of the higher level logic in the frontend. This should make writing a new backend easier
This commit is contained in:
Andrew Tridgell
parent
429431157b
commit
cb037f3416
@ -3,9 +3,4 @@
|
||||
/// @file AP_RangeFinder.h
|
||||
/// @brief Catch-all header that defines all supported RangeFinder classes.
|
||||
|
||||
#include "AP_RangeFinder_SharpGP2Y.h"
|
||||
#include "AP_RangeFinder_MaxsonarXL.h"
|
||||
#include "AP_RangeFinder_MaxsonarI2CXL.h"
|
||||
#include "AP_RangeFinder_PulsedLightLRF.h"
|
||||
#include "AP_RangeFinder_analog.h"
|
||||
#include "AP_RangeFinder_PX4.h"
|
||||
#include "RangeFinder.h"
|
||||
|
||||
@ -14,34 +14,182 @@
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
|
||||
* infrared proximity sensor
|
||||
* Code by Jose Julio and Randy Mackay. DIYDrones.com
|
||||
*
|
||||
* This has the basic functions that all RangeFinders need implemented
|
||||
*/
|
||||
#include "RangeFinder.h"
|
||||
#include "AP_RangeFinder_analog.h"
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
|
||||
// @Param: _TYPE
|
||||
// @DisplayName: Rangefinder type
|
||||
// @Description: what type of rangefinder is connected
|
||||
// @Values: 0:None,1:Auto,2:Analog,3:MaxbotixI2C,4:PulsedLightI2C,5:PX4
|
||||
AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0),
|
||||
|
||||
// Read Sensor data - post of the ahrd work is done by the child class's convert_raw_to_distance
|
||||
int16_t RangeFinder::read()
|
||||
// @Param: _PIN
|
||||
// @DisplayName: Rangefinder pin
|
||||
// @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
|
||||
AP_GROUPINFO("_PIN", 1, RangeFinder, _pin[0], -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: meters/Volt
|
||||
// @Increment: 0.001
|
||||
AP_GROUPINFO("_SCALING", 2, RangeFinder, _scaling[0], 3.0),
|
||||
|
||||
// @Param: _OFFSET
|
||||
// @DisplayName: rangefinder offset
|
||||
// @Description: Offset in volts for zero distance
|
||||
// @Units: Volts
|
||||
// @Increment: 0.001
|
||||
AP_GROUPINFO("_OFFSET", 3, RangeFinder, _offset[0], 0.0),
|
||||
|
||||
// @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
|
||||
AP_GROUPINFO("_FUNCTION", 4, RangeFinder, _function[0], 0),
|
||||
|
||||
// @Param: _MIN_CM
|
||||
// @DisplayName: Rangefinder minimum distance
|
||||
// @Description: minimum distance in centimeters that rangefinder can reliably read
|
||||
// @Units: centimeters
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("_MIN_CM", 5, RangeFinder, _min_distance_cm[0], 20),
|
||||
|
||||
// @Param: _MAX_CM
|
||||
// @DisplayName: Rangefinder maximum distance
|
||||
// @Description: maximum distance in centimeters that rangefinder can reliably read
|
||||
// @Units: centimeters
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("_MAX_CM", 6, RangeFinder, _max_distance_cm[0], 700),
|
||||
|
||||
// @Param: _STOP_PIN
|
||||
// @DisplayName: Rangefinder stop pin
|
||||
// @Description: Digital pin that enables/disables rangefinder measurement for an analog sonar. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the sonar and set to 0 to disable it. This can be used to ensure that multiple sonars don't interfere with each other.
|
||||
AP_GROUPINFO("_STOP_PIN", 7, RangeFinder, _stop_pin[0], -1),
|
||||
|
||||
// @Param: _SETTLE_MS
|
||||
// @DisplayName: Sonar settle time
|
||||
// @Description: The time in milliseconds that the sonar reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the sonar to give a reading after we set the STOP_PIN high. For a sonar 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: milliseconds
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("_SETTLE_MS", 8, RangeFinder, _settle_time_ms[0], 0),
|
||||
|
||||
// 9..12 left for future expansion
|
||||
|
||||
#if RANGEFINDER_MAX_INSTANCES > 1
|
||||
// @Param: 2_PIN
|
||||
// @DisplayName: Rangefinder pin
|
||||
// @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
|
||||
AP_GROUPINFO("2_PIN", 13, RangeFinder, _pin[1], -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: meters/Volt
|
||||
// @Increment: 0.001
|
||||
AP_GROUPINFO("2_SCALING", 14, RangeFinder, _scaling[1], 3.0),
|
||||
|
||||
// @Param: 2_OFFSET
|
||||
// @DisplayName: rangefinder offset
|
||||
// @Description: Offset in volts for zero distance
|
||||
// @Units: Volts
|
||||
// @Increment: 0.001
|
||||
AP_GROUPINFO("2_OFFSET", 15, RangeFinder, _offset[1], 0.0),
|
||||
|
||||
// @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
|
||||
AP_GROUPINFO("2_FUNCTION", 16, RangeFinder, _function[1], 0),
|
||||
|
||||
// @Param: 2_MIN_CM
|
||||
// @DisplayName: Rangefinder minimum distance
|
||||
// @Description: minimum distance in centimeters that rangefinder can reliably read
|
||||
// @Units: centimeters
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("2_MIN_CM", 17, RangeFinder, _min_distance_cm[1], 20),
|
||||
|
||||
// @Param: 2_MAX_CM
|
||||
// @DisplayName: Rangefinder maximum distance
|
||||
// @Description: maximum distance in centimeters that rangefinder can reliably read
|
||||
// @Units: centimeters
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("2_MAX_CM", 18, RangeFinder, _max_distance_cm[1], 700),
|
||||
|
||||
// @Param: 2_STOP_PIN
|
||||
// @DisplayName: Rangefinder stop pin
|
||||
// @Description: Digital pin that enables/disables rangefinder measurement for an analog sonar. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the sonar and set to 0 to disable it. This can be used to ensure that multiple sonars don't interfere with each other.
|
||||
AP_GROUPINFO("2_STOP_PIN", 19, RangeFinder, _stop_pin[1], -1),
|
||||
|
||||
// @Param: 2_SETTLE_MS
|
||||
// @DisplayName: Sonar settle time
|
||||
// @Description: The time in milliseconds that the sonar reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the sonar to give a reading after we set the STOP_PIN high. For a sonar 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: milliseconds
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("2_SETTLE_MS", 20, RangeFinder, _settle_time_ms[1], 0),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
/*
|
||||
initialise the RangeFinder class. We do detection of attached range
|
||||
finders here. For now we won't allow for hot-plugging of
|
||||
rangefinders.
|
||||
*/
|
||||
void RangeFinder::init(void)
|
||||
{
|
||||
int16_t temp_dist;
|
||||
|
||||
// convert analog value to distance in cm (using child implementation most likely)
|
||||
temp_dist = convert_raw_to_distance(_analog_source->read_average());
|
||||
|
||||
// ensure distance is within min and max
|
||||
temp_dist = constrain_float(temp_dist, min_distance, max_distance);
|
||||
|
||||
if (_mode_filter) {
|
||||
distance = _mode_filter->apply(temp_dist);
|
||||
if (num_instances != 0) {
|
||||
// init called a 2nd time?
|
||||
return;
|
||||
}
|
||||
else {
|
||||
distance = temp_dist;
|
||||
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
|
||||
detect_instance(i);
|
||||
if (drivers[i] != NULL) {
|
||||
// we loaded a driver for this instance, so it must be
|
||||
// present (although it may not be healthy)
|
||||
num_instances = i+1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update RangeFinder state for all instances. This should be called at
|
||||
around 10Hz by main loop
|
||||
*/
|
||||
void RangeFinder::update(void)
|
||||
{
|
||||
for (uint8_t i=0; i<num_instances; i++) {
|
||||
if (drivers[i] != NULL) {
|
||||
if (_type[i] == RangeFinder_TYPE_NONE) {
|
||||
// allow user to disable a rangefinder at runtime
|
||||
state[i].healthy = false;
|
||||
continue;
|
||||
}
|
||||
drivers[i]->update();
|
||||
}
|
||||
}
|
||||
|
||||
// work out primary instance - first healthy sensor
|
||||
for (int8_t i=num_instances-1; i>=0; i--) {
|
||||
if (drivers[i] != NULL && state[i].healthy) {
|
||||
primary_instance = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
detect if an instance of a rangefinder is connected.
|
||||
*/
|
||||
void RangeFinder::detect_instance(uint8_t instance)
|
||||
{
|
||||
if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_ANALOG) {
|
||||
if (AP_RangeFinder_analog::detect(*this, instance)) {
|
||||
state[instance].instance = instance;
|
||||
drivers[instance] = new AP_RangeFinder_analog(*this, instance, state[instance]);
|
||||
}
|
||||
}
|
||||
return distance;
|
||||
}
|
||||
|
||||
|
||||
@ -19,56 +19,63 @@
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_HAL.h>
|
||||
#include <Filter.h> // Filter library
|
||||
#include <rotations.h>
|
||||
#include <AP_Param.h>
|
||||
|
||||
// Maximum number of range finder instances available on this platform
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
#define RANGEFINDER_MAX_INSTANCES 2
|
||||
#else
|
||||
#define RANGEFINDER_MAX_INSTANCES 1
|
||||
#endif
|
||||
|
||||
class AP_RangeFinder_Backend;
|
||||
|
||||
class AP_RangeFinder_Backend;
|
||||
|
||||
class RangeFinder
|
||||
{
|
||||
public:
|
||||
RangeFinder(FilterInt16 *filter) :
|
||||
_mode_filter(filter) {
|
||||
RangeFinder(void) :
|
||||
num_instances(0)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
// RangeFinder driver types
|
||||
enum RangeFinder_Type {
|
||||
RangeFinder_TYPE_NONE = 0,
|
||||
RangeFinder_TYPE_AUTO = 1,
|
||||
RangeFinder_TYPE_NONE = 0,
|
||||
RangeFinder_TYPE_AUTO = 1,
|
||||
RangeFinder_TYPE_ANALOG = 2,
|
||||
RangeFinder_TYPE_MBI2C = 3,
|
||||
RangeFinder_TYPE_PLI2C = 4
|
||||
RangeFinder_TYPE_PLI2C = 4,
|
||||
RangeFinder_TYPE_PX4 = 5
|
||||
};
|
||||
|
||||
enum RangeFinder_Location {
|
||||
RangeFinder_LOCATION_FRONT = 0,
|
||||
RangeFinder_LOCATION_RIGHT = 1,
|
||||
RangeFinder_LOCATION_LEFT = 2,
|
||||
RangeFinder_LOCATION_BACK = 3
|
||||
enum RangeFinder_Function {
|
||||
FUNCTION_LINEAR = 0,
|
||||
FUNCTION_INVERTED = 1,
|
||||
FUNCTION_HYPERBOLA = 2
|
||||
};
|
||||
|
||||
|
||||
|
||||
// The RangeFinder_State structure is filled in by the backend driver
|
||||
struct RangeFinder_State {
|
||||
uint8_t instance; // the instance number of this RangeFinder
|
||||
int16_t distance; // distance: in cm
|
||||
int16_t max_distance; // maximum measurable distance: in cm
|
||||
int16_t min_distance; // minimum measurable distance: in cm
|
||||
bool healthy; // sensor is communicating correctly
|
||||
Rotation orientation; // none would imply that it is pointing out the craft front
|
||||
RangeFinder_Location location; // generic approximation of the sensor's location on the craft
|
||||
uint8_t instance; // the instance number of this RangeFinder
|
||||
uint16_t distance_cm; // distance: in cm
|
||||
uint16_t voltage_mv; // voltage in millivolts,
|
||||
// if applicable, otherwise 0
|
||||
bool healthy; // sensor is communicating correctly
|
||||
};
|
||||
|
||||
AP_Int8 _type[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Int8 _pin[RANGEFINDER_MAX_INSTANCES];
|
||||
FilterInt16 * _mode_filter;
|
||||
// parameters for each instance
|
||||
AP_Int8 _type[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Int8 _pin[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Int8 _stop_pin[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Int16 _settle_time_ms[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Float _scaling[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Float _offset[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Int8 _function[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Int16 _min_distance_cm[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_Int16 _max_distance_cm[RANGEFINDER_MAX_INSTANCES];
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// Return the number of range finder instances
|
||||
@ -76,7 +83,11 @@ public:
|
||||
return num_instances;
|
||||
}
|
||||
|
||||
// detect and initialise any available rangefinders
|
||||
void init(void);
|
||||
|
||||
// update state of all rangefinders. Should be called at around
|
||||
// 10Hz from main loop
|
||||
void update(void);
|
||||
|
||||
#if RANGEFINDER_MAX_INSTANCES == 1
|
||||
@ -85,25 +96,32 @@ public:
|
||||
# define _RangeFinder_STATE(instance) state[instance]
|
||||
#endif
|
||||
|
||||
int16_t distance(uint8_t instance) const {
|
||||
return _RangeFinder_STATE(instance).distance;
|
||||
uint16_t distance_cm(uint8_t instance) const {
|
||||
return _RangeFinder_STATE(instance).distance_cm;
|
||||
}
|
||||
int16_t distance() const {
|
||||
return distance(primary_instance);
|
||||
uint16_t distance_cm() const {
|
||||
return distance_cm(primary_instance);
|
||||
}
|
||||
|
||||
int16_t max_distance(uint8_t instance) const {
|
||||
return _RangeFinder_STATE(instance).max_distance;
|
||||
uint16_t voltage_mv(uint8_t instance) const {
|
||||
return _RangeFinder_STATE(instance).voltage_mv;
|
||||
}
|
||||
int16_t max_distance() const {
|
||||
return max_distance(primary_instance);
|
||||
uint16_t voltage_mv() const {
|
||||
return voltage_mv(primary_instance);
|
||||
}
|
||||
|
||||
int16_t min_distance(uint8_t instance) const {
|
||||
return _RangeFinder_STATE(instance).min_distance;
|
||||
int16_t max_distance_cm(uint8_t instance) const {
|
||||
return _max_distance_cm[instance];
|
||||
}
|
||||
int16_t min_distance() const {
|
||||
return min_distance(primary_instance);
|
||||
int16_t max_distance_cm() const {
|
||||
return max_distance_cm(primary_instance);
|
||||
}
|
||||
|
||||
int16_t min_distance_cm(uint8_t instance) const {
|
||||
return _min_distance_cm[instance];
|
||||
}
|
||||
int16_t min_distance_cm() const {
|
||||
return min_distance_cm(primary_instance);
|
||||
}
|
||||
|
||||
bool healthy(uint8_t instance) const {
|
||||
@ -113,20 +131,6 @@ public:
|
||||
return healthy(primary_instance);
|
||||
}
|
||||
|
||||
Rotation orientation(uint8_t instance) const {
|
||||
return _RangeFinder_STATE(instance).orientation;
|
||||
}
|
||||
Rotation orientation() const {
|
||||
return orientation(primary_instance);
|
||||
}
|
||||
|
||||
RangeFinder_Location location(uint8_t instance) const {
|
||||
return _RangeFinder_STATE(instance).location;
|
||||
}
|
||||
RangeFinder_Location location() const {
|
||||
return location(primary_instance);
|
||||
}
|
||||
|
||||
private:
|
||||
RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
|
||||
AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
|
||||
|
||||
30
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
Normal file
30
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
Normal file
@ -0,0 +1,30 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_HAL.h>
|
||||
#include "RangeFinder.h"
|
||||
#include "RangeFinder_Backend.h"
|
||||
|
||||
/*
|
||||
base class constructor.
|
||||
This incorporates initialisation as well.
|
||||
*/
|
||||
AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
|
||||
ranger(_ranger),
|
||||
state(_state)
|
||||
{
|
||||
}
|
||||
@ -19,28 +19,23 @@
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_HAL.h>
|
||||
#include <Filter.h> // Filter library
|
||||
#include "RangeFinder.h"
|
||||
|
||||
class AP_RangeFinder_Backend
|
||||
{
|
||||
public:
|
||||
AP_RangeFinder_Backend(AP_RangeFinder &_ranger, AP_RangeFinder::RangeFinder_State &_state);
|
||||
// constructor. This incorporates initialisation as well.
|
||||
AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
|
||||
|
||||
// we declare a virtual destructor so that RangeFinder drivers can
|
||||
// override with a custom destructor if need be
|
||||
virtual ~AP_RangeFinder_Backend(void) {}
|
||||
|
||||
// Backend specific init functionality
|
||||
virtual void init(void);
|
||||
// update the state structure
|
||||
virtual void update() = 0;
|
||||
|
||||
// Backend specific read functionality
|
||||
virtual int16_t read();
|
||||
|
||||
// Each driver will have a static detect method - example:
|
||||
// static bool _detect(????); // Not sure yet what data to pass in
|
||||
|
||||
protected:
|
||||
AP_RangeFinder &ranger;
|
||||
AP_RangeFinder::RangeFinder_State &state;
|
||||
RangeFinder &ranger;
|
||||
RangeFinder::RangeFinder_State &state;
|
||||
};
|
||||
#endif // __AP_RANGEFINDER_BACKEND_H__
|
||||
|
||||
Loading…
Reference in New Issue
Block a user