Cleanup set_tune() functions in the commander_helper

- Existing code was hard to read and quite ambiguous
- Also it allowed constantly re-sending the tune_control request for a
fixed duration tunes like "TUNE_ID_BATTERY_WARNING_SLOW", while not
respecting the tune duration

src/modules/commander/commander_helper.cpp

@@ -123,18 +123,26 @@ bool is_ground_rover(const vehicle_status_s &current_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
+// 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 hrt_abstime blink_msg_end = 0;
-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] {};
-
 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];
+	const hrt_abstime current_time = hrt_absolute_time();
-
+	const unsigned int new_tune_duration = tune_durations[tune_id];
-	/* don't interrupt currently playing non-repeating tune by repeating */
+	const bool current_tune_is_repeating = (tune_end == 0) ? true : false;
-	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
+	const bool new_tune_is_repeating = (new_tune_duration == 0) ? true : false;
-		/* allow interrupting current non-repeating tune by the same tune */
+
-		if (tune != tune_current || new_tune_duration != 0) {
+	bool set_new_tune = false;
-			tune_control.tune_id = tune;
+
-			tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
+	if (!current_tune_is_repeating) {
-			tune_control.tune_override = false;
+		// Current non repeating tune has ended
-			tune_control.timestamp = hrt_absolute_time();
+		if (current_time > tune_end) {
-			orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
+			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;
 		}
 	}

src/modules/commander/commander_helper.h

@@ -60,8 +60,24 @@ bool is_ground_rover(const vehicle_status_s &current_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);