From 92b76b4be449adb58268db67e2514f56e1eaf9a1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Jun 2014 12:57:28 +1000 Subject: [PATCH] AP_RangeFinder: converted analog rangefinder backend to new API this gets analog rangefinders of all types working --- .../AP_RangeFinder/AP_RangeFinder_analog.cpp | 219 ++++++------------ .../AP_RangeFinder/AP_RangeFinder_analog.h | 53 ++--- 2 files changed, 89 insertions(+), 183 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index 7ffa17a115..5eb6a984a9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -20,171 +20,100 @@ */ #include +#include +#include +#include "RangeFinder.h" #include "AP_RangeFinder_analog.h" extern const AP_HAL::HAL& hal; -#define SONAR_DEFAULT_PIN 0 - -// table of user settable parameters -const AP_Param::GroupInfo AP_RangeFinder_analog::var_info[] PROGMEM = { - - // @Param: PIN - // @DisplayName: Sonar pin - // @Description: Analog pin that sonar 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", 0, AP_RangeFinder_analog, _pin, SONAR_DEFAULT_PIN), - - // @Param: SCALING - // @DisplayName: Sonar scaling - // @Description: Scaling factor between sonar 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", 1, AP_RangeFinder_analog, _scaling, 3.0), - - // @Param: OFFSET - // @DisplayName: Sonar offset - // @Description: Offset in volts for zero distance - // @Units: Volts - // @Increment: 0.001 - AP_GROUPINFO("OFFSET", 2, AP_RangeFinder_analog, _offset, 0.0), - - // @Param: FUNCTION - // @DisplayName: Sonar 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", 3, AP_RangeFinder_analog, _function, 0), - - // @Param: MIN_CM - // @DisplayName: Sonar minimum distance - // @Description: minimum distance in centimeters that sonar can reliably read - // @Units: centimeters - // @Increment: 1 - AP_GROUPINFO("MIN_CM", 4, AP_RangeFinder_analog, _min_distance_cm, 20), - - // @Param: MAX_CM - // @DisplayName: Sonar maximum distance - // @Description: maximum distance in centimeters that sonar can reliably read - // @Units: centimeters - // @Increment: 1 - AP_GROUPINFO("MAX_CM", 5, AP_RangeFinder_analog, _max_distance_cm, 700), - - // @Param: ENABLE - // @DisplayName: Sonar enabled - // @Description: set to 1 to enable this sonar - // @Values: 0:Disabled,1:Enabled - AP_GROUPINFO("ENABLE", 6, AP_RangeFinder_analog, _enabled, 0), - - // @Param: STOP_PIN - // @DisplayName: Sonar stop pin - // @Description: Digital pin that enables/disables sonar measurement. 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, AP_RangeFinder_analog, _stop_pin, -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, AP_RangeFinder_analog, _settle_time_ms, 0), - - AP_GROUPEND -}; - -// Constructor -AP_RangeFinder_analog::AP_RangeFinder_analog(void) -{ - AP_Param::setup_object_defaults(this, var_info); -} - - -/* Initialisation: - we pass the analog source in at Init() time rather than in the - constructor as otherwise the object could not have parameters, as - only static objects can have parameters, but the analog sources in - AP_HAL are allocated at runtime +/* + The constructor also initialises the rangefinder. Note that this + constructor is not called until detect() returns true, so we + already know that we should setup the rangefinder */ -void AP_RangeFinder_analog::Init(void *adc) +AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : + AP_RangeFinder_Backend(_ranger, instance, _state) { - if (!_enabled) { - return; - } - if (_source != NULL) { - return; - } - _source = hal.analogin->channel(_pin); - _source->set_stop_pin((uint8_t)_stop_pin); - _source->set_settle_time((uint16_t)_settle_time_ms); + source = hal.analogin->channel(ranger._pin[instance]); + if (source == NULL) { + // failed to allocate a ADC channel? This shouldn't happen + state.healthy = false; + return; + } + source->set_stop_pin((uint8_t)ranger._stop_pin[instance]); + source->set_settle_time((uint16_t)ranger._settle_time_ms[instance]); } +/* + detect if an analog rangefinder is connected. The only thing we + 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 &_ranger, uint8_t instance) +{ + if (_ranger._pin[instance] != -1) { + return true; + } + return false; +} + + /* - return raw voltage + update raw voltage state */ -float AP_RangeFinder_analog::voltage(void) +void AP_RangeFinder_analog::update_voltage(void) { - if (!_enabled) { - return 0.0f; - } - if (_source == NULL) { - return 0.0f; + if (source == NULL) { + state.voltage_mv = 0; + return; } // cope with changed settings - _source->set_pin(_pin); - _source->set_stop_pin((uint8_t)_stop_pin); - _source->set_settle_time((uint16_t)_settle_time_ms); - return _source->voltage_average_ratiometric(); + source->set_pin(ranger._pin[state.instance]); + source->set_stop_pin((uint8_t)ranger._stop_pin[state.instance]); + source->set_settle_time((uint16_t)ranger._settle_time_ms[state.instance]); + state.voltage_mv = source->voltage_average_ratiometric() * 1000U; } /* - return distance in centimeters + update distance_cm */ -float AP_RangeFinder_analog::distance_cm(void) +void AP_RangeFinder_analog::update(void) { - if (!_enabled) { - return 0.0f; - } + update_voltage(); + float v = state.voltage_mv * 0.001f; + float dist_m = 0; + float scaling = ranger._scaling[state.instance]; + float offset = ranger._offset[state.instance]; + RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)ranger._function[state.instance].get(); + int16_t max_distance_cm = ranger._max_distance_cm[state.instance]; - /* first convert to volts */ - float v = voltage(); - float dist_m = 0; - - switch ((AP_RangeFinder_analog::RangeFinder_Function)_function.get()) { - case FUNCTION_LINEAR: - dist_m = (v - _offset) * _scaling; - break; + switch (function) { + case RangeFinder::FUNCTION_LINEAR: + dist_m = (v - offset) * scaling; + break; - case FUNCTION_INVERTED: - dist_m = (_offset - v) * _scaling; - break; + case RangeFinder::FUNCTION_INVERTED: + dist_m = (offset - v) * scaling; + break; - case FUNCTION_HYPERBOLA: - if (v <= _offset) { - dist_m = 0; - } - dist_m = _scaling / (v - _offset); - if (isinf(dist_m) || dist_m > _max_distance_cm) { - dist_m = _max_distance_cm * 0.01; - } - break; - } - if (dist_m < 0) { - dist_m = 0; - } - return dist_m * 100.0f; + case RangeFinder::FUNCTION_HYPERBOLA: + if (v <= offset) { + dist_m = 0; + } + dist_m = scaling / (v - offset); + if (isinf(dist_m) || dist_m > max_distance_cm) { + dist_m = max_distance_cm * 0.01; + } + break; + } + if (dist_m < 0) { + dist_m = 0; + } + state.distance_cm = dist_m * 100.0f; + + // we can't actually tell if an analog rangefinder is healthy, so + // always set as healthy + state.healthy = true; } -/* - return true if we are in the configured range of the device - */ -bool AP_RangeFinder_analog::in_range(void) -{ - if (!_enabled) { - return false; - } - float dist_cm = distance_cm(); - if (dist_cm >= _max_distance_cm) { - return false; - } - if (dist_cm <= _min_distance_cm) { - return false; - } - return true; -} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h index 9eafbcea9f..2f2b2484fe 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h @@ -1,51 +1,28 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#ifndef __AP_RangeFinder_analog_H__ -#define __AP_RangeFinder_analog_H__ +#ifndef __AP_RANGEFINDER_ANALOG_H__ +#define __AP_RANGEFINDER_ANALOG_H__ #include "RangeFinder.h" -#include +#include "RangeFinder_Backend.h" -class AP_RangeFinder_analog +class AP_RangeFinder_analog : public AP_RangeFinder_Backend { public: // constructor - AP_RangeFinder_analog(); + AP_RangeFinder_analog(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); - // initialise the rangefinder - void Init(void *adc); - - // return distance in centimeters - float distance_cm(void); + // static detection function + static bool detect(RangeFinder &ranger, uint8_t instance); - // return raw voltage. Used for calibration - float voltage(void); - - // return true if the sonar is in the configured range - bool in_range(void); - - // return true if enabled - bool enabled(void) { return (bool)_enabled.get(); } - - enum RangeFinder_Function { - FUNCTION_LINEAR = 0, - FUNCTION_INVERTED = 1, - FUNCTION_HYPERBOLA = 2 - }; - - static const struct AP_Param::GroupInfo var_info[]; + // update state + void update(void); private: - AP_HAL::AnalogSource *_source; - AP_Int8 _pin; - 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 _enabled; -}; -#endif // __AP_RangeFinder_analog_H__ + // update raw voltage + void update_voltage(void); + + AP_HAL::AnalogSource *source; +}; +#endif // __AP_RANGEFINDER_ANALOG_H__