diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 5c76e0425a254b1079b349baad41f60fd07f58c0..70cbf07aaac7724c3fe938e16b9cf5f195dd0c06 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -43,6 +43,7 @@ public: uint16_t failsafe_battery : 1; // 1 if battery failsafe uint16_t failsafe_gps : 1; // 1 if gps failsafe uint16_t arming_failed : 1; // 1 if copter failed to arm after user input + uint16_t parachute_release : 1; // 1 if parachute is being released // additional flags uint16_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter) diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm_PX4.cpp index 4f860180f935556616de46c6ddd86758848b0dfc..17e39c0127e74923206a6d6a713956d669b9beee 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.cpp +++ b/libraries/AP_Notify/ToneAlarm_PX4.cpp @@ -64,7 +64,8 @@ void ToneAlarm_PX4::update() if (_tonealarm_fd == -1) { return; } - + + // check for arming failure if(flags.arming_failed != AP_Notify::flags.arming_failed) { flags.arming_failed = AP_Notify::flags.arming_failed; if(flags.arming_failed) { @@ -110,6 +111,15 @@ void ToneAlarm_PX4::update() play_tune(TONE_GPS_WARNING_TUNE); } } + + // check parachute release + if (flags.parachute_release != AP_Notify::flags.parachute_release) { + flags.parachute_release = AP_Notify::flags.parachute_release; + if (flags.parachute_release) { + // parachute release warning tune + play_tune(TONE_PARACHUTE_RELEASE_TUNE); + } + } } #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_Notify/ToneAlarm_PX4.h b/libraries/AP_Notify/ToneAlarm_PX4.h index 2a353898f923ef0d6a7a185b29d542d250b3a2fc..ac5514424aa5a5f44c6e9ed86da33d5df0d28204 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.h +++ b/libraries/AP_Notify/ToneAlarm_PX4.h @@ -42,6 +42,7 @@ private: uint8_t gps_glitching : 1; // 1 if gps position is not good uint8_t failsafe_gps : 1; // 1 if gps failsafe uint8_t arming_failed : 1; // 0 = failing checks, 1 = passed + uint8_t parachute_release : 1; // 1 if parachute is being released } flags; };