Skip to content

Commit

Permalink
Filter: example fix travis warning
Browse files Browse the repository at this point in the history
missing function declaration
implicit cast
some style fix
  • Loading branch information
khancyr authored and OXINARF committed Apr 13, 2017
1 parent 49c9e3c commit 892a999
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 26 deletions.
13 changes: 8 additions & 5 deletions libraries/Filter/examples/Derivative/Derivative.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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 changes: 7 additions & 3 deletions libraries/Filter/examples/Filter/Filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down Expand Up @@ -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];
Expand All @@ -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);
}
Expand Down
17 changes: 8 additions & 9 deletions libraries/Filter/examples/LowPassFilter/LowPassFilter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
}
Expand Down
16 changes: 7 additions & 9 deletions libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
}
Expand Down

0 comments on commit 892a999

Please sign in to comment.