b1df1d1843
mode is a member of the class already, so call the local variable fmode git-svn-id: https://arducopter.googlecode.com/svn/trunk@3081 f9c3cf11-9bcb-44bc-f272-b75c42450872
84 lines
1.8 KiB
C++
84 lines
1.8 KiB
C++
/*
|
|
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 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;
|
|
}
|