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

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;

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

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;

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

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

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

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

libraries/AP_RangeFinder/RangeFinder.h

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