From 6631ecf04a0592c816dfb832fa928a95877184d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 11:45:35 +0100 Subject: [PATCH 1/4] commander: reset blink_msg_end when blink message completed to set normal LED status immediately --- src/modules/commander/commander_helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 033e7dc88e..04624275f9 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -161,6 +161,7 @@ int blink_msg_state() return 0; } else if (hrt_absolute_time() > blink_msg_end) { + blink_msg_end = 0; return 2; } else { From 855944fb2eab33074537b5ccfc82e2462b662b5b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 00:24:40 +0100 Subject: [PATCH 2/4] commander: beeps and blinks cleanup --- .../commander/accelerometer_calibration.cpp | 2 +- .../commander/airspeed_calibration.cpp | 2 +- src/modules/commander/commander.cpp | 46 ++++------ src/modules/commander/commander_helper.cpp | 86 +++++++++++-------- src/modules/commander/commander_helper.h | 14 +-- 5 files changed, 76 insertions(+), 74 deletions(-) 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 c039b85737..193a7473eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1101,7 +1101,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); } } @@ -1196,8 +1196,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 */ @@ -1253,7 +1254,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 && 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); } } @@ -1308,21 +1309,18 @@ 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; + set_tune(TONE_ARMING_WARNING_TUNE); } 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_BATTERY_WARNING_SLOW_TUNE); } 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 (battery_tune_played) { - tune_stop(); + set_tune(TONE_STOP_TUNE); battery_tune_played = false; } @@ -1693,15 +1691,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); } } @@ -1715,7 +1707,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); } } @@ -1723,27 +1715,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: @@ -1883,9 +1875,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 04624275f9..265c0134cb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -82,10 +83,21 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) } static int buzzer; -static hrt_abstime blink_msg_end; +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 = 0; // currently playing tune, can be interrupted after tune_end +static 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] = 700000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { @@ -93,6 +105,8 @@ int buzzer_init() return ERROR; } + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + return OK; } @@ -101,58 +115,60 @@ void buzzer_deinit() close(buzzer); } -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +void set_tune(int tune) { + 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_busser == 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_busser == 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_busser == 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() 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(); From 0613b299c033c21e1ddcbd8faeb9d3430d72386c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 09:13:51 +0100 Subject: [PATCH 3/4] commander: play warning tune (as for low battery) when in failsafe state --- src/modules/commander/commander.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 193a7473eb..20bfca4774 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,14 +1311,14 @@ int commander_thread_main(int argc, char *argv[]) /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* play tune on battery warning */ - set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ 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) { set_tune(TONE_STOP_TUNE); battery_tune_played = false; @@ -1420,11 +1420,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 { From 0ead560059fff0a31183f40a6b848e82aa2dae80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 18:24:20 +0100 Subject: [PATCH 4/4] commander: tunes cleanup and fixes --- src/modules/commander/commander.cpp | 9 ++---- src/modules/commander/commander_helper.cpp | 32 ++++++++++------------ 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 20bfca4774..7284b38c91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -607,7 +607,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 */ @@ -1020,14 +1019,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); @@ -1310,6 +1307,7 @@ int commander_thread_main(int argc, char *argv[]) if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); + arm_tune_played = true; } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ @@ -1319,13 +1317,12 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (battery_tune_played) { + } else { set_tune(TONE_STOP_TUNE); - battery_tune_played = false; } /* 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; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 265c0134cb..fe6c9bfaa5 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -82,20 +82,20 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } -static int buzzer; +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 = 0; // currently playing tune, can be interrupted after tune_end -static int tune_durations[TONE_NUMBER_OF_TUNES]; +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] = 700000; - tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + 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); @@ -105,8 +105,6 @@ int buzzer_init() return ERROR; } - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - return OK; } @@ -116,15 +114,15 @@ void buzzer_deinit() } void set_tune(int tune) { - int new_tune_duration = tune_durations[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) { + 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) { + if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } tune_current = tune; - if (new_tune_duration > 0) { + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; } else { tune_end = 0; @@ -133,7 +131,7 @@ void set_tune(int tune) { } /** - * Blink green LED and play positive tune (if use_busser == true). + * Blink green LED and play positive tune (if use_buzzer == true). */ void tune_positive(bool use_buzzer) { @@ -146,7 +144,7 @@ void tune_positive(bool use_buzzer) } /** - * Blink white LED and play neutral tune (if use_busser == true). + * Blink white LED and play neutral tune (if use_buzzer == true). */ void tune_neutral(bool use_buzzer) { @@ -159,7 +157,7 @@ void tune_neutral(bool use_buzzer) } /** - * Blink red LED and play negative tune (if use_busser == true). + * Blink red LED and play negative tune (if use_buzzer == true). */ void tune_negative(bool use_buzzer) { @@ -185,8 +183,8 @@ int blink_msg_state() } } -static int leds; -static int rgbleds; +static int leds = -1; +static int rgbleds = -1; int led_init() {