You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
83 lines
1.8 KiB
83 lines
1.8 KiB
/* |
|
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. |
|
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; |
|
}
|
|
|