From 73e53369e83e520b844dccfad99bcab5fbbf67a6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 11 Jan 2021 07:09:44 +1100
Subject: [PATCH] AP_Notify: added notify events for temperature calibration

---
 libraries/AP_Notify/AP_BoardLED2.cpp |  4 +++-
 libraries/AP_Notify/AP_Notify.h      |  4 ++++
 libraries/AP_Notify/ToneAlarm.cpp    | 34 ++++++++++++++++++++++------
 libraries/AP_Notify/ToneAlarm.h      |  1 +
 4 files changed, 35 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Notify/AP_BoardLED2.cpp b/libraries/AP_Notify/AP_BoardLED2.cpp
index 6673aef982..24a80e4559 100644
--- a/libraries/AP_Notify/AP_BoardLED2.cpp
+++ b/libraries/AP_Notify/AP_BoardLED2.cpp
@@ -89,7 +89,9 @@ void AP_BoardLED2::update(void)
         return;
     }
 
-    if(AP_Notify::flags.compass_cal_running){ // compass calibration
+    if(AP_Notify::flags.compass_cal_running ||
+       AP_Notify::flags.temp_cal_running){
+        // compass calibration or IMU temperature calibration
         switch(save_trim_counter) {
         case 0:
             hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); // short blinks by both LEDs
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index 108a56e3e8..66251ca637 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -103,6 +103,7 @@ public:
         bool waiting_for_throw;   // true when copter is in THROW mode and waiting to detect the user hand launch
         bool powering_off;        // true when the vehicle is powering off
         bool video_recording;     // true when the vehicle is recording video
+        bool temp_cal_running;    // true if a temperature calibration is running
     };
 
     /// notify_events_type - bitmask of active events.
@@ -125,6 +126,9 @@ public:
         uint32_t tune_next              : 3;    // tuning switched to next parameter
         uint32_t tune_save              : 1;    // tuning saved parameters
         uint32_t tune_error             : 1;    // tuning controller error
+        uint32_t initiated_temp_cal     : 1;    // 1 when temperature calibration starts
+        uint32_t temp_cal_saved         : 1;    // 1 when temperature calibration was just saved
+        uint32_t temp_cal_failed        : 1;    // 1 when temperature calibration has just failed
     };
 
     // The notify flags and values are static to allow direct class access
diff --git a/libraries/AP_Notify/ToneAlarm.cpp b/libraries/AP_Notify/ToneAlarm.cpp
index 5fcca1b5f8..7fcc9e78d7 100644
--- a/libraries/AP_Notify/ToneAlarm.cpp
+++ b/libraries/AP_Notify/ToneAlarm.cpp
@@ -62,7 +62,7 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] {
     { "MBT200>A#1", true },
 #define AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS 13
     { "MBNT255>A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8", true },
-#define AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS 14
+#define AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS 14
     { "MBNT255<C16P2", true },
 #define AP_NOTIFY_TONE_WAITING_FOR_THROW 15
     { "MBNT90L4O2A#O3DFN0N0N0", true},
@@ -206,10 +206,10 @@ void AP_ToneAlarm::update()
 
     if (AP_Notify::flags.compass_cal_running != flags.compass_cal_running) {
         if (AP_Notify::flags.compass_cal_running) {
-            play_tone(AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS);
+            play_tone(AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS);
             play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
         } else {
-            if (_cont_tone_playing == AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS) {
+            if (_cont_tone_playing == AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS) {
                 stop_cont_tone();
             }
         }
@@ -226,18 +226,38 @@ void AP_ToneAlarm::update()
         return;
     }
 
-    if (AP_Notify::events.compass_cal_saved) {
+    if (AP_Notify::events.compass_cal_saved ||
+        AP_Notify::events.temp_cal_saved) {
         play_tone(AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED);
         return;
     }
 
-    if (AP_Notify::events.compass_cal_failed) {
+    if (AP_Notify::events.compass_cal_failed ||
+        AP_Notify::events.temp_cal_failed) {
         play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
         return;
     }
 
-    // don't play other tones if compass cal is running
-    if (AP_Notify::flags.compass_cal_running) {
+    if (AP_Notify::events.initiated_temp_cal) {
+        play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
+        return;
+    }
+
+    if (AP_Notify::flags.temp_cal_running != flags.temp_cal_running) {
+        if (AP_Notify::flags.temp_cal_running) {
+            play_tone(AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS);
+            play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
+        } else {
+            if (_cont_tone_playing == AP_NOTIFY_TONE_QUIET_CALIBRATING_CTS) {
+                stop_cont_tone();
+            }
+        }
+    }
+    flags.temp_cal_running = AP_Notify::flags.temp_cal_running;
+    
+    // don't play other tones if cal is running
+    if (AP_Notify::flags.compass_cal_running ||
+        AP_Notify::flags.temp_cal_running) {
         return;
     }
 
diff --git a/libraries/AP_Notify/ToneAlarm.h b/libraries/AP_Notify/ToneAlarm.h
index ae4aaa2f1f..4a09acf15c 100644
--- a/libraries/AP_Notify/ToneAlarm.h
+++ b/libraries/AP_Notify/ToneAlarm.h
@@ -64,6 +64,7 @@ private:
         uint16_t waiting_for_throw     : 1;    // 1 if waiting for copter throw launch
         uint16_t leak_detected         : 1;    // 1 if leak detected
         uint16_t powering_off          : 1;    // 1 if smart battery is powering off
+        uint16_t temp_cal_running      : 1;    // 1 if temperature calibration is running
     } flags;
     bool _have_played_ready_tone : 1;