Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PID: example sketch prints individual P, I and D values

This commit is contained in:
Randy Mackay 2014-05-27 22:22:21 +09:00
parent 5eb206e6bb
commit faf3415e5e
1 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde
View File

@ -36,12 +36,12 @@ void loop()
AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100);
uint16_t radio_in;
uint16_t radio_trim;
int error;
int control;
int16_t error;
float control_P, control_I, control_D;
float dt = 1000/50;
// display PID gains
hal.console->printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
hal.console->printf("P %f I %f D %f imax %f\n", (float)pid.kP(), (float)pid.kI(), (float)pid.kD(), (float)pid.imax());
// capture radio trim
radio_trim = hal.rcin->read(0);
@ -49,10 +49,15 @@ void loop()
while( true ) {
radio_in = hal.rcin->read(0);
error = radio_in - radio_trim;
control = pid.get_pid(error, dt);
control_P = pid.get_p(error);
control_I = pid.get_i(error, dt);
control_D = pid.get_d(error, dt);
// display pid results
hal.console->printf("radio: %d\t err: %d\t pid:%d\n", radio_in, error, control);
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);
hal.scheduler->delay(50);
}
}