rmackay9
13 years ago
5 changed files with 0 additions and 174 deletions
@ -1,87 +0,0 @@
@@ -1,87 +0,0 @@
|
||||
/*
|
||||
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> |
||||
#if defined(ARDUINO) && ARDUINO >= 100 |
||||
#include "Arduino.h" |
||||
#else |
||||
#include "WProgram.h" |
||||
#endif |
||||
|
||||
|
||||
// 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.
|
||||
int16_t ModeFilter::mode(){ |
||||
int fmode = 0; |
||||
byte i = 0; |
||||
byte count = 0; |
||||
byte maxCount = 0; |
||||
byte bimodal = 0; |
||||
|
||||
while(count > maxCount){ |
||||
fmode = _samples[i]; |
||||
maxCount = count; |
||||
bimodal = 0; |
||||
} |
||||
|
||||
if(count == 0) i++; |
||||
|
||||
if(count == maxCount){ //If the dataset has 2 or more modes.
|
||||
bimodal = 1; |
||||
} |
||||
|
||||
if(fmode == 0 || bimodal == 1){ //Return the median if there is no mode.
|
||||
fmode = _samples[(MOD_FILTER_SIZE / 2)]; |
||||
} |
||||
|
||||
return fmode; |
||||
} |
@ -1,26 +0,0 @@
@@ -1,26 +0,0 @@
|
||||
#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 |
||||
|
||||
|
||||
|
@ -1,2 +0,0 @@
@@ -1,2 +0,0 @@
|
||||
BOARD = mega
|
||||
include ../../../AP_Common/Arduino.mk |
@ -1,51 +0,0 @@
@@ -1,51 +0,0 @@
|
||||
/* |
||||
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(); |
||||
} |
||||
|
Loading…
Reference in new issue