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

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2968 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/ModeFilter/ModeFilter.cpp

@@ -0,0 +1,83 @@
+/*
+	ModeFilter.cpp - Mode Filter Library for Ardupilot Mega. Arduino
+	Code by Jason Short. DIYDrones.com
+	Adapted from code by Jason Lessels(June 6, 2011), Bill Gentles (Nov. 12, 2010)
+
+
+	This library is free software; you can redistribute it and/or
+	modify it under the terms of the GNU Lesser General Public
+	License as published by the Free Software Foundation; either
+	version 2.1 of the License, or (at your option) any later version.
+
+
+*/
+#include "ModeFilter.h"
+
+#include <avr/interrupt.h>
+#include "WProgram.h"
+
+
+// Constructors ////////////////////////////////////////////////////////////////
+
+ModeFilter::ModeFilter() :
+	_sample_index(0)
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+	//Sorting function
+	// sort function (Author: Bill Gentles, Nov. 12, 2010)
+	//	*a is an array pointer function
+
+int ModeFilter::get_filtered_with_sample(int _sample){
+	_samples[_sample_index] = _sample;
+
+	_sample_index++;
+
+	if (_sample_index >= MOD_FILTER_SIZE)
+		_sample_index = 0;
+
+	isort();
+
+	return mode();
+}
+
+
+void ModeFilter::isort()
+{
+	for (int i = 1; i < MOD_FILTER_SIZE; ++i)   {
+		int j = _samples[i];
+		int k;
+		for (k = i - 1; (k >= 0) && (j < _samples[k]); k--){
+			_samples[k + 1] = _samples[k];
+		}
+		_samples[k + 1] = j;
+	}
+}
+
+//Mode function, returning the mode or median.
+int ModeFilter::mode(){
+	int mode 		= 0;
+	byte i 			= 0;
+	byte count 		= 0;
+	byte maxCount 	= 0;
+	byte bimodal 	= 0;
+
+	while(count > maxCount){
+		mode 		= _samples[i];
+		maxCount 	= count;
+		bimodal 	= 0;
+	}
+
+	if(count == 0) i++;
+
+	if(count == maxCount){ //If the dataset has 2 or more modes.
+		bimodal = 1;
+	}
+
+	if(mode == 0 || bimodal == 1){ //Return the median if there is no mode.
+		mode = _samples[(MOD_FILTER_SIZE / 2)];
+	}
+
+	return mode;
+}

libraries/ModeFilter/ModeFilter.h

@@ -0,0 +1,26 @@
+#ifndef ModeFilter_h
+#define ModeFilter_h
+
+#define MOD_FILTER_SIZE 6
+
+#include <inttypes.h>
+
+class ModeFilter
+{
+  private:
+  public:
+	ModeFilter();
+
+	int 		get_filtered_with_sample(int _sample);
+	int16_t 	_samples[MOD_FILTER_SIZE];
+
+  private:
+	void 		isort();
+	int16_t		mode();
+	int8_t		_sample_index;
+};
+
+#endif
+
+
+

libraries/ModeFilter/examples/ModeFilter/Makefile

@@ -0,0 +1,2 @@
+BOARD	=	mega
+include ../../../AP_Common/Arduino.mk

libraries/ModeFilter/examples/ModeFilter/ModeFilter.pde

@@ -0,0 +1,51 @@
+/*
+	Example of APM_RC library.
+	Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
+
+	Print Input values and send Output to the servos
+	(Works with last PPM_encoder firmware)
+*/
+
+#include <ModeFilter.h> // ArduPilot Mega RC Library
+
+int rangevalue[] = {31000, 31000, 50, 55, 60, 55, 10, 0, 31000};
+
+ModeFilter mfilter;
+byte i = 0;
+
+void setup()
+{
+	//Open up a serial connection
+	Serial.begin(115200);
+	//Wait for the serial connection
+	delay(500);
+}
+
+//Main loop where the action takes place
+void loop()
+{
+	while(i < 9){
+		printArray(mfilter._samples, 6);
+		int modE = mfilter.get_filtered_with_sample(rangevalue[i]);
+		i++;
+
+		Serial.print("The mode/median is: ");
+		Serial.print(modE);
+		Serial.println();
+	}
+	delay(100000);
+}
+
+/*-----------Functions------------*/
+//Function to print the arrays.
+void printArray(int *a, int n)
+{
+	for (int i = 0; i < n; i++)
+	{
+		Serial.print(a[i], DEC);
+		Serial.print(' ');
+	}
+
+	Serial.println();
+}
+

libraries/ModeFilter/keywords.txt

@@ -0,0 +1,8 @@
+APM_RC	KEYWORD1
+begin	KEYWORD2
+InputCh	KEYWORD2
+OutputCh	KEYWORD2
+GetState	KEYWORD2
+Force_Out0_Out1	KEYWORD2
+Force_Out2_Out3	KEYWORD2
+Force_Out6_Out7	KEYWORD2