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

AP_RangeFinder: converted analog rangefinder backend to new API 

this gets analog rangefinders of all types working
master
Andrew Tridgell 11 years ago
parent
cb037f3416
commit
92b76b4be4
2 changed files with 87 additions and 181 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. 217
      libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
  2. 51
      libraries/AP_RangeFinder/AP_RangeFinder_analog.h

217
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
Unescape View File

@ -20,171 +20,100 @@ @@ -20,171 +20,100 @@
*/
#include <AP_HAL.h>
#include <AP_Common.h>
#include <AP_Math.h>
#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)
/*
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
*/
AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
{
AP_Param::setup_object_defaults(this, var_info);
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]);
}
/* 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
/*
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
*/
void AP_RangeFinder_analog::Init(void *adc)
bool AP_RangeFinder_analog::detect(RangeFinder &_ranger, uint8_t instance)
{
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);
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;
}
/* 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;
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];
switch (function) {
case RangeFinder::FUNCTION_LINEAR:
dist_m = (v - offset) * scaling;
break;
case 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_INVERTED:
dist_m = (offset - v) * scaling;
break;
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;
}

51
libraries/AP_RangeFinder/AP_RangeFinder_analog.h
Unescape View File

@ -1,51 +1,28 @@ @@ -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 <Filter.h>
#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;
// update raw voltage
void update_voltage(void);
AP_HAL::AnalogSource *source;
};
#endif // __AP_RangeFinder_analog_H__
#endif // __AP_RANGEFINDER_ANALOG_H__

Write Preview
Loading…
Cancel
Save