diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 36b75dd58a..1cbdf9bf85 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
(double)accel_ref[orient][2]);
data_collected[orient] = true;
- tune_neutral();
+ tune_neutral(true);
}
close(sensor_combined_sub);
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 1809f96888..6039d92a76 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd)
}
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
- tune_neutral();
+ tune_neutral(true);
close(diff_pres_sub);
return OK;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6d14472f36..e809dfbf70 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -610,7 +610,6 @@ int commander_thread_main(int argc, char *argv[])
/* not yet initialized */
commander_initialized = false;
- bool battery_tune_played = false;
bool arm_tune_played = false;
/* set parameters */
@@ -1024,14 +1023,12 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
- battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
- battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
@@ -1105,7 +1102,7 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
status.condition_home_position_valid = true;
- tune_positive();
+ tune_positive(true);
}
}
@@ -1200,8 +1197,9 @@ int commander_thread_main(int argc, char *argv[])
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status);
+ /* play tune on mode change only if armed, blink LED always */
if (res == TRANSITION_CHANGED) {
- tune_positive();
+ tune_positive(armed.armed);
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
@@ -1257,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[])
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
- tune_positive();
+ tune_positive(armed.armed);
}
}
@@ -1312,26 +1310,23 @@ int commander_thread_main(int argc, char *argv[])
/* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
/* play tune when armed */
- if (tune_arm() == OK)
- arm_tune_played = true;
-
- } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
- /* play tune on battery warning */
- if (tune_low_bat() == OK)
- battery_tune_played = true;
+ set_tune(TONE_ARMING_WARNING_TUNE);
+ arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
- if (tune_critical_bat() == OK)
- battery_tune_played = true;
+ set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
+
+ } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+ /* play tune on battery warning or failsafe */
+ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
- } else if (battery_tune_played) {
- tune_stop();
- battery_tune_played = false;
+ } else {
+ set_tune(TONE_STOP_TUNE);
}
/* reset arm_tune_played when disarmed */
- if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
+ if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
arm_tune_played = false;
}
@@ -1426,11 +1421,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
if (set_normal_color) {
/* set color */
- if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
- if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
- rgbled_set_color(RGBLED_COLOR_AMBER);
- }
-
+ if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
+ rgbled_set_color(RGBLED_COLOR_AMBER);
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
} else {
@@ -1697,15 +1689,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg)
sprintf(s, "#audio: REJECT %s", msg);
mavlink_log_critical(mavlink_fd, s);
- // only buzz if armed, because else we're driving people nuts indoors
- // they really need to look at the leds as well.
- if (status->arming_state == ARMING_STATE_ARMED) {
- tune_negative();
- } else {
-
- // Always show the led indication
- led_negative();
- }
+ /* only buzz if armed, because else we're driving people nuts indoors
+ they really need to look at the leds as well. */
+ tune_negative(armed.armed);
}
}
@@ -1719,7 +1705,7 @@ print_reject_arm(const char *msg)
char s[80];
sprintf(s, "#audio: %s", msg);
mavlink_log_critical(mavlink_fd, s);
- tune_negative();
+ tune_negative(true);
}
}
@@ -1727,27 +1713,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
- tune_positive();
+ tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
- tune_negative();
+ tune_negative(true);
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
- tune_negative();
+ tune_negative(true);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
- tune_negative();
+ tune_negative(true);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
- tune_negative();
+ tune_negative(true);
break;
default:
@@ -1887,9 +1873,9 @@ void *commander_low_prio_loop(void *arg)
}
if (calib_ret == OK)
- tune_positive();
+ tune_positive(true);
else
- tune_negative();
+ tune_negative(true);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 033e7dc88e..fe6c9bfaa5 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -45,6 +45,7 @@
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
+#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -81,11 +82,22 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
}
-static int buzzer;
-static hrt_abstime blink_msg_end;
+static int buzzer = -1;
+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 int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end
+static unsigned int tune_durations[TONE_NUMBER_OF_TUNES];
int buzzer_init()
{
+ tune_end = 0;
+ tune_current = 0;
+ memset(tune_durations, 0, sizeof(tune_durations));
+ tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000;
+ tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000;
+ tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
+ tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
+
buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
if (buzzer < 0) {
@@ -101,58 +113,60 @@ void buzzer_deinit()
close(buzzer);
}
-void tune_error()
-{
- ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
+void set_tune(int tune) {
+ 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) {
+ ioctl(buzzer, TONE_SET_ALARM, tune);
+ }
+ tune_current = tune;
+ if (new_tune_duration != 0) {
+ tune_end = hrt_absolute_time() + new_tune_duration;
+ } else {
+ tune_end = 0;
+ }
+ }
}
-void tune_positive()
+/**
+ * Blink green LED and play positive tune (if use_buzzer == true).
+ */
+void tune_positive(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
- ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
+ if (use_buzzer) {
+ set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+ }
}
-void tune_neutral()
+/**
+ * Blink white LED and play neutral tune (if use_buzzer == true).
+ */
+void tune_neutral(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
- ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
-}
-
-void tune_negative()
-{
- led_negative();
- ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+ if (use_buzzer) {
+ set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
+ }
}
-void led_negative()
+/**
+ * Blink red LED and play negative tune (if use_buzzer == true).
+ */
+void tune_negative(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-}
-
-int tune_arm()
-{
- return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
-}
-
-int tune_low_bat()
-{
- return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
-}
-
-int tune_critical_bat()
-{
- return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
-}
-
-void tune_stop()
-{
- ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
+ if (use_buzzer) {
+ set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
+ }
}
int blink_msg_state()
@@ -161,6 +175,7 @@ int blink_msg_state()
return 0;
} else if (hrt_absolute_time() > blink_msg_end) {
+ blink_msg_end = 0;
return 2;
} else {
@@ -168,8 +183,8 @@ int blink_msg_state()
}
}
-static int leds;
-static int rgbleds;
+static int leds = -1;
+static int rgbleds = -1;
int led_init()
{
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index af25a5e979..e75f2592f5 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
int buzzer_init(void);
void buzzer_deinit(void);
-void tune_error(void);
-void tune_positive(void);
-void tune_neutral(void);
-void tune_negative(void);
-int tune_arm(void);
-int tune_low_bat(void);
-int tune_critical_bat(void);
-void tune_stop(void);
-
-void led_negative();
+void set_tune(int tune);
+void tune_positive(bool use_buzzer);
+void tune_neutral(bool use_buzzer);
+void tune_negative(bool use_buzzer);
int blink_msg_state();