Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: converted analog rangefinder backend to new API

Browse Source 
this gets analog rangefinders of all types working
This commit is contained in:
Andrew Tridgell 2014-06-27 12:57:28 +10:00
parent cb037f3416
commit 92b76b4be4
2 changed files with 89 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

219
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -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)
{
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]);
}
/*
return raw voltage
*/
float AP_RangeFinder_analog::voltage(void)
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 (!_enabled) {
return 0.0f;
}
if (_source == NULL) {
return 0.0f;
if (_ranger._pin[instance] != -1) {
return true;
}
return false;
}
/*
update raw voltage state
*/
void AP_RangeFinder_analog::update_voltage(void)
{
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 (function) {
case RangeFinder::FUNCTION_LINEAR:
dist_m = (v - offset) * scaling;
break;
switch ((AP_RangeFinder_analog::RangeFinder_Function)_function.get()) {
case FUNCTION_LINEAR:
dist_m = (v - _offset) * _scaling;
break;
case RangeFinder::FUNCTION_INVERTED:
dist_m = (offset - v) * scaling;
break;
case 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;
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;
// 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;
}

53
libraries/AP_RangeFinder/AP_RangeFinder_analog.h
View File

@ -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);
// static detection function
static bool detect(RangeFinder &ranger, uint8_t instance);
// return distance in centimeters
float distance_cm(void);
// 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__