diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 8958114616..02ff1aa428 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -184,17 +184,18 @@ void Copter::esc_calibration_auto() // delay for 5 seconds while outputting pulses uint32_t tstart = millis(); while (millis() - tstart < 5000) { + SRV_Channels::cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); + SRV_Channels::push(); esc_calibration_notify(); delay(3); } - // reduce throttle to minimum - motors->set_throttle_passthrough_for_esc_calibration(0.0f); - // block until we restart while(1) { + SRV_Channels::cork(); motors->set_throttle_passthrough_for_esc_calibration(0.0f); + SRV_Channels::push(); esc_calibration_notify(); delay(3); }