From 2362041dc127620ac1fb823b8aab8416ba17f0c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 19:58:41 +0200 Subject: commander: state indication changed, 3 green blinks = succsess, 3 white blinks = neutral, 3 red blinks = reject --- src/modules/commander/commander.cpp | 119 ++++++++++++++--------------- src/modules/commander/commander_helper.cpp | 50 +++++++++--- src/modules/commander/commander_helper.h | 3 +- 3 files changed, 100 insertions(+), 72 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a2443b0f6..1e86e8e24 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -5,6 +5,7 @@ * Lorenz Meier * Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; - uint64_t start_time = 0; bool status_changed = true; @@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - toggle_status_leds(&status, &armed, true); + control_status_leds(&status, &armed, true); /* now initialized */ commander_initialized = true; @@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - // XXX not sure what should happen when voltage is low in flight - //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - // XXX should we still allow to arm with critical battery? - //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -1166,9 +1164,6 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; - - } else { - status_changed = false; } hrt_abstime t1 = hrt_absolute_time(); @@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; - toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + int blink_state = blink_msg_state(); + if (blink_state > 0) { + /* blinking LED message, don't touch LEDs */ + if (blink_state == 2) { + /* blinking LED message completed, restore normal state */ + control_status_leds(&status, &armed, true); + } + } else { + /* normal state */ + control_status_leds(&status, &armed, status_changed); + } + + status_changed = false; usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { + /* driving rgbled */ + if (changed) { + bool set_normal_color = false; + /* set mode */ + if (status->arming_state == ARMING_STATE_ARMED) { + rgbled_set_mode(RGBLED_MODE_ON); + set_normal_color = true; + + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + rgbled_set_color(RGBLED_COLOR_RED); + + } else if (status->arming_state == ARMING_STATE_STANDBY) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; + + } else { // STANDBY_ERROR and other states + rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); + rgbled_set_color(RGBLED_COLOR_RED); + } + + 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); + } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + + } else { + if (status->condition_local_position_valid) { + rgbled_set_color(RGBLED_COLOR_GREEN); + + } else { + rgbled_set_color(RGBLED_COLOR_BLUE); + } + } + } + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ @@ -1298,54 +1345,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif - if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ - - if (armed->armed) { - rgbled_set_mode(RGBLED_MODE_ON); - - } else if (armed->ready_to_arm) { - rgbled_set_mode(RGBLED_MODE_BREATHE); - - } else { - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - } - } - - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - - default: - break; - } - - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - - default: - break; - } - } - /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { if (leds_counter % 2 == 0) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 017499cb5..565b4b66a 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,7 +56,6 @@ #include #include - #include "commander_helper.h" /* oddly, ERROR is not defined for c++ */ @@ -64,21 +64,24 @@ #endif static const int ERROR = -1; +#define BLINK_MSG_TIME 700000 // 3 fast blinks + bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } static int buzzer; +static hrt_abstime blink_msg_end; int buzzer_init() { @@ -104,16 +107,25 @@ void tune_error() void tune_positive() { + 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); } void tune_neutral() { + 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() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_RED); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } @@ -132,18 +144,31 @@ 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); } +int blink_msg_state() +{ + if (blink_msg_end == 0) { + return 0; + + } else if (hrt_absolute_time() > blink_msg_end) { + return 2; + + } else { + return 1; + } +} + static int leds; static int rgbleds; int led_init() { + blink_msg_end = 0; + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); @@ -159,6 +184,7 @@ int led_init() warnx("Blue LED: ioctl fail\n"); return ERROR; } + #endif if (ioctl(leds, LED_ON, LED_AMBER)) { @@ -168,6 +194,7 @@ int led_init() /* 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); @@ -203,19 +230,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -void rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } -void rgbled_set_mode(rgbled_mode_t mode) { +void rgbled_set_mode(rgbled_mode_t mode) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } -void rgbled_set_pattern(rgbled_pattern_t *pattern) { +void rgbled_set_pattern(rgbled_pattern_t *pattern) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 027d0535f..e9514446c 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,7 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); +int blink_msg_state(); int led_init(void); void led_deinit(void); @@ -70,9 +71,7 @@ int led_on(int led); int led_off(int led); void rgbled_set_color(rgbled_color_t color); - void rgbled_set_mode(rgbled_mode_t mode); - void rgbled_set_pattern(rgbled_pattern_t *pattern); /** -- cgit v1.2.3