dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: tunes cleanup and fixes

This commit is contained in:
Anton Babushkin 2014-02-11 18:24:20 +01:00
parent 0613b299c0
commit 0ead560059
2 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/commander/commander.cpp
View File

@ -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;
}

32
src/modules/commander/commander_helper.cpp
View File

@ -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()
{