zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
Andrew Tridgell 11 years ago
parent
429431157b
commit
cb037f3416
5 changed files with 263 additions and 91 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      libraries/AP_RangeFinder/AP_RangeFinder.h
  2. 190
      libraries/AP_RangeFinder/RangeFinder.cpp
  3. 108
      libraries/AP_RangeFinder/RangeFinder.h
  4. 30
      libraries/AP_RangeFinder/RangeFinder_Backend.cpp
  5. 19
      libraries/AP_RangeFinder/RangeFinder_Backend.h

7
libraries/AP_RangeFinder/AP_RangeFinder.h
Unescape View File

@ -3,9 +3,4 @@ @@ -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"

190
libraries/AP_RangeFinder/RangeFinder.cpp
Unescape View File

@ -14,34 +14,182 @@ @@ -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()
{
int16_t temp_dist;
// @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),
// convert analog value to distance in cm (using child implementation most likely)
temp_dist = convert_raw_to_distance(_analog_source->read_average());
// @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),
// ensure distance is within min and max
temp_dist = constrain_float(temp_dist, min_distance, max_distance);
// @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
if (_mode_filter) {
distance = _mode_filter->apply(temp_dist);
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)
{
if (num_instances != 0) {
// init called a 2nd time?
return;
}
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;
}
}
else {
distance = temp_dist;
}
/*
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;
}

108
libraries/AP_RangeFinder/RangeFinder.h
Unescape View File

@ -19,56 +19,63 @@ @@ -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: @@ -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: @@ -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;
}
uint16_t distance_cm() const {
return distance_cm(primary_instance);
}
uint16_t voltage_mv(uint8_t instance) const {
return _RangeFinder_STATE(instance).voltage_mv;
}
int16_t distance() const {
return distance(primary_instance);
uint16_t voltage_mv() const {
return voltage_mv(primary_instance);
}
int16_t max_distance(uint8_t instance) const {
return _RangeFinder_STATE(instance).max_distance;
int16_t max_distance_cm(uint8_t instance) const {
return _max_distance_cm[instance];
}
int16_t max_distance() const {
return max_distance(primary_instance);
int16_t max_distance_cm() const {
return max_distance_cm(primary_instance);
}
int16_t min_distance(uint8_t instance) const {
return _RangeFinder_STATE(instance).min_distance;
int16_t min_distance_cm(uint8_t instance) const {
return _min_distance_cm[instance];
}
int16_t min_distance() const {
return min_distance(primary_instance);
int16_t min_distance_cm() const {
return min_distance_cm(primary_instance);
}
bool healthy(uint8_t instance) const {
@ -113,20 +131,6 @@ public: @@ -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
Unescape View File

@ -0,0 +1,30 @@ @@ -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
libraries/AP_RangeFinder/RangeFinder_Backend.h
Unescape View File

@ -19,28 +19,23 @@ @@ -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__

Write Preview
Loading…
Cancel
Save