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

commander: beeps and blinks cleanup

This commit is contained in:
Anton Babushkin 2014-02-11 00:24:40 +01:00
parent 6631ecf04a
commit 855944fb2e
5 changed files with 76 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/commander/accelerometer_calibration.cpp
View File

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

2
src/modules/commander/airspeed_calibration.cpp
View File

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

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

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

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

@ -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>
@ -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);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
}
void tune_negative()
{
led_negative();
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_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()

14
src/modules/commander/commander_helper.h
View File

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