diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index eb4a5a125c..12a547c305 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -123,18 +123,26 @@ bool is_ground_rover(const vehicle_status_s ¤t_status) return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER; } -static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message -static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence -static uint8_t tune_current = tune_control_s::TUNE_ID_STOP; // currently playing tune, can be interrupted after tune_end -static unsigned int tune_durations[tune_control_s::NUMBER_OF_TUNES] {}; - +// End time for currently blinking LED message, 0 if no blink message +static hrt_abstime blink_msg_end = 0; static int fd_leds{-1}; static led_control_s led_control {}; static orb_advert_t led_control_pub = nullptr; + +// Static array that defines the duration of each tune, 0 if it's a repeating tune (therefore no fixed duration) +static unsigned int tune_durations[tune_control_s::NUMBER_OF_TUNES] {}; + +// End time of currently played tune, 0 for repeating tunes or silence +static hrt_abstime tune_end = 0; + +// currently playing tune, can be interrupted after tune_end +static uint8_t tune_current = tune_control_s::TUNE_ID_STOP; + static tune_control_s tune_control {}; static orb_advert_t tune_control_pub = nullptr; + int buzzer_init() { tune_durations[tune_control_s::TUNE_ID_NOTIFY_POSITIVE] = 800000; @@ -156,36 +164,57 @@ void buzzer_deinit() orb_unadvertise(tune_control_pub); } -void set_tune_override(int tune) +void set_tune_override(const int tune_id) { - tune_control.tune_id = tune; + tune_control.tune_id = tune_id; tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; tune_control.tune_override = true; tune_control.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); } -void set_tune(int tune) +void set_tune(const int tune_id) { - unsigned int new_tune_duration = tune_durations[tune]; - - /* don't interrupt currently playing non-repeating tune by repeating */ - if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { - /* allow interrupting current non-repeating tune by the same tune */ - if (tune != tune_current || new_tune_duration != 0) { - tune_control.tune_id = tune; - tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - tune_control.tune_override = false; - tune_control.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); + const hrt_abstime current_time = hrt_absolute_time(); + const unsigned int new_tune_duration = tune_durations[tune_id]; + const bool current_tune_is_repeating = (tune_end == 0) ? true : false; + const bool new_tune_is_repeating = (new_tune_duration == 0) ? true : false; + + bool set_new_tune = false; + + if (!current_tune_is_repeating) { + // Current non repeating tune has ended + if (current_time > tune_end) { + set_new_tune = true; + } + + // Allow non repeating tune to interrupt current non repeating tune, if it's different + if (!new_tune_is_repeating && (tune_id != tune_current)) { + set_new_tune = true; + } + + } else { + // Allow interrupting repeating tune as long as it's a different tune + if (tune_id != tune_current) { + set_new_tune = true; } + } + + if (set_new_tune) { + tune_control.tune_id = tune_id; + tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; + tune_control.tune_override = false; + tune_control.timestamp = current_time; + orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); - tune_current = tune; + tune_current = tune_id; if (new_tune_duration != 0) { - tune_end = hrt_absolute_time() + new_tune_duration; + // Set tune ending time for a finite duration tunes + tune_end = current_time + new_tune_duration; } else { + // Set tune ending time as 0 to indicate it's a repeating tune tune_end = 0; } } diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 8ff0922b66..02029acdb9 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -60,8 +60,24 @@ bool is_ground_rover(const vehicle_status_s ¤t_status); int buzzer_init(void); void buzzer_deinit(void); -void set_tune_override(int tune); -void set_tune(int tune); +/** + * @brief Override tune control and play the given tune + */ +void set_tune_override(const int tune_id); + +/** + * @brief Set the new tune to play under predefined conditions + * + * Setting a new tune will only be possible for the following 3 cases: + * + * 1. Current playing tune is non-repeating and has ended + * 2. Current playing tune is non-repeating, but the requested tune is also non-repeating, and they are different tunes + * 3. Current playing tune is repeating, and the requested tune is different from teh current tune + * + * This is to prevent repeating tunes from overriding single-play tunes. + */ +void set_tune(const int tune_id); + void tune_home_set(bool use_buzzer); void tune_mission_ok(bool use_buzzer); void tune_mission_warn(bool use_buzzer);