diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
index f75d241e76..f074af7d9a 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.h
@@ -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"
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 064db169a6..293c4bb417 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -14,34 +14,182 @@
along with this program. If not, see .
*/
-/*
- * 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; iupdate();
+ }
+ }
+
+ // 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;
}
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index 6fc4b46299..870b6e11f1 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -19,56 +19,63 @@
#include
#include
-#include // Filter library
-#include
+#include
// 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;
+ }
+ 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:
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];
diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp
new file mode 100644
index 0000000000..c694cac498
--- /dev/null
+++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp
@@ -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 .
+ */
+
+#include
+#include
+#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)
+{
+}
diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h
index 05c5093cc0..fc1caaf48e 100644
--- a/libraries/AP_RangeFinder/RangeFinder_Backend.h
+++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h
@@ -19,28 +19,23 @@
#include
#include
-#include // 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__