Filter: ported to AP_HAL

libraries/Filter/AverageFilter.h

@@ -9,12 +9,11 @@
 /// @file	AverageFilter.h
 /// @brief	A class to provide the average of a number of samples
 
-#ifndef AverageFilter_h
-#define AverageFilter_h
+#ifndef __AVERAGE_FILTER_H__
+#define __AVERAGE_FILTER_H__
 
-#include <inttypes.h>
-#include <Filter.h>
-#include <FilterWithBuffer.h>
+#include "FilterClass.h"
+#include "FilterWithBuffer.h"
 
 // 1st parameter <T> is the type of data being filtered.
 // 2nd parameter <U> is a larger data type used during summation to prevent overflows
@@ -100,4 +99,4 @@ void AverageFilter<T,U,FILTER_SIZE>::        reset()
     _num_samples = 0;
 }
 
-#endif
+#endif // __AVERAGE_FILTER_H__

libraries/Filter/DerivativeFilter.cpp

@@ -9,12 +9,13 @@
 /// @file	Derivative.cpp
 /// @brief	A class to implement a derivative (slope) filter
 /// See http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
-
-#include <FastSerial.h>
+//
 #include <inttypes.h>
+#include <math.h>
 #include <Filter.h>
 #include <DerivativeFilter.h>
 
