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

commander: Fix status checks for leds and adjust stack size based on actual use

This commit is contained in:
Lorenz Meier 2015-03-15 14:33:22 +01:00
parent 9db48df3d6
commit 49b3906b78
2 changed files with 37 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
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, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
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) {

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

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