mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
RangeFinder: rework to use AnalogSource library
this removes the hacks that check for the ADC object, and instead choose the method of getting the analog sonar value via the AnalogSource API
This commit is contained in:
parent
0c0a1b1dcc
commit
f36ded2854
@ -31,8 +31,9 @@
|
||||
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
|
||||
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_ADC *adc, ModeFilter *filter) :
|
||||
RangeFinder(adc, filter)
|
||||
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source,
|
||||
ModeFilter *filter) :
|
||||
RangeFinder(source, filter)
|
||||
{
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
|
||||
|
@ -11,7 +11,7 @@ class AP_RangeFinder_MaxsonarXL : public RangeFinder
|
||||
// public:
|
||||
//AP_GPS_MTK(Stream *s);
|
||||
public:
|
||||
AP_RangeFinder_MaxsonarXL(AP_ADC *adc, ModeFilter *filter);
|
||||
AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
|
||||
|
||||
int convert_raw_to_distance(int _raw_value) { return _raw_value; } // read value from analog port and return distance in cm
|
||||
};
|
||||
|
@ -31,8 +31,9 @@
|
||||
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
|
||||
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_ADC *adc, ModeFilter *filter) :
|
||||
RangeFinder(adc, filter)
|
||||
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source,
|
||||
ModeFilter *filter) :
|
||||
RangeFinder(source, filter)
|
||||
{
|
||||
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE;
|
||||
|
@ -9,7 +9,7 @@
|
||||
class AP_RangeFinder_SharpGP2Y : public RangeFinder
|
||||
{
|
||||
public:
|
||||
AP_RangeFinder_SharpGP2Y(AP_ADC *adc, ModeFilter *filter);
|
||||
AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter);
|
||||
int convert_raw_to_distance(int _raw_value) { if( _raw_value == 0 ) return max_distance; else return 14500/_raw_value; } // read value from analog port and return distance in cm
|
||||
|
||||
};
|
||||
|
@ -19,12 +19,6 @@
|
||||
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void RangeFinder::set_analog_port(int analogPort)
|
||||
{
|
||||
// store the analog port to be used
|
||||
_analogPort = analogPort;
|
||||
pinMode(analogPort, INPUT);
|
||||
}
|
||||
|
||||
void RangeFinder::set_orientation(int x, int y, int z)
|
||||
{
|
||||
@ -36,14 +30,9 @@ void RangeFinder::set_orientation(int x, int y, int z)
|
||||
// Read Sensor data - only the raw_value is filled in by this parent class
|
||||
int RangeFinder::read()
|
||||
{
|
||||
// read from the analog port or pitot tube
|
||||
if( _ap_adc != NULL ){
|
||||
// values from ADC are twice as big as you'd expect
|
||||
raw_value = _ap_adc->Ch(AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL) >> 2;
|
||||
}else{
|
||||
// read raw sensor value and convert to distance
|
||||
raw_value = analogRead(_analogPort);
|
||||
}
|
||||
raw_value = _analog_source->read();
|
||||
|
||||
raw_value = convert_raw_to_distance(raw_value);
|
||||
|
||||
// convert analog value to distance in cm (using child implementation most likely)
|
||||
raw_value = convert_raw_to_distance(raw_value);
|
||||
|
@ -3,7 +3,7 @@
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <inttypes.h>
|
||||
#include "../AP_ADC/AP_ADC.h"
|
||||
#include "../AP_AnalogSource/AP_AnalogSource.h"
|
||||
#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library
|
||||
|
||||
/*
|
||||
@ -19,7 +19,6 @@
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0
|
||||
*/
|
||||
//#define AP_RANGEFINDER_PITOT_TUBE 1007
|
||||
#define AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL 7
|
||||
|
||||
//#define AP_RANGEFINDER_NUM_AVERAGES 4
|
||||
|
||||
@ -27,10 +26,9 @@ class RangeFinder
|
||||
{
|
||||
protected:
|
||||
//GPS(Stream *s) : _port(s) {};
|
||||
RangeFinder(AP_ADC *adc, ModeFilter *filter) :
|
||||
_ap_adc(adc),
|
||||
_mode_filter(filter),
|
||||
_analogPort(-1)
|
||||
RangeFinder(AP_AnalogSource * source, ModeFilter *filter) :
|
||||
_analog_source(source),
|
||||
_mode_filter(filter)
|
||||
{}
|
||||
public:
|
||||
|
||||
@ -40,13 +38,11 @@ class RangeFinder
|
||||
int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor
|
||||
int orientation_x, orientation_y, orientation_z;
|
||||
|
||||
virtual void set_analog_port(int analogPort);
|
||||
virtual void set_orientation(int x, int y, int z);
|
||||
virtual int convert_raw_to_distance(int _raw_value) { return _raw_value; } // function that each child class should override to convert voltage to distance
|
||||
virtual int read(); // read value from sensor and return distance in cm
|
||||
|
||||
int _analogPort; // the port to which the sensor is connected
|
||||
AP_ADC *_ap_adc; // pointer to AP_ADC used for pitot tube
|
||||
AP_AnalogSource *_analog_source;
|
||||
ModeFilter *_mode_filter;
|
||||
};
|
||||
#endif
|
||||
|
@ -5,6 +5,7 @@
|
||||
|
||||
#include <AP_RangeFinder.h> // Range finder library
|
||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
#include <AP_AnalogSource.h>
|
||||
#include <ModeFilter.h> // mode filter
|
||||
|
||||
#define RF_PIN AP_RANGEFINDER_PITOT_TUBE // the pitot tube on the front of the oilpan
|
||||
@ -12,11 +13,12 @@
|
||||
|
||||
// declare global instances for reading pitot tube
|
||||
AP_ADC_ADS7844 adc;
|
||||
AP_AnalogSource_ADC adc_source(&adc, AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL, 0.25);
|
||||
ModeFilter mode_filter;
|
||||
|
||||
// create the range finder object
|
||||
//AP_RangeFinder_SharpGP2Y aRF(&adc, &mode_filter);
|
||||
AP_RangeFinder_MaxsonarXL aRF(&adc, &mode_filter);
|
||||
//AP_RangeFinder_SharpGP2Y aRF(&adc_source, &mode_filter);
|
||||
AP_RangeFinder_MaxsonarXL aRF(&adc_source, &mode_filter);
|
||||
|
||||
void setup()
|
||||
{
|
||||
|
@ -3,7 +3,6 @@ AP_RangeFinder KEYWORD1
|
||||
AP_RangeFinder_SharpGP2Y KEYWORD1
|
||||
AP_RangeFinder_MaxsonarXL KEYWORD1
|
||||
read KEYWORD2
|
||||
set_analog_port KEYWORD2
|
||||
set_orientation KEYWORD2
|
||||
convert_raw_to_distance KEYWORD2
|
||||
raw_value KEYWORD2
|
||||
|
Loading…
Reference in New Issue
Block a user