Browse Source

Filter: example fix travis warning

missing function declaration
implicit cast
some style fix
master
Pierre Kancir 8 years ago committed by Francisco Ferreira
parent
commit
892a999ba5
  1. 13
      libraries/Filter/examples/Derivative/Derivative.cpp
  2. 10
      libraries/Filter/examples/Filter/Filter.cpp
  3. 17
      libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp
  4. 16
      libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp

13
libraries/Filter/examples/Derivative/Derivative.cpp

@ -6,14 +6,17 @@ @@ -6,14 +6,17 @@
#include <Filter/Filter.h>
#include <Filter/DerivativeFilter.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define USE_NOISE 0
DerivativeFilter<float,11> derivative;
DerivativeFilter<float, 11> derivative;
// setup routine
void setup(){}
void setup() {}
static float noise(void)
{
@ -24,17 +27,17 @@ static float noise(void) @@ -24,17 +27,17 @@ static float noise(void)
#endif
}
//Main loop where the action takes place
// Main loop where the action takes place
void loop()
{
hal.scheduler->delay(50);
float t = AP_HAL::millis()*1.0e-3f;
float t = AP_HAL::millis() * 1.0e-3f;
float s = sinf(t);
s += noise();
uint32_t t1 = AP_HAL::micros();
derivative.update(s, t1);
float output = derivative.slope() * 1.0e6f;
hal.console->printf("%f %f %f %f\n", t, output, s, cosf(t));
hal.console->printf("%f %f %f %f\n", (double)t, (double)output, (double)s, (double)cosf(t));
}
AP_HAL_MAIN();

10
libraries/Filter/examples/Filter/Filter.cpp

@ -8,6 +8,10 @@ @@ -8,6 +8,10 @@
#include <Filter/ModeFilter.h> // ModeFilter class (inherits from Filter class)
#include <Filter/AverageFilter.h> // AverageFilter class (inherits from Filter class)
void setup();
void loop();
void readTemp();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
int16_t rangevalue[] = {31000, 31000, 50, 55, 60, 55, 10, 0, 31000};
@ -38,7 +42,7 @@ void readTemp() @@ -38,7 +42,7 @@ void readTemp()
uint16_t _temp_sensor;
next_num++;
buf[0] = next_num; //next_num;
buf[0] = next_num; //next_num;
buf[1] = 0xFF;
_temp_sensor = buf[0];
@ -53,10 +57,10 @@ void readTemp() @@ -53,10 +57,10 @@ void readTemp()
hal.console->printf("RT: %lu\n", (unsigned long)raw_temp);
}
//Main loop where the action takes place
// Main loop where the action takes place
void loop()
{
for (uint8_t j=0; j<0xFF; j++ ) {
for (uint8_t j = 0; j < 0xFF; j++) {
readTemp();
hal.scheduler->delay(100);
}

17
libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp

@ -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);
}

16
libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp

@ -7,6 +7,8 @@ @@ -7,6 +7,8 @@
#include <Filter/Filter.h> // Filter library
#include <Filter/LowPassFilter2p.h>
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
// craete an instance with 800Hz sample rate and 30Hz cutoff
@ -21,23 +23,19 @@ static void setup() @@ -21,23 +23,19 @@ static void setup()
void loop()
{
int16_t i;
float new_value;
float filtered_value;
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);
const float filtered_value = low_pass_filter.apply(new_value);
// 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);
}

Loading…
Cancel
Save