Copter: fix LED notify during auto esc calibration

ArduCopter/Copter.h

@@ -589,6 +589,9 @@ private:
     // last valid RC input time
     uint32_t last_radio_update_ms;
 
+    // last esc calibration notification update
+    uint32_t esc_calibration_notify_update_ms;
+
 #if VISUAL_ODOMETRY_ENABLED == ENABLED
     // last visual odometry update time
     uint32_t visual_odom_last_update_ms;
@@ -939,6 +942,7 @@ private:
     void esc_calibration_startup_check();
     void esc_calibration_passthrough();
     void esc_calibration_auto();
+    void esc_calibration_notify();
     bool should_disarm_on_failsafe();
     void failsafe_radio_on_event();
     void failsafe_radio_off_event();

ArduCopter/esc_calibration.cpp

@@ -102,15 +102,9 @@ void Copter::esc_calibration_passthrough()
     motors->enable();
     SRV_Channels::enable_by_mask(motors->get_motor_mask());
 
-    uint32_t last_notify_update_ms = 0;
     while(1) {
-        // flash LEDS
+        // flash LEDs
-        AP_Notify::flags.esc_calibration = true;
+        esc_calibration_notify();
-        uint32_t now = AP_HAL::millis();
-        if (now - last_notify_update_ms > 20) {
-            last_notify_update_ms = now;
-            update_notify();
-        }
 
         // read pilot input
         read_radio();
@@ -147,8 +141,8 @@ void Copter::esc_calibration_auto()
     motors->enable();
     SRV_Channels::enable_by_mask(motors->get_motor_mask());
 
-    // flash LEDS
+    // flash LEDs
-    AP_Notify::flags.esc_calibration = true;
+    esc_calibration_notify();
 
     // raise throttle to maximum
     delay(10);
@@ -160,6 +154,7 @@ void Copter::esc_calibration_auto()
             printed_msg = true;
         }
         motors->set_throttle_passthrough_for_esc_calibration(1.0f);
+        esc_calibration_notify();
         delay(3);
     }
 
@@ -167,6 +162,7 @@ void Copter::esc_calibration_auto()
     uint32_t tstart = millis();
     while (millis() - tstart < 5000) {
         motors->set_throttle_passthrough_for_esc_calibration(1.0f);
+        esc_calibration_notify();
         delay(3);
     }
 
@@ -178,8 +174,20 @@ void Copter::esc_calibration_auto()
 
     // block until we restart
     while(1) {
-        delay(3);
         motors->set_throttle_passthrough_for_esc_calibration(0.0f);
+        esc_calibration_notify();
+        delay(3);
     }
 #endif // FRAME_CONFIG != HELI_FRAME
 }
+
+// flash LEDs to notify the user that ESC calibration is happening
+void Copter::esc_calibration_notify()
+{
+    AP_Notify::flags.esc_calibration = true;
+    uint32_t now = AP_HAL::millis();
+    if (now - esc_calibration_notify_update_ms > 20) {
+        esc_calibration_notify_update_ms = now;
+        update_notify();
+    }
+}