|
|
|
@ -44,12 +44,18 @@ void loop()
@@ -44,12 +44,18 @@ void loop()
|
|
|
|
|
// display PID gains
|
|
|
|
|
hal.console->printf("P %f I %f D %f imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax()); |
|
|
|
|
|
|
|
|
|
RC_Channel *ch = rc().channel(0); |
|
|
|
|
if (ch == nullptr) { |
|
|
|
|
hal.console->printf("No channel 0?"); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// capture radio trim
|
|
|
|
|
radio_trim = RC_Channels::get_radio_in(0); |
|
|
|
|
radio_trim = ch->get_radio_in(); |
|
|
|
|
|
|
|
|
|
while (true) { |
|
|
|
|
RC_Channels::read_input(); // poll the radio for new values
|
|
|
|
|
radio_in = RC_Channels::get_radio_in(0); |
|
|
|
|
rc().read_input(); // poll the radio for new values
|
|
|
|
|
radio_in = ch->get_radio_in(); |
|
|
|
|
error = radio_in - radio_trim; |
|
|
|
|
pid.set_input_filter_all(error); |
|
|
|
|
control_P = pid.get_p(); |
|
|
|
|