From 49b3906b7827a7f2b5b4e947d080db7496d044da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 14:33:22 +0100 Subject: commander: Fix status checks for leds and adjust stack size based on actual use --- src/modules/commander/commander.cpp | 27 +++++++++++++------------ src/modules/commander/commander_helper.cpp | 32 +++++++++++++++++++++--------- 2 files changed, 37 insertions(+), 22 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index aa7a17a22..763c617bf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -967,19 +967,6 @@ int commander_thread_main(int argc, char *argv[]) int ret; - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2000); - - struct sched_param param; - (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); - pthread_attr_destroy(&commander_low_prio_attr); - /* Start monitoring loop */ unsigned counter = 0; unsigned stick_off_counter = 0; @@ -1144,6 +1131,20 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = false; bool failsafe_old = false; + /* initialize low priority thread */ + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 1600); + + struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index b5ec6c4e6..a2e827a15 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -231,7 +231,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED0_DEVICE_PATH, 0); - if (rgbleds == -1) { + if (rgbleds < 0) { warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); } @@ -240,50 +240,64 @@ int led_init() void led_deinit() { - close(leds); + if (leds >= 0) { + close(leds); + } - if (rgbleds != -1) { + if (rgbleds >= 0) { close(rgbleds); } } int led_toggle(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_ON, led); } int led_off(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) -- cgit v1.2.3