Browse Source

AC_PID: example fix travis warning

missing function declaration
implicit cast
some style fix
mission-4.1.18
Pierre Kancir 8 years ago committed by Francisco Ferreira
parent
commit
320c5e1b96
  1. 11
      libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp

11
libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp

@ -7,6 +7,9 @@ @@ -7,6 +7,9 @@
#include <AC_PID/AC_PID.h>
#include <AC_PID/AC_HELI_PID.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
// default PID values
@ -38,12 +41,12 @@ void loop() @@ -38,12 +41,12 @@ void loop()
float control_P, control_I, control_D;
// display PID gains
hal.console->printf("P %f I %f D %f imax %f\n", (float)pid.kP(), (float)pid.kI(), (float)pid.kD(), (float)pid.imax());
hal.console->printf("P %f I %f D %f imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax());
// capture radio trim
radio_trim = hal.rcin->read(0);
while( true ) {
while (true) {
radio_in = hal.rcin->read(0);
error = radio_in - radio_trim;
pid.set_input_filter_all(error);
@ -54,8 +57,8 @@ void loop() @@ -54,8 +57,8 @@ void loop()
// display pid results
hal.console->printf("radio: %d\t err: %d\t pid:%4.2f (p:%4.2f i:%4.2f d:%4.2f)\n",
(int)radio_in, (int)error,
(float)(control_P+control_I+control_D),
(float)control_P, (float)control_I, (float)control_D);
(double)(control_P+control_I+control_D),
(double)control_P, (double)control_I, (double)control_D);
hal.scheduler->delay(50);
}
}

Loading…
Cancel
Save