Copter: esc calibration fix

ArduCopter/esc_calibration.cpp

@@ -104,6 +104,17 @@ void Copter::esc_calibration_passthrough()
     // disable safety if requested
     BoardConfig.init_safety();
 
+    // wait for safety switch to be pressed
+    while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
+        bool printed_msg = false;
+        if (!printed_msg) {
+            gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch");
+            printed_msg = true;
+        }
+        esc_calibration_notify();
+        delay(3);
+    }
+
     // arm motors
     motors->armed(true);
     SRV_Channels::enable_by_mask(motors->get_motor_mask());
@@ -132,8 +143,6 @@ void Copter::esc_calibration_passthrough()
 void Copter::esc_calibration_auto()
 {
 #if FRAME_CONFIG != HELI_FRAME
-    bool printed_msg = false;
-
     // clear esc flag for next time
     g.esc_calibrate.set_and_save(ESCCAL_NONE);
 
@@ -151,30 +160,27 @@ void Copter::esc_calibration_auto()
     // disable safety if requested
     BoardConfig.init_safety();
 
-    // arm and enable motors
-    motors->armed(true);
-    SRV_Channels::enable_by_mask(motors->get_motor_mask());
-    hal.util->set_soft_armed(true);
-
-    // flash LEDs
-    esc_calibration_notify();
-
-    // raise throttle to maximum
-    delay(10);
-
     // wait for safety switch to be pressed
     while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
+        bool printed_msg = false;
         if (!printed_msg) {
             gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch");
             printed_msg = true;
         }
-        SRV_Channels::cork();
-        motors->set_throttle_passthrough_for_esc_calibration(1.0f);
-        SRV_Channels::push();
         esc_calibration_notify();
         delay(3);
     }
 
+    // arm and enable motors
+    motors->armed(true);
+    SRV_Channels::enable_by_mask(motors->get_motor_mask());
+    hal.util->set_soft_armed(true);
+
+    // raise throttle to maximum
+    SRV_Channels::cork();
+    motors->set_throttle_passthrough_for_esc_calibration(1.0f);
+    SRV_Channels::push();
+
     // delay for 5 seconds while outputting pulses
     uint32_t tstart = millis();
     while (millis() - tstart < 5000) {