|
|
|
@ -7,6 +7,9 @@
@@ -7,6 +7,9 @@
|
|
|
|
|
#include <Filter/Filter.h> // Filter library |
|
|
|
|
#include <Filter/LowPassFilter.h> // LowPassFilter class (inherits from Filter class) |
|
|
|
|
|
|
|
|
|
void setup(); |
|
|
|
|
void loop(); |
|
|
|
|
|
|
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); |
|
|
|
|
|
|
|
|
|
// create a global instance of the class
|
|
|
|
@ -28,26 +31,22 @@ void setup()
@@ -28,26 +31,22 @@ void setup()
|
|
|
|
|
//Main loop where the action takes place
|
|
|
|
|
void loop() |
|
|
|
|
{ |
|
|
|
|
int16_t i; |
|
|
|
|
float new_value; |
|
|
|
|
float filtered_value; |
|
|
|
|
|
|
|
|
|
// reset value to 100. If not reset the filter will start at the first value entered
|
|
|
|
|
low_pass_filter.reset(0); |
|
|
|
|
|
|
|
|
|
for( i=0; i<300; i++ ) { |
|
|
|
|
for(int16_t i = 0; i < 300; i++ ) { |
|
|
|
|
|
|
|
|
|
// new data value
|
|
|
|
|
new_value = sinf((float)i*2*M_PI*5/50.0f); // 5hz
|
|
|
|
|
const float new_value = sinf((float)i * 2 * M_PI * 5 / 50.0f); // 5hz
|
|
|
|
|
|
|
|
|
|
// output to user
|
|
|
|
|
hal.console->printf("applying: %6.4f", new_value); |
|
|
|
|
hal.console->printf("applying: %6.4f", (double)new_value); |
|
|
|
|
|
|
|
|
|
// apply new value and retrieved filtered result
|
|
|
|
|
filtered_value = low_pass_filter.apply(new_value, 0.02f); |
|
|
|
|
const float filtered_value = low_pass_filter.apply(new_value, 0.02f); |
|
|
|
|
|
|
|
|
|
// display results
|
|
|
|
|
hal.console->printf("\toutput: %6.4f\n", filtered_value); |
|
|
|
|
hal.console->printf("\toutput: %6.4f\n", (double)filtered_value); |
|
|
|
|
|
|
|
|
|
hal.scheduler->delay(10); |
|
|
|
|
} |
|
|
|
|