diff options
Diffstat (limited to 'src/modules/commander/commander_helper.cpp')
| -rw-r--r-- | src/modules/commander/commander_helper.cpp | 135 |
1 files changed, 80 insertions, 55 deletions
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 033e7dc88..d5fe122cb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,11 @@ /** * @file commander_helper.cpp * Commander helper functions implementations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * */ #include <stdio.h> @@ -45,6 +47,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 +84,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 +115,68 @@ void buzzer_deinit() close(buzzer); } -void tune_error() +void set_tune(int tune) { - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_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 +185,7 @@ int blink_msg_state() return 0; } else if (hrt_absolute_time() > blink_msg_end) { + blink_msg_end = 0; return 2; } else { @@ -168,8 +193,8 @@ int blink_msg_state() } } -static int leds; -static int rgbleds; +static int leds = -1; +static int rgbleds = -1; int led_init() { @@ -183,30 +208,26 @@ int led_init() return ERROR; } - /* the blue LED is only available on FMUv1 but not FMUv2 */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - if (ioctl(leds, LED_ON, LED_BLUE)) { - warnx("Blue LED: ioctl fail\n"); - return ERROR; - } + /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ + (void)ioctl(leds, LED_ON, LED_BLUE); -#endif + /* switch blue off */ + led_off(LED_BLUE); + /* we consider the amber led mandatory */ if (ioctl(leds, LED_ON, LED_AMBER)) { warnx("Amber LED: ioctl fail\n"); return ERROR; } + /* switch amber off */ + led_off(LED_AMBER); + /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED_DEVICE_PATH, 0); if (rgbleds == -1) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - errx(1, "Unable to open " RGBLED_DEVICE_PATH); -#else - warnx("No RGB LED found"); -#endif + warnx("No RGB LED found at " RGBLED_DEVICE_PATH); } return 0; @@ -239,22 +260,25 @@ int led_off(int led) void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + } } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + } } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + } } float battery_remaining_estimate_voltage(float voltage, float discharged) @@ -294,6 +318,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { /* else use voltage */ ret = remaining_voltage; |