rmackay9
13 years ago
5 changed files with 0 additions and 174 deletions
@ -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 @@ |
|||||||
#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 @@ |
|||||||
BOARD = mega
|
|
||||||
include ../../../AP_Common/Arduino.mk |
|
@ -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