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

View File

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

View File

@ -7,9 +7,11 @@
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645 #define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
class AP_RangeFinder_MaxsonarLV : public RangeFinder class AP_RangeFinder_MaxsonarLV : public RangeFinder
{ {
public: 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 int convert_raw_to_distance(int _raw_value) { return _raw_value * 2.54; } // read value from analog port and return distance in cm
}; };
#endif #endif

View File

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

View File

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

View File

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

View File

@ -7,10 +7,11 @@
#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150 #define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150
class AP_RangeFinder_SharpGP2Y : public RangeFinder class AP_RangeFinder_SharpGP2Y : public RangeFinder
{ {
public: 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 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 #endif

View File

@ -17,40 +17,13 @@
#include "RangeFinder.h" #include "RangeFinder.h"
// Constructor /////////////////////////////////////////////////////////////////
RangeFinder::RangeFinder() :
_ap_adc(NULL),
_num_averages(AP_RANGEFINDER_NUM_AVERAGES),
_history_ptr(0)
{
}
// Public Methods ////////////////////////////////////////////////////////////// // 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 // store the analog port to be used
_analogPort = analogPort; _analogPort = analogPort;
pinMode(analogPort, INPUT);
// 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) 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 // Read Sensor data - only the raw_value is filled in by this parent class
int RangeFinder::read() int RangeFinder::read()
{ {
// local variables
int temp_dist;
int total = 0;
// read from the analog port or pitot tube // read from the analog port or pitot tube
if( _analogPort == AP_RANGEFINDER_PITOT_TUBE ) { if( _ap_adc != NULL ){
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
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{ }else{
// read raw sensor value and convert to distance // read raw sensor value and convert to distance
raw_value = analogRead(_analogPort); raw_value = analogRead(_analogPort);
@ -81,45 +46,11 @@ int RangeFinder::read()
// convert analog value to distance in cm (using child implementation most likely) // convert analog value to distance in cm (using child implementation most likely)
raw_value = convert_raw_to_distance(raw_value); raw_value = convert_raw_to_distance(raw_value);
// ensure distance is within min and max // ensure distance is within min and max
raw_value = constrain(raw_value, min_distance, max_distance); raw_value = constrain(raw_value, min_distance, max_distance);
//distance = raw_value; distance = _mode_filter->get_filtered_with_sample(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
return distance; return distance;
} }

View File

@ -4,7 +4,9 @@
#include <stdlib.h> #include <stdlib.h>
#include <inttypes.h> #include <inttypes.h>
#include "../AP_ADC/AP_ADC.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_FRONT 0, 10, 0
#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0 #define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0
#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 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_RIGHT -5, -5, 0
#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0 #define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0
#define AP_RANGEFINDER_ORIENTATION_FRONT_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_PITOT_TUBE_ADC_CHANNEL 7
#define AP_RANGEFINDER_NUM_AVERAGES 4 //#define AP_RANGEFINDER_NUM_AVERAGES 4
class RangeFinder class RangeFinder
{ {
protected:
//GPS(Stream *s) : _port(s) {};
RangeFinder(AP_ADC *adc, ModeFilter *filter) :
_ap_adc(adc),
_mode_filter(filter),
_analogPort(-1)
{}
public: public:
RangeFinder(); // constructor
int _analogPort; // the port to which the sensor is connected //int _history[AP_RANGEFINDER_NUM_AVERAGES]; // history of recent distances used for filtering
AP_ADC *_ap_adc; // pointer to AP_ADC used for pitot tube //int _num_averages; // filter will return average of this many historic values (must be < AP_RANGEFINDER_NUM_AVERAGES)
int _history[AP_RANGEFINDER_NUM_AVERAGES]; // history of recent distances used for filtering //int _history_ptr; // pointer to the most recent entry in the history table
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 raw_value; // raw value from the sensor int raw_value; // raw value from the sensor
int distance; // distance in cm int distance; // distance in cm
int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor 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 min_distance; // minimum measurable distance (in cm) - should be set in child's constructor
int orientation_x, orientation_y, orientation_z; 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_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 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 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 #endif