+
 template <class T,  uint8_t FILTER_SIZE>
 void DerivativeFilter<T,FILTER_SIZE>::update(T sample, uint32_t timestamp)
 {

libraries/Filter/DerivativeFilter.h

@@ -10,12 +10,11 @@
 /// @brief	A class to implement a derivative (slope) filter
 /// See http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
 
-#ifndef Derivative_h
-#define Derivative_h
+#ifndef __DERIVATIVE_FILTER_H__
+#define __DERIVATIVE_FILTER_H__
 
-#include <inttypes.h>
-#include <Filter.h>
-#include <FilterWithBuffer.h>
+#include "FilterClass.h"
+#include "FilterWithBuffer.h"
 
 // 1st parameter <T> is the type of data being filtered.
 // 2nd parameter <FILTER_SIZE> is the number of elements in the filter
@@ -50,5 +49,5 @@ typedef DerivativeFilter<float,7> DerivativeFilterFloat_Size7;
 typedef DerivativeFilter<float,9> DerivativeFilterFloat_Size9;
 
 
-#endif // Derivative_h
+#endif // __DERIVATIVE_FILTER_H__
 

libraries/Filter/Filter.h

@@ -1,49 +1,16 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-//
-// This 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.
-//
 
-/// @file	Filter.h
-/// @brief	A base class from which various filters classes should inherit
-///
+#ifndef __FILTER_H__
+#define __FILTER_H__
 
-#ifndef Filter_h
-#define Filter_h
-
-#include <inttypes.h>
-#include <AP_Common.h>
-
-template <class T>
-class Filter
-{
-public:
-    // constructor
-    Filter() {
-    };
-
-    // apply - Add a new raw value to the filter, retrieve the filtered result
-    virtual T        apply(T sample) {
-        return sample;
-    };
-
-    // reset - clear the filter
-    virtual void        reset() {
-    };
-
-};
-
-// Typedef for convenience
-typedef Filter<int8_t> FilterInt8;
-typedef Filter<uint8_t> FilterUInt8;
-typedef Filter<int16_t> FilterInt16;
-typedef Filter<uint16_t> FilterUInt16;
-typedef Filter<int32_t> FilterInt32;
-typedef Filter<uint32_t> FilterUInt32;
-
-#endif
+/* Umbrella header for the Filter library */
 
+#include "FilterClass.h"
+#include "AverageFilter.h"
+#include "DerivativeFilter.h"
+#include "FilterWithBuffer.h"
+#include "LowPassFilter.h"
+#include "ModeFilter.h"
+#include "ThirdOrderCompFilter.h"
 
+#endif //__FILTER_H__
 

libraries/Filter/FilterClass.h

@@ -0,0 +1,39 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+// This 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.
+//
+
+/// @file	FilterClass.h
+/// @brief	A pure virtual interface class
+///
+
+#ifndef __FILTER_CLASS_H__
+#define __FILTER_CLASS_H__
+
+#include <inttypes.h>
+
+template <class T>
+class Filter
+{
+public:
+    // apply - Add a new raw value to the filter, retrieve the filtered result
+    virtual T apply(T sample) = 0;
+
+    // reset - clear the filter
+    virtual void reset()  = 0;
+
+};
+
+// Typedef for convenience
+typedef Filter<int8_t> FilterInt8;
+typedef Filter<uint8_t> FilterUInt8;
+typedef Filter<int16_t> FilterInt16;
+typedef Filter<uint16_t> FilterUInt16;
+typedef Filter<int32_t> FilterInt32;
+typedef Filter<uint32_t> FilterUInt32;
+
+#endif // __FILTER_CLASS_H__
+

libraries/Filter/FilterWithBuffer.h

@@ -12,12 +12,10 @@
 ///         restrictions caused by the use of templates which makes different sizes essentially
 ///         completely different classes
 
-#ifndef FilterWithBuffer_h
-#define FilterWithBuffer_h
+#ifndef __FILTER_WITH_BUFFER_H__
+#define __FILTER_WITH_BUFFER_H__
 
-#include <inttypes.h>
-#include <AP_Common.h>
-#include <Filter.h>
+#include "FilterClass.h"
 
 template <class T, uint8_t FILTER_SIZE>
 class FilterWithBuffer : public Filter<T>
@@ -27,13 +25,13 @@ public:
     FilterWithBuffer();
 
     // apply - Add a new raw value to the filter, retrieve the filtered result
-    virtual T        apply(T sample);
+    T apply(T sample);
 
     // reset - clear the filter
-    virtual void        reset();
+    void reset();
 
     // get filter size
-    virtual uint8_t        get_filter_size() {
+    uint8_t get_filter_size() {
         return FILTER_SIZE;
     };
 
@@ -76,7 +74,6 @@ typedef FilterWithBuffer<uint32_t,7> FilterWithBufferUInt32_Size7;
 // Constructor
 template <class T, uint8_t FILTER_SIZE>
 FilterWithBuffer<T,FILTER_SIZE>::FilterWithBuffer() :
-    Filter<T>(),
     sample_index(0)
 {
     // clear sample buffer
@@ -85,11 +82,8 @@ FilterWithBuffer<T,FILTER_SIZE>::FilterWithBuffer() :
 
 // reset - clear all samples from the buffer
 template <class T, uint8_t FILTER_SIZE>
-void FilterWithBuffer<T,FILTER_SIZE>::        reset()
+void FilterWithBuffer<T,FILTER_SIZE>::reset()
 {
-    // call base class reset
-    Filter<T>::reset();
-
     // clear samples buffer
     for( int8_t i=0; i<FILTER_SIZE; i++ ) {
         samples[i] = 0;

libraries/Filter/LowPassFilter.h

@@ -11,11 +11,11 @@
 ///         the downside being that it's a little slower as it internally uses a float
 ///         and it consumes an extra 4 bytes of memory to hold the constant gain
 
-#ifndef LowPassFilter_h
-#define LowPassFilter_h
+#ifndef __LOW_PASS_FILTER_H__
+#define __LOW_PASS_FILTER_H__
 
-#include <inttypes.h>
-#include <Filter.h>
+#include <AP_Math.h>
+#include "FilterClass.h"
 
 // 1st parameter <T> is the type of data being filtered.
 template <class T>
@@ -106,4 +106,4 @@ T LowPassFilter<T>::apply(T sample)
     return (T)_base_value;
 }
 
-#endif
+#endif // __LOW_PASS_FILTER_H__

libraries/Filter/ModeFilter.h

@@ -10,12 +10,12 @@
 /// @brief	A class to apply a mode filter which is basically picking the median value from the last x samples
 ///         the filter size (i.e buffer size) should always be an odd number
 
-#ifndef ModeFilter_h
-#define ModeFilter_h
+#ifndef __MODE_FILTER_H__
+#define __MODE_FILTER_H__
 
 #include <inttypes.h>
-#include <Filter.h>
-#include <FilterWithBuffer.h>
+#include "FilterClass.h"
+#include "FilterWithBuffer.h"
 
 template <class T, uint8_t FILTER_SIZE>
 class ModeFilter : public FilterWithBuffer<T,FILTER_SIZE>
@@ -135,4 +135,4 @@ void ModeFilter<T,FILTER_SIZE>::        isort(T new_sample, bool drop_high)
     }
 }
 
-#endif
+#endif // __MODE_FILTER_H__

libraries/Filter/examples/Derivative/Derivative.pde

@@ -2,38 +2,22 @@
  *       Example sketch to demonstrate use of DerivativeFilter library.
  */
 
-#include <FastSerial.h>
+#include <stdlib.h>
 #include <AP_Common.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
 #include <AP_Param.h>
 #include <AP_Math.h>
 #include <Filter.h>
 #include <DerivativeFilter.h>
 #include <AP_Buffer.h>
 
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
+
 #ifdef DESKTOP_BUILD
-// all of this is needed to build with SITL
- #include <DataFlash.h>
- #include <APM_RC.h>
- #include <GCS_MAVLink.h>
- #include <Arduino_Mega_ISR_Registry.h>
- #include <AP_PeriodicProcess.h>
- #include <AP_ADC.h>
- #include <AP_Baro.h>
- #include <AP_Compass.h>
- #include <AP_GPS.h>
- #include <Filter.h>
- #include <SITL.h>
- #include <I2C.h>
- #include <SPI.h>
- #include <AP_Declination.h>
- #include <AP_Semaphore.h>
-Arduino_Mega_ISR_Registry isr_registry;
-AP_Baro_BMP085_HIL barometer;
-AP_Compass_HIL compass;
-SITL sitl;
+#error Desktop build not supported with AP_HAL
 #endif
 
-FastSerialPort0(Serial);        // FTDI/console
 
 DerivativeFilter<float,11> derivative;
 
@@ -41,7 +25,7 @@ DerivativeFilter<float,11> derivative;
 void setup()
 {
     // Open up a serial connection
-    Serial.begin(115200);
+    hal.uart0->begin(115200);
 }
 
 static float noise(void)
@@ -52,13 +36,15 @@ static float noise(void)
 //Main loop where the action takes place
 void loop()
 {
-    delay(50);
-    float t = millis()*1.0e-3;
+    hal.scheduler->delay(50);
+    float t = hal.scheduler->millis()*1.0e-3;
     float s = sin(t);
     //s += noise();
-    uint32_t t1 = micros();
+    uint32_t t1 = hal.scheduler->micros();
     derivative.update(s, t1);
     float output = derivative.slope() * 1.0e6;
-    uint32_t t2 = micros();
-    Serial.printf("%f %f %f %f\n", t, output, s, cos(t));
+    uint32_t t2 = hal.scheduler->micros();
+    hal.console->printf("%f %f %f %f\n", t, output, s, cos(t));
 }
+
+AP_HAL_MAIN();

libraries/Filter/examples/Filter/Filter.pde

@@ -3,8 +3,10 @@
  *       Code by Randy Mackay and Jason Short. DIYDrones.com
  */
 
-#include <FastSerial.h>
 #include <AP_Common.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+
 #include <AP_Param.h>
 #include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 #include <Filter.h>                     // Filter library
@@ -12,10 +14,7 @@
 #include <AverageFilter.h>      // AverageFilter class (inherits from Filter class)
 #include <AP_Buffer.h>
 
-////////////////////////////////////////////////////////////////////////////////
-// Serial ports
-////////////////////////////////////////////////////////////////////////////////
-FastSerialPort0(Serial);        // FTDI/console
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
 
 int16_t rangevalue[] = {31000, 31000, 50, 55, 60, 55, 10, 0, 31000};
 
@@ -25,34 +24,23 @@ ModeFilterInt16_Size5 mfilter(2);  // buffer of 5 values, result will be from bu
 
 AverageFilterUInt16_Size4 _temp_filter;
 
-// Function to print contents of a filter
-// we need to ues FilterWithBuffer class because we want to access the individual elements
-void printFilter(FilterWithBufferInt16_Size5& filter)
-{
-    for(uint8_t i=0; i < filter.get_filter_size(); i++)
-    {
-	    Serial.printf("%d ",(int)filter.get_sample(i));
-    }
-    Serial.println();
-}
-
 void setup()
 {
     // Open up a serial connection
-    Serial.begin(115200);
+    hal.uart0->begin(115200);
 
     // introduction
-    Serial.printf("ArduPilot ModeFilter library test ver 1.0\n\n");
+    hal.console->printf("ArduPilot ModeFilter library test ver 1.0\n\n");
 
     // Wait for the serial connection
-    delay(500);
+    hal.scheduler->delay(500);
 }
 
 // Read Raw Temperature values
-void ReadTemp()
+void readTemp()
 {
     static uint8_t next_num = 0;
-    static int32_t RawTemp = 0;
+    static int32_t raw_temp = 0;
     uint8_t buf[2];
     uint16_t _temp_sensor;
 
@@ -63,9 +51,9 @@ void ReadTemp()
     _temp_sensor = buf[0];
     _temp_sensor = (_temp_sensor << 8) | buf[1];
 
-    RawTemp = _temp_filter.apply(_temp_sensor);
+    raw_temp = _temp_filter.apply(_temp_sensor);
 
-    Serial.printf("RT: %ld\n",RawTemp);
+    hal.console->printf("RT: %ld\n", raw_temp);
 }
 
 //Main loop where the action takes place
@@ -77,28 +65,10 @@ void loop()
     int16_t j;
 
     for(j=0; j<0xFF; j++ ) {
-        ReadTemp();
+        readTemp();
+        hal.scheduler->delay(100);
     }
-    /*while( i < 9 ) {
-     *
-     *       // output to user
-     *       Serial.printf("applying: %d\n",(int)rangevalue[i]);
-     *
-     *       // display original
-     *       Serial.printf("before: ");
-     *       printFilter(mfilter);
-     *
-     *       // apply new value and retrieved filtered result
-     *       filtered_value = mfilter.apply(rangevalue[i]);
-     *
-     *       // display results
-     *       Serial.printf("after: ");
-     *       printFilter(mfilter);
-     *       Serial.printf("The filtered value is: %d\n\n",(int)filtered_value);
-     *
-     *       i++;
-     *  }*/
-    delay(10000);
+    hal.scheduler->delay(10000);
 }
 
-
+AP_HAL_MAIN();

libraries/Filter/examples/LowPassFilter/LowPassFilter.pde

@@ -3,18 +3,16 @@
  *       Code by Randy Mackay. DIYDrones.com
  */
 
-#include <FastSerial.h>
 #include <AP_Common.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
 #include <AP_Param.h>
 #include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 #include <Filter.h>                     // Filter library
 #include <LowPassFilter.h>      // LowPassFilter class (inherits from Filter class)
 #include <AP_Buffer.h>
 
-////////////////////////////////////////////////////////////////////////////////
-// Serial ports
-////////////////////////////////////////////////////////////////////////////////
-FastSerialPort0(Serial);        // FTDI/console
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
 
 // create a global instance of the class
 LowPassFilterFloat low_pass_filter;
@@ -23,17 +21,17 @@ LowPassFilterFloat low_pass_filter;
 void setup()
 {
     // Open up a serial connection
-    Serial.begin(115200);
+    hal.uart0->begin(115200);
 
     // introduction
-    Serial.printf("ArduPilot LowPassFilter test ver 1.0\n\n");
+    hal.console->printf("ArduPilot LowPassFilter test ver 1.0\n\n");
 
     // set-up filter
     low_pass_filter.set_time_constant(0.02, 0.015);
     //low_pass_filter.set_cutoff_frequency(0.02, 1.0);
 
     // Wait for the serial connection
-    delay(500);
+    hal.scheduler->delay(500);
 }
 
 //Main loop where the action takes place
@@ -52,13 +50,17 @@ void loop()
         new_value = sin((float)i*2*M_PI*5/50.0);  // 5hz
 
         // output to user
-        Serial.printf("applying: %6.4f",(float)new_value);
+        hal.console->printf("applying: %6.4f", new_value);
 
         // apply new value and retrieved filtered result
         filtered_value = low_pass_filter.apply(new_value);
 
         // display results
-        Serial.printf("\toutput: %6.4f\n",(float)filtered_value);
+        hal.console->printf("\toutput: %6.4f\n", filtered_value);
+
+        hal.scheduler->delay(10);
     }
-    delay(10000);
+    hal.scheduler->delay(10000);
 }
+
+AP_HAL_MAIN();