From 5915439c4e634630985a14a0a513422d797b9c21 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 9 May 2017 15:57:40 +0900
Subject: [PATCH] Copter: fix LED notify during auto esc calibration

---
 ArduCopter/Copter.h            |  4 ++++
 ArduCopter/esc_calibration.cpp | 30 +++++++++++++++++++-----------
 2 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 80ebea7380..5faee3f8c3 100644
--- a/ArduCopter/Copter.h
+++ b/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();
diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp
index af9ea21646..b5c068c473 100644
--- a/ArduCopter/esc_calibration.cpp
+++ b/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
-        AP_Notify::flags.esc_calibration = true;
-        uint32_t now = AP_HAL::millis();
-        if (now - last_notify_update_ms > 20) {
-            last_notify_update_ms = now;
-            update_notify();
-        }
+        // flash LEDs
+        esc_calibration_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
-    AP_Notify::flags.esc_calibration = true;
+    // flash LEDs
+    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();
+    }
+}