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

Reworked the filtering algorithm based on Maxbotics recommendations to use a Mode filter 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2965 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-07-30 20:35:36 +00:00
parent 39677e7e66
commit ebf1697158
8 changed files with 80 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/*
AP_RangeFinder_MaxsonarLV.cpp - Arduino Library for Maxbotix's LV-MaxSonar
AP_RangeFinder_MaxsonarLV.cpp - Arduino Library for Maxbotix's LV-MaxSonar
Sonic proximity sensor
Code by Jose Julio and Randy Mackay. DIYDrones.com
@ -11,19 +11,19 @@
Sparkfun URL: http://www.sparkfun.com/products/8502
datasheet: http://www.maxbotix.com/uploads/LV-MaxSonar-EZ0-Datasheet.pdf
Sensor should be connected to one of the analog ports
Variables:
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)
Methods:
init(int analogPort) : Initialization of sensor
read() : read value from analog port and returns the distance (in cm)
*/
// AVR LibC Includes
@ -31,7 +31,14 @@
#include "AP_RangeFinder_MaxsonarLV.h"
// Constructor //////////////////////////////////////////////////////////////
AP_RangeFinder_MaxsonarLV::AP_RangeFinder_MaxsonarLV()
/*AP_RangeFinder_MaxsonarLV::AP_RangeFinder_MaxsonarLV()
{
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
}
*/
AP_RangeFinder_MaxsonarLV::AP_RangeFinder_MaxsonarLV(AP_ADC *adc, ModeFilter *filter) :
RangeFinder(adc, filter)
{
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;

6
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.h
View File

@ -7,9 +7,11 @@
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
class AP_RangeFinder_MaxsonarLV : public RangeFinder
{
{
public:
AP_RangeFinder_MaxsonarLV();
//AP_RangeFinder_MaxsonarLV();
AP_RangeFinder_MaxsonarLV(AP_ADC *adc, ModeFilter *filter);
int convert_raw_to_distance(int _raw_value) { return _raw_value * 2.54; } // read value from analog port and return distance in cm
};
#endif

17
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/*
AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
infrared proximity sensor
Code by Jose Julio and Randy Mackay. DIYDrones.com
@ -11,19 +11,19 @@
Sparkfun URL: http://www.sparkfun.com/products/9491
datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf
Sensor should be connected to one of the analog ports
Variables:
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)
Methods:
init(int analogPort) : Initialization of sensor
read() : read value from analog port and returns the distance (in cm)
*/
// AVR LibC Includes
@ -31,7 +31,12 @@
#include "AP_RangeFinder_MaxsonarXL.h"
// Constructor //////////////////////////////////////////////////////////////
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL()
//AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
//{
//}
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_ADC *adc, ModeFilter *filter) :
RangeFinder(adc, filter)
{
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;

7
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
View File

@ -7,9 +7,12 @@
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 700
class AP_RangeFinder_MaxsonarXL : public RangeFinder
{
{
// public:
//AP_GPS_MTK(Stream *s);
public:
AP_RangeFinder_MaxsonarXL();
AP_RangeFinder_MaxsonarXL(AP_ADC *adc, ModeFilter *filter);
int convert_raw_to_distance(int _raw_value) { return _raw_value; } // read value from analog port and return distance in cm
};
#endif

20
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/*
AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
infrared proximity sensor
Code by Jose Julio and Randy Mackay. DIYDrones.com
@ -10,20 +10,20 @@
version 2.1 of the License, or (at your option) any later version.
Sensor should be conected to one of the analog ports
Sparkfun URL: http://www.sparkfun.com/products/8958
datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf
Variables:
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)
Methods:
init(int analogPort) : Initialization of sensor
read() : read value from analog port
read() : read value from analog port
*/
// AVR LibC Includes
@ -31,10 +31,18 @@
#include "AP_RangeFinder_SharpGP2Y.h"
// Constructor //////////////////////////////////////////////////////////////
/*
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y()
{
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE;
}
*/
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_ADC *adc, ModeFilter *filter) :
RangeFinder(adc, filter)
{
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE;
}
// Public Methods //////////////////////////////////////////////////////////////

7
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
View File

@ -7,10 +7,11 @@
#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150
class AP_RangeFinder_SharpGP2Y : public RangeFinder
{
{
public:
AP_RangeFinder_SharpGP2Y();
AP_RangeFinder_SharpGP2Y(AP_ADC *adc, ModeFilter *filter);
//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

81
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -17,40 +17,13 @@
#include "RangeFinder.h"
// Constructor /////////////////////////////////////////////////////////////////
RangeFinder::RangeFinder() :
_ap_adc(NULL),
_num_averages(AP_RANGEFINDER_NUM_AVERAGES),
_history_ptr(0)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void RangeFinder::init(int analogPort, AP_ADC *ap_adc)
void RangeFinder::set_analog_port(int analogPort)
{
// 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;
pinMode(analogPort, INPUT);
}
void RangeFinder::set_orientation(int x, int y, int z)
@ -63,17 +36,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()
{
// local variables
int temp_dist;
int total = 0;
// 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;
}
if( _ap_adc != NULL ){
raw_value = _ap_adc->Ch_raw(AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL) >> 2; // values from ADC are twice as big as you'd expect
}else{
// read raw sensor value and convert to distance
raw_value = analogRead(_analogPort);
@ -81,45 +46,11 @@ int RangeFinder::read()
// convert analog value to distance in cm (using child implementation most likely)
raw_value = convert_raw_to_distance(raw_value);
// ensure distance is within min and max
raw_value = constrain(raw_value, min_distance, max_distance);
//distance = raw_value;
// slew rate
if(raw_value > distance){
temp_dist = min(distance + 20, raw_value);
}else{
temp_dist = max(distance - 20, raw_value);
}
if(_num_averages > 0){
// filter the results
// ------------------
// add to filter
_history[_history_ptr] = temp_dist;
// increment our filter
_history_ptr++;
// loop our filter
if(_history_ptr == AP_RANGEFINDER_NUM_AVERAGES)
_history_ptr = 0;
// sum our filter
for(uint8_t i = 0; i < AP_RANGEFINDER_NUM_AVERAGES; i++){
total += _history[i];
}
// average our sampels
distance = total / AP_RANGEFINDER_NUM_AVERAGES;
}else{
distance = temp_dist;
}
// return distance
distance = _mode_filter->get_filtered_with_sample(raw_value);
return distance;
}

35
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -4,7 +4,9 @@
#include <stdlib.h>
#include <inttypes.h>
#include "../AP_ADC/AP_ADC.h"
#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library
/*
#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0
#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0
#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0
@ -15,32 +17,41 @@
#define AP_RANGEFINDER_ORIENTATION_BACK_RIGHT -5, -5, 0
#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 1007
#define AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL 7
#define AP_RANGEFINDER_NUM_AVERAGES 4
//#define AP_RANGEFINDER_NUM_AVERAGES 4
class RangeFinder
{
protected:
//GPS(Stream *s) : _port(s) {};
RangeFinder(AP_ADC *adc, ModeFilter *filter) :
_ap_adc(adc),
_mode_filter(filter),
_analogPort(-1)
{}
public:
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[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)
int _history_ptr; // pointer to the most recent entry in the history table
public:
//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)
//int _history_ptr; // pointer to the most recent entry in the history table
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;
virtual void init(int analogPort, AP_ADC *adc = (AP_ADC*)NULL);
virtual void set_analog_port(int analogPort);
virtual void set_orientation(int x, int y, int z);
virtual void set_filter(int num_averages) { _num_averages = num_averages; } // allows control of amount of filtering done
//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
int _analogPort; // the port to which the sensor is connected
AP_ADC *_ap_adc; // pointer to AP_ADC used for pitot tube
ModeFilter *_mode_filter;
};
#endif