Incorporate Filters code into NuEVI repo for smoother setup.

NuEVI/FilterOnePole.cpp

@@ -0,0 +1,240 @@
+// Copyright 2014 Jonathan Driscoll
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "FilterOnePole.h"
+
+#include <Arduino.h>
+
+FilterOnePole::FilterOnePole( FILTER_TYPE ft, float fc, float initialValue ) {
+  setFilter( ft, fc, initialValue );
+}
+
+void FilterOnePole::setFilter( FILTER_TYPE ft, float fc, float initialValue ) {
+  FT = ft;
+  setFrequency( fc );
+
+  Y = initialValue;
+  Ylast = initialValue;
+  X = initialValue;
+
+  LastUS = micros();
+}
+
+float FilterOnePole::input( float inVal ) {
+  long time = micros();
+  ElapsedUS = float(time - LastUS);   // cast to float here, for math
+  LastUS = time;                      // update this now
+
+  // shift the data values
+  Ylast = Y;
+  X = inVal;                          // this is now the most recent input value
+
+  // filter value is controlled by a parameter called X
+  // tau is set by the user in microseconds, but must be converted to samples here
+  TauSamps = TauUS / ElapsedUS;
+
+  float ampFactor;
+#ifdef ARM_FLOAT
+  ampFactor = expf( -1.0 / TauSamps );     // this is 1 if called quickly
+#else
+  ampFactor = exp( -1.0 / TauSamps );      // this is 1 if called quickly
+#endif
+
+  Y = (1.0-ampFactor)*X + ampFactor*Ylast;     // set the new value
+
+  return output();
+}
+
+void FilterOnePole::setFrequency( float newFrequency ) {
+  setTau( 1.0/(TWO_PI*newFrequency ) ); // τ=1/ω
+}
+
+void FilterOnePole::setTau( float newTau ) {
+  TauUS = newTau * 1e6;
+}
+
+float FilterOnePole::output() {
+    // figure out which button to read
+  switch (FT) {
+    case LOWPASS:
+      // return the last value
+      return Y;
+      break;
+    case INTEGRATOR:
+      // using a lowpass, but normaize
+      return Y * (TauUS/1.0e6);
+      break;
+    case HIGHPASS:
+      // highpass is the _difference_
+      return X-Y;
+      break;
+    case DIFFERENTIATOR:
+      // like a highpass, but normalize
+      return (X-Y)/(TauUS/1.0e6);
+      break;
+    default:
+      // should never get to here, return 0 just in case
+      return 0;
+  }
+}
+
+void FilterOnePole::print() {
+  Serial.println("");
+  Serial.print(" Y: ");        Serial.print( Y );
+  Serial.print(" Ylast: ");      Serial.print( Ylast );
+  Serial.print(" X ");         Serial.print( X );
+  Serial.print(" ElapsedUS ");  Serial.print( ElapsedUS );
+  Serial.print(" TauSamps: ");  Serial.print( TauSamps );
+  //Serial.print(" ampFactor " );       Serial.print( ampFactor );
+  Serial.print(" TauUS: ");     Serial.print( TauUS );
+  Serial.println("");
+}
+
+void FilterOnePole::test() {
+  float tau = 10;
+  float updateInterval = 1;
+  float nextupdateTime = millis()*1e-3;
+
+  float inputValue = 0;
+  FilterOnePole hp( HIGHPASS, tau, inputValue );
+  FilterOnePole lp( LOWPASS, tau, inputValue );
+
+  while( true ) {
+    float now = millis()*1e-3;
+
+    // switch input values on a 20 second cycle
+    if( round(now/20.0)-(now/20.0) < 0 )
+    inputValue = 0;
+    else
+    inputValue = 100;
+
+    hp.input(inputValue);
+    lp.input(inputValue);
+
+    if( now > nextupdateTime ) {
+      nextupdateTime += updateInterval;
+
+      Serial.print("inputValue: "); Serial.print( inputValue );
+      Serial.print("\t high-passed: "); Serial.print( hp.output() );
+      Serial.print("\t low-passed: "); Serial.print( lp.output() );
+      Serial.println();
+    }
+  }
+}
+
+void FilterOnePole::setToNewValue( float newVal ) {
+  Y = Ylast = X = newVal;
+}
+
+
+// stuff for filter2 (lowpass only)
+// should be able to set a separate fall time as well
+FilterOnePoleCascade::FilterOnePoleCascade( float riseTime, float initialValue ) {
+  setRiseTime( riseTime );
+  setToNewValue( initialValue );
+}
+
+void FilterOnePoleCascade::setRiseTime( float riseTime ) {
+  float tauScale = 3.36;      // found emperically, by running test();
+
+  Pole1.setTau( riseTime / tauScale );
+  Pole2.setTau( riseTime / tauScale );
+}
+
+float FilterOnePoleCascade::input( float inVal  ) {
+  Pole2.input( Pole1.input( inVal ));
+  return output();
+}
+
+// clears out the values in the filter
+void FilterOnePoleCascade::setToNewValue( float newVal ) {
+  Pole1.setToNewValue( newVal );
+  Pole2.setToNewValue( newVal );
+}
+
+float FilterOnePoleCascade::output() {
+  return Pole2.output();
+}
+
+void FilterOnePoleCascade::test() {
+  // make a filter, how fast does it run:
+
+  float rise = 1.0;
+  FilterOnePoleCascade myFilter( rise );
+
+  // first, test the filter speed ...
+  long nLoops = 1000;
+
+  Serial.print( "testing filter with a rise time of ");
+  Serial.print( rise ); Serial.print( "s" );
+
+  Serial.print( "\n running filter speed loop ... ");
+
+  float startTime, stopTime;
+
+  startTime = millis()*1e-3;
+  for( long i=0; i<nLoops; ++i ) {
+    myFilter.input( PI );   // use pi, so it will actually do a full calculation
+  }
+  stopTime = millis()*1e-3;
+
+  Serial.print( "done, filter runs at " );
+  Serial.print( float(nLoops) / (stopTime - startTime) );
+  Serial.print( " hz " );
+  Serial.print( "\n filter value: " ); Serial.print( myFilter.output() );
+
+  myFilter.setToNewValue( 0.0 );
+  Serial.print( "\n after reset to 0: "); Serial.print( myFilter.output() );
+
+  Serial.print( "\n testing rise time (10% to 90%) ...");
+
+  bool crossedTenPercent = false;
+  while( myFilter.output() < 0.9 ) {
+    myFilter.input( 1.0 );
+    if( myFilter.output() > 0.1 && !crossedTenPercent ) {
+      // filter first crossed the 10% point
+      startTime = millis()*1e-3;
+      crossedTenPercent = true;
+    }
+  }
+  stopTime = millis()*1e-3;
+
+  Serial.print( "done, rise time: " ); Serial.print( stopTime-startTime );
+
+  Serial.print( "testing attenuation at f = 1/risetime" );
+
+  myFilter.setToNewValue( 0.0 );
+
+  float maxVal = 0;
+  float valWasOutputThisCycle = true;
+
+  __unused float lastFilterVal = 0;
+
+  while( true ) {
+    float now = 1e-3*millis();
+
+    float currentFilterVal = myFilter.input( sin( TWO_PI*now) );
+
+    if( currentFilterVal < 0.0 ) {
+      if( !valWasOutputThisCycle ) {
+        // just crossed below zero, output the max
+        Serial.print( maxVal*100 ); Serial.print( " %\n" );
+        valWasOutputThisCycle = true;
+      }
+
+    }
+
+  }
+
+}