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;