From 95e8e26ae0ae85f27ac24d1ca3eda5440eaefb38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 24 Feb 2017 10:16:54 +0100 Subject: [PATCH] commander: use led_control uorb topic --- src/modules/commander/commander.cpp | 44 +++++++++------ src/modules/commander/commander_helper.cpp | 65 +++++++++------------- src/modules/commander/commander_helper.h | 11 ++-- 3 files changed, 58 insertions(+), 62 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fe72b4a7ae..bc35266dbe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3163,7 +3163,7 @@ int commander_thread_main(int argc, char *argv[]) warn("join failed: %d", ret); } - rgbled_set_mode(RGBLED_MODE_OFF); + rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF); /* close fds */ led_deinit(); @@ -3226,6 +3226,8 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* driving rgbled */ if (changed || last_overload != overload) { + uint8_t led_mode = led_control_s::MODE_OFF; + uint8_t led_color; bool set_normal_color = false; bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; @@ -3233,48 +3235,54 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* set mode */ if (overload && ((hrt_absolute_time() - overload_start) > overload_warn_delay)) { - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - rgbled_set_color(RGBLED_COLOR_PURPLE); - set_normal_color = false; + led_mode = led_control_s::MODE_BLINK_FAST; + led_color = led_control_s::COLOR_PURPLE; } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - rgbled_set_mode(RGBLED_MODE_ON); + led_color = led_control_s::MODE_ON; set_normal_color = true; - } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status_flags.condition_system_sensors_initialized && hotplug_timeout)) { - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - rgbled_set_color(RGBLED_COLOR_RED); + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || + (!status_flags.condition_system_sensors_initialized && hotplug_timeout)) { + led_mode = led_control_s::MODE_BLINK_FAST; + led_color = led_control_s::COLOR_RED; } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - rgbled_set_mode(RGBLED_MODE_BREATHE); + led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; } else if (!status_flags.condition_system_sensors_initialized && !hotplug_timeout) { - rgbled_set_mode(RGBLED_MODE_BREATHE); + led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - + }else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_INIT) { + // if in init status it should not be in the error state + led_mode = led_control_s::MODE_OFF; } else { // STANDBY_ERROR and other states - rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); - rgbled_set_color(RGBLED_COLOR_RED); + led_mode = led_control_s::MODE_BLINK_NORMAL; + led_color = led_control_s::COLOR_RED; } if (set_normal_color) { /* set color */ if (status.failsafe) { - rgbled_set_color(RGBLED_COLOR_PURPLE); + led_color = led_control_s::COLOR_PURPLE; + } else if (battery_local->warning == battery_status_s::BATTERY_WARNING_LOW) { - rgbled_set_color(RGBLED_COLOR_AMBER); + led_color = led_control_s::COLOR_AMBER; } else if (battery_local->warning == battery_status_s::BATTERY_WARNING_CRITICAL) { - rgbled_set_color(RGBLED_COLOR_RED); + led_color = led_control_s::COLOR_RED; } else { if (status_flags.condition_home_position_valid && status_flags.condition_global_position_valid) { - rgbled_set_color(RGBLED_COLOR_GREEN); + led_color = led_control_s::COLOR_GREEN; } else { - rgbled_set_color(RGBLED_COLOR_BLUE); + led_color = led_control_s::COLOR_BLUE; } } } + if (led_mode != led_control_s::MODE_OFF) { + rgbled_set_color_and_mode(led_color, led_mode); + } } last_overload = overload; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 15e1c8130e..f1cbff7010 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -79,7 +80,7 @@ using namespace DriverFramework; #define VEHICLE_TYPE_VTOL_RESERVED4 24 #define VEHICLE_TYPE_VTOL_RESERVED5 25 -#define BLINK_MSG_TIME 700000 // 3 fast blinks +#define BLINK_MSG_TIME 700000 // 3 fast blinks (in us) bool is_multirotor(const struct vehicle_status_s *current_status) { @@ -111,8 +112,9 @@ static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be int static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; static DevHandle h_leds; -static DevHandle h_rgbleds; static DevHandle h_buzzer; +static led_control_s led_control = {}; +static orb_advert_t led_control_pub = nullptr; int buzzer_init() { @@ -169,8 +171,7 @@ void set_tune(int tune) void tune_home_set(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); + rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { set_tune(TONE_HOME_SET); @@ -180,8 +181,7 @@ void tune_home_set(bool use_buzzer) void tune_mission_ok(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); + rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { set_tune(TONE_NOTIFY_NEUTRAL_TUNE); @@ -191,8 +191,7 @@ void tune_mission_ok(bool use_buzzer) void tune_mission_fail(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); + rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { set_tune(TONE_NOTIFY_NEGATIVE_TUNE); @@ -205,8 +204,7 @@ void tune_mission_fail(bool use_buzzer) 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); + rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); @@ -219,8 +217,7 @@ void tune_positive(bool use_buzzer) 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); + rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { set_tune(TONE_NOTIFY_NEUTRAL_TUNE); @@ -233,8 +230,7 @@ void tune_neutral(bool use_buzzer) 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); + rgbled_set_color_and_mode(led_control_s::COLOR_RED, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { set_tune(TONE_NOTIFY_NEGATIVE_TUNE); @@ -244,8 +240,7 @@ void tune_negative(bool use_buzzer) void tune_failsafe(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; - rgbled_set_color(RGBLED_COLOR_PURPLE); - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + rgbled_set_color_and_mode(led_control_s::COLOR_PURPLE, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { set_tune(TONE_BATTERY_WARNING_FAST_TUNE); @@ -270,6 +265,12 @@ int led_init() { blink_msg_end = 0; + led_control.led_mask = 0xff; + led_control.mode = led_control_s::MODE_OFF; + led_control.priority = 0; + led_control.timestamp = hrt_absolute_time(); + led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH); + #ifndef CONFIG_ARCH_BOARD_RPI /* first open normal LEDs */ DevMgr::getHandle(LED0_DEVICE_PATH, h_leds); @@ -295,23 +296,15 @@ int led_init() led_off(LED_AMBER); #endif - /* then try RGB LEDs, this can fail on FMUv1*/ - DevHandle h; - DevMgr::getHandle(RGBLED0_DEVICE_PATH, h_rgbleds); - - if (!h_rgbleds.isValid()) { - PX4_WARN("No RGB LED found at " RGBLED0_DEVICE_PATH); - } - return 0; } void led_deinit() { + orb_unadvertise(led_control_pub); #ifndef CONFIG_ARCH_BOARD_RPI DevMgr::releaseHandle(h_leds); #endif - DevMgr::releaseHandle(h_rgbleds); } int led_toggle(int led) @@ -329,20 +322,12 @@ int led_off(int led) return h_leds.ioctl(LED_OFF, led); } -void rgbled_set_color(rgbled_color_t color) +void rgbled_set_color_and_mode(uint8_t color, uint8_t mode) { - - h_rgbleds.ioctl(RGBLED_SET_COLOR, (unsigned long)color); -} - -void rgbled_set_mode(rgbled_mode_t mode) -{ - - h_rgbleds.ioctl(RGBLED_SET_MODE, (unsigned long)mode); -} - -void rgbled_set_pattern(rgbled_pattern_t *pattern) -{ - - h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern); + led_control.mode = mode; + led_control.color = color; + led_control.num_blinks = 0; + led_control.priority = 0; + led_control.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(led_control), led_control_pub, &led_control); } diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index df105c09c7..320d4a37b3 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include @@ -76,8 +76,11 @@ int led_toggle(int led); 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); +/** + * set the LED color & mode + * @param color @see led_control_s::COLOR_* + * @param mode @see led_control_s::MODE_* + */ +void rgbled_set_color_and_mode(uint8_t color, uint8_t mode); #endif /* COMMANDER_HELPER_H_ */