From ebf1697158c3ad470207a146417c4abb71cb76df Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 30 Jul 2011 20:35:36 +0000 Subject: [PATCH] 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 --- .../AP_RangeFinder_MaxsonarLV.cpp | 19 +++-- .../AP_RangeFinder_MaxsonarLV.h | 6 +- .../AP_RangeFinder_MaxsonarXL.cpp | 17 ++-- .../AP_RangeFinder_MaxsonarXL.h | 7 +- .../AP_RangeFinder_SharpGP2Y.cpp | 20 +++-- .../AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h | 7 +- libraries/AP_RangeFinder/RangeFinder.cpp | 81 ++----------------- libraries/AP_RangeFinder/RangeFinder.h | 35 +++++--- 8 files changed, 80 insertions(+), 112 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.cpp index e45bce9d16..3ea14aab05 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.cpp @@ -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; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.h index 580091a101..f258895095 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarLV.h @@ -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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp index 3e1f26970d..36a740c072 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp @@ -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; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h index 9ee93f6674..c1f23b4a5d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h @@ -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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp index 5f01f84cc5..88ae5b6d9b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp @@ -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 ////////////////////////////////////////////////////////////// diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h index 7252a60553..ee6032f81b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h @@ -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 diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 921c4f12ba..4621a90be8 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -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; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 34b88c73ae..b22a67b14f 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -4,7 +4,9 @@ #include #include #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