Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
rmackay9@yahoo.com 2011-01-03 04:17:43 +00:00
parent 45f8112244
commit 95674b8518
9 changed files with 120 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.cpp
View File

@ -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);
// Public Methods //////////////////////////////////////////////////////////////
// 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);
}

4
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.h
View File

@ -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

34
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

@ -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 //////////////////////////////////////////////////////////////

4
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
View File

@ -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

33
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
View File

@ -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 //////////////////////////////////////////////////////////////

5
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
View File

@ -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

78
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -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;
}

30
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -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

16
libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde
View File

@ -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);
}