mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder - changes to allow sonar to work using pitot tube. Also moved most functionality into RangeFinder class instead of child classes.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1404 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
45f8112244
commit
95674b8518
|
@ -30,38 +30,13 @@
|
|||
#include "WConstants.h"
|
||||
#include "AP_RangeFinder_MaxsonarLV.h"
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void AP_RangeFinder_MaxsonarLV::init(int analogPort)
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
AP_RangeFinder_MaxsonarLV::AP_RangeFinder_MaxsonarLV()
|
||||
{
|
||||
// local variables
|
||||
int i;
|
||||
|
||||
// set the given analog port to an input
|
||||
pinMode(analogPort, INPUT);
|
||||
|
||||
// initialise everything
|
||||
_analogPort = analogPort;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
|
||||
|
||||
// make first call to read to get initial distance
|
||||
read();
|
||||
|
||||
// initialise history
|
||||
for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
|
||||
_history[i] = distance;
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
int AP_RangeFinder_MaxsonarLV::read()
|
||||
{
|
||||
// read raw sensor value and convert to distance
|
||||
raw_value = analogRead(_analogPort);
|
||||
|
||||
// for this sensor, the sensor value is in inches, need to convert to cm
|
||||
distance = raw_value * 2.54;
|
||||
distance = constrain(distance,min_distance,max_distance); // converts from inches to cm
|
||||
|
||||
// return distance
|
||||
return filter(distance);
|
||||
}
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
|
|
@ -9,7 +9,7 @@
|
|||
class AP_RangeFinder_MaxsonarLV : public RangeFinder
|
||||
{
|
||||
public:
|
||||
void init(int analogPort);
|
||||
int read(); // read value from analog port and return distance in cm
|
||||
AP_RangeFinder_MaxsonarLV();
|
||||
int convert_raw_to_distance(int raw_value) { return raw_value * 2.54; } // read value from analog port and return distance in cm
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -30,37 +30,11 @@
|
|||
#include "WConstants.h"
|
||||
#include "AP_RangeFinder_MaxsonarXL.h"
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void AP_RangeFinder_MaxsonarXL::init(int analogPort)
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL()
|
||||
{
|
||||
// local variables
|
||||
int i;
|
||||
|
||||
// set the given analog port to an input
|
||||
pinMode(analogPort, INPUT);
|
||||
|
||||
// initialise everything
|
||||
_analogPort = analogPort;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
|
||||
|
||||
// make first call to read to get initial distance
|
||||
read();
|
||||
|
||||
// initialise history
|
||||
for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
|
||||
_history[i] = distance;
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
int AP_RangeFinder_MaxsonarXL::read()
|
||||
{
|
||||
// read raw sensor value and convert to distance
|
||||
raw_value = analogRead(_analogPort);
|
||||
|
||||
// for this sensor, the sensor value is the distance in cm! nice and easy!
|
||||
distance = constrain(raw_value,min_distance,max_distance);
|
||||
|
||||
// return distance
|
||||
return filter(distance);
|
||||
}
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
|
|
@ -9,7 +9,7 @@
|
|||
class AP_RangeFinder_MaxsonarXL : public RangeFinder
|
||||
{
|
||||
public:
|
||||
void init(int analogPort);
|
||||
int read(); // read value from analog port and return distance in cm
|
||||
AP_RangeFinder_MaxsonarXL();
|
||||
int convert_raw_to_distance(int raw_value) { return raw_value; } // read value from analog port and return distance in cm
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -30,38 +30,11 @@
|
|||
#include "WConstants.h"
|
||||
#include "AP_RangeFinder_SharpGP2Y.h"
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void AP_RangeFinder_SharpGP2Y::init(int analogPort)
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y()
|
||||
{
|
||||
// local variables
|
||||
int i;
|
||||
|
||||
// set the given analog port to an input
|
||||
pinMode(analogPort, INPUT);
|
||||
|
||||
// initialise everything
|
||||
_analogPort = analogPort;
|
||||
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE;
|
||||
|
||||
// make first call to read to get initial distance
|
||||
read();
|
||||
|
||||
// initialise history
|
||||
for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
|
||||
_history[i] = distance;
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
int AP_RangeFinder_SharpGP2Y::read()
|
||||
{
|
||||
// read raw sensor value and convert to distance
|
||||
raw_value = analogRead(_analogPort);
|
||||
if( raw_value == 0 )
|
||||
distance = max_distance;
|
||||
else
|
||||
distance = constrain(14500/raw_value,min_distance,max_distance);
|
||||
|
||||
// return distance
|
||||
return filter(distance);
|
||||
}
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
|
|
@ -9,7 +9,8 @@
|
|||
class AP_RangeFinder_SharpGP2Y : public RangeFinder
|
||||
{
|
||||
public:
|
||||
void init(int analogPort);
|
||||
int read(); // read value from analog port and return distance in cm
|
||||
AP_RangeFinder_SharpGP2Y();
|
||||
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
|
||||
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -16,7 +16,40 @@
|
|||
#include "WConstants.h"
|
||||
#include "RangeFinder.h"
|
||||
|
||||
// Constructor /////////////////////////////////////////////////////////////////
|
||||
RangeFinder::RangeFinder() : _num_averages(AP_RANGEFINDER_NUM_AVERAGES), _ap_adc(NULL)
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void RangeFinder::init(int analogPort, AP_ADC *ap_adc)
|
||||
{
|
||||
// local variables
|
||||
int i;
|
||||
|
||||
// store the analog port to be used
|
||||
_analogPort = analogPort;
|
||||
|
||||
// set the given analog port to an input
|
||||
if( analogPort != AP_RANGEFINDER_PITOT_TUBE )
|
||||
{
|
||||
pinMode(analogPort, INPUT);
|
||||
}else{
|
||||
_num_averages = 0; // turn off averaging for pitot tube because AP_ADC does this for us
|
||||
}
|
||||
|
||||
// capture the AP_ADC object if passed in
|
||||
if( ap_adc != NULL )
|
||||
_ap_adc = ap_adc;
|
||||
|
||||
// make first call to read to get initial distance
|
||||
read();
|
||||
|
||||
// initialise history
|
||||
for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
|
||||
_history[i] = distance;
|
||||
}
|
||||
|
||||
void RangeFinder::set_orientation(int x, int y, int z)
|
||||
{
|
||||
orientation_x = x;
|
||||
|
@ -24,14 +57,41 @@ void RangeFinder::set_orientation(int x, int y, int z)
|
|||
orientation_z = z;
|
||||
}
|
||||
|
||||
// Protected Methods //////////////////////////////////////////////////////////
|
||||
int RangeFinder::filter(int latestValue)
|
||||
// Read Sensor data - only the raw_value is filled in by this parent class
|
||||
int RangeFinder::read()
|
||||
{
|
||||
int i;
|
||||
int total = 0;
|
||||
_history_ptr = (_history_ptr + 1) % AP_RANGEFINDER_NUM_AVERAGES;
|
||||
_history[_history_ptr] = latestValue;
|
||||
for(i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
|
||||
total += _history[i];
|
||||
return total / AP_RANGEFINDER_NUM_AVERAGES;
|
||||
}
|
||||
// local variables
|
||||
int temp_dist;
|
||||
int total = 0;
|
||||
int i;
|
||||
|
||||
// read from the analog port or pitot tube
|
||||
if( _analogPort == AP_RANGEFINDER_PITOT_TUBE ) {
|
||||
if( _ap_adc != NULL )
|
||||
raw_value = _ap_adc->Ch(AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL) >> 2; // values from ADC are twice as big as you'd expect
|
||||
else
|
||||
raw_value = 0;
|
||||
}else{
|
||||
// read raw sensor value and convert to distance
|
||||
raw_value = analogRead(_analogPort);
|
||||
}
|
||||
|
||||
// convert analog value to distance in cm (using child implementation most likely)
|
||||
temp_dist = convert_raw_to_distance(raw_value);
|
||||
|
||||
// ensure distance is within min and max
|
||||
distance = constrain(temp_dist, min_distance, max_distance);
|
||||
|
||||
// filter the results
|
||||
if( _num_averages > 1 )
|
||||
{
|
||||
_history_ptr = (_history_ptr + 1) % _num_averages;
|
||||
_history[_history_ptr] = distance;
|
||||
for(i=0; i<_num_averages; i++ )
|
||||
total += _history[i];
|
||||
distance = total / _num_averages;
|
||||
}
|
||||
|
||||
// return distance
|
||||
return distance;
|
||||
}
|
|
@ -1,7 +1,9 @@
|
|||
#ifndef RangeFinder_h
|
||||
#define RangeFinder_h
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <inttypes.h>
|
||||
#include "../AP_ADC/AP_ADC.h"
|
||||
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0
|
||||
|
@ -14,25 +16,31 @@
|
|||
#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0
|
||||
#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
|
||||
|
||||
class RangeFinder
|
||||
{
|
||||
public:
|
||||
int _analogPort; // the port to which the sensor is connected
|
||||
int _history_ptr;
|
||||
int _history[AP_RANGEFINDER_NUM_AVERAGES];
|
||||
RangeFinder(); // constructor
|
||||
int _analogPort; // the port to which the sensor is connected
|
||||
AP_ADC *_ap_adc; // pointer to AP_ADC used for pitot tube
|
||||
int _history_ptr; // pointer to the most recent entry in the history table
|
||||
int _history[AP_RANGEFINDER_NUM_AVERAGES]; // history of recent distances used for filtering
|
||||
int _num_averages; // filter will return average of this many historic values (must be < AP_RANGEFINDER_NUM_AVERAGES)
|
||||
public:
|
||||
int raw_value; // raw value from the sensor
|
||||
int distance; // distance in cm
|
||||
int max_distance; // maximum measurable distance (in cm)
|
||||
int min_distance; // minimum measurable distance (in cm)
|
||||
int raw_value; // raw value from the sensor
|
||||
int distance; // distance in cm
|
||||
int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor
|
||||
int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor
|
||||
int orientation_x, orientation_y, orientation_z;
|
||||
|
||||
int filter(int latestValue); // returns the average of the last AP_RANGEFINDER_NUM_AVERAGES values
|
||||
|
||||
virtual void init(int analogPort);
|
||||
virtual void init(int analogPort, AP_ADC *adc = (AP_ADC*)NULL);
|
||||
virtual void set_orientation(int x, int y, int z);
|
||||
virtual int read(); // read value from analog port and return distance in cm
|
||||
virtual void set_filter(int num_averages) { _num_averages = num_averages; } // allows control of amount of filtering done
|
||||
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
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -4,19 +4,25 @@
|
|||
*/
|
||||
|
||||
#include <AP_RangeFinder.h> // Range finder library
|
||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
|
||||
#define RF_PIN A5 // the far back-right pin on the oilpan (near the CLI switch)
|
||||
#define RF_PIN AP_RANGEFINDER_PITOT_TUBE // the far back-right pin on the oilpan (near the CLI switch)
|
||||
//#define RF_PIN A5 // A5 is the far back-right pin on the oilpan (near the CLI switch)
|
||||
|
||||
// declare global instances for reading pitot tube
|
||||
AP_ADC_ADS7844 adc;
|
||||
|
||||
// create the range finder object
|
||||
//AP_RangeFinder_SharpGP2Y aRF;
|
||||
//AP_RangeFinder_MaxsonarXL aRF;
|
||||
AP_RangeFinder_MaxsonarLV aRF;
|
||||
AP_RangeFinder_MaxsonarXL aRF;
|
||||
//AP_RangeFinder_MaxsonarLV aRF;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial.println("Range Finder Test v1.0");
|
||||
aRF.init(RF_PIN);
|
||||
adc.Init(); // APM ADC library initialization
|
||||
aRF.init(RF_PIN, &adc);
|
||||
}
|
||||
|
||||
void loop()
|
||||
|
@ -27,6 +33,6 @@ void loop()
|
|||
Serial.print("\traw:");
|
||||
Serial.print(aRF.raw_value);
|
||||
Serial.println();
|
||||
delay(50);
|
||||
delay(20);
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue