From adef826a42cc79c7777c4c6b8d77e216e305f441 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 9 Mar 2015 19:01:29 -0700
Subject: [PATCH] AP_Notify: play tones for compass cal

---
 libraries/AP_Notify/ToneAlarm_PX4.cpp | 34 +++++++++++++++++++++++++++
 libraries/AP_Notify/ToneAlarm_PX4.h   |  1 +
 2 files changed, 35 insertions(+)

diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm_PX4.cpp
index 05abfc3312..b2ea4435cf 100644
--- a/libraries/AP_Notify/ToneAlarm_PX4.cpp
+++ b/libraries/AP_Notify/ToneAlarm_PX4.cpp
@@ -63,6 +63,8 @@ const ToneAlarm_PX4::Tone ToneAlarm_PX4::_tones[] {
     { "MBT200>B#1", true },
     #define AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS 13
     { "MBNT255>B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8", true },
+    #define AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS 14
+    { "MBNT255<C16P2", true },
 };
 
 bool ToneAlarm_PX4::init()
@@ -132,6 +134,38 @@ void ToneAlarm_PX4::update()
 
     check_cont_tone();
 
+    if (AP_Notify::flags.compass_cal_running != flags.compass_cal_running) {
+        if(AP_Notify::flags.compass_cal_running) {
+            play_tone(AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS);
+            play_tone(AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK);
+        } else {
+            if(_cont_tone_playing == AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS) {
+                stop_cont_tone();
+            }
+        }
+    }
+    flags.compass_cal_running = AP_Notify::flags.compass_cal_running;
+
+    if (AP_Notify::events.initiated_compass_cal) {
+        play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK);
+        return;
+    }
+
+    if (AP_Notify::events.compass_cal_saved) {
+        play_tone(AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED);
+        return;
+    }
+
+    if (AP_Notify::events.compass_cal_failed) {
+        play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK);
+        return;
+    }
+
+    // don't play other tones if compass cal is running
+    if (AP_Notify::flags.compass_cal_running) {
+        return;
+    }
+
     // notify the user when autotune or mission completes
     if (AP_Notify::flags.armed && (AP_Notify::events.autotune_complete || AP_Notify::events.mission_complete)) {
         play_tone(AP_NOTIFY_PX4_TONE_LOUD_READY_OR_FINISHED);
diff --git a/libraries/AP_Notify/ToneAlarm_PX4.h b/libraries/AP_Notify/ToneAlarm_PX4.h
index 60c0458084..76e52310da 100644
--- a/libraries/AP_Notify/ToneAlarm_PX4.h
+++ b/libraries/AP_Notify/ToneAlarm_PX4.h
@@ -55,6 +55,7 @@ private:
         uint8_t pre_arm_check         : 1;    // 0 = failing checks, 1 = passed
         uint8_t failsafe_radio        : 1;    // 1 if radio failsafe
         uint8_t vehicle_lost          : 1;    // 1 if lost copter tone requested
+        uint8_t compass_cal_running   : 1;    // 1 if compass calibration is running
     } flags;
 
     int8_t _cont_tone_playing;