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

Merge pull request #19 from cvg/rgbled 

RGBLED changes
This commit is contained in:
Lorenz Meier 2013-08-29 11:21:12 -07:00
parent 94d678098c 9bcfe49cff
commit 5fba00f158
5 changed files with 87 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/blinkm/blinkm.cpp
View File

@ -541,7 +541,7 @@ BlinkM::led()
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
@ -553,7 +553,7 @@ BlinkM::led()
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;

2
src/drivers/drv_rgbled.h
View File

@ -78,6 +78,7 @@
/** set pattern */
#define RGBLED_SET_PATTERN _RGBLEDIOC(7)
/*
structure passed to RGBLED_SET_RGB ioctl()
Note that the driver scales the brightness to 0 to 255, regardless
@ -115,6 +116,7 @@ typedef enum {
RGBLED_MODE_BLINK_SLOW,
RGBLED_MODE_BLINK_NORMAL,
RGBLED_MODE_BLINK_FAST,
RGBLED_MODE_BREATHE,
RGBLED_MODE_PATTERN
} rgbled_mode_t;

46
src/drivers/rgbled/rgbled.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012, 2013 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
@ -96,6 +96,11 @@ private:
rgbled_mode_t _mode;
rgbled_pattern_t _pattern;
float _brightness;
uint8_t _r;
uint8_t _g;
uint8_t _b;
bool _should_run;
bool _running;
int _led_interval;
@ -104,6 +109,7 @@ private:
void set_color(rgbled_color_t ledcolor);
void set_mode(rgbled_mode_t mode);
void set_pattern(rgbled_pattern_t *pattern);
void set_brightness(float brightness);
static void led_trampoline(void *arg);
void led();
@ -128,6 +134,10 @@ RGBLED::RGBLED(int bus, int rgbled) :
_color(RGBLED_COLOR_OFF),
_mode(RGBLED_MODE_OFF),
_running(false),
_brightness(1.0f),
_r(0),
_g(0),
_b(0),
_led_interval(0),
_counter(0)
{
@ -191,7 +201,6 @@ int
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = ENOTTY;
switch (cmd) {
case RGBLED_SET_RGB:
/* set the specified RGB values */
@ -246,6 +255,15 @@ RGBLED::led()
else
set_on(false);
break;
case RGBLED_MODE_BREATHE:
if (_counter >= 30)
_counter = 0;
if (_counter <= 15) {
set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
} else {
set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
}
break;
case RGBLED_MODE_PATTERN:
/* don't run out of the pattern array and stop if the next frame is 0 */
if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
@ -294,7 +312,7 @@ RGBLED::set_color(rgbled_color_t color) {
case RGBLED_COLOR_AMBER: // amber
set_rgb(255,20,0);
break;
case RGBLED_COLOR_DIM_RED: // red
case RGBLED_COLOR_DIM_RED: // red
set_rgb(90,0,0);
break;
case RGBLED_COLOR_DIM_YELLOW: // yellow
@ -306,7 +324,7 @@ RGBLED::set_color(rgbled_color_t color) {
case RGBLED_COLOR_DIM_GREEN: // green
set_rgb(0,90,0);
break;
case RGBLED_COLOR_DIM_BLUE: // blue
case RGBLED_COLOR_DIM_BLUE: // blue
set_rgb(0,0,90);
break;
case RGBLED_COLOR_DIM_WHITE: // white
@ -347,6 +365,12 @@ RGBLED::set_mode(rgbled_mode_t mode)
_should_run = true;
_led_interval = 100;
break;
case RGBLED_MODE_BREATHE:
_should_run = true;
set_on(true);
_counter = 0;
_led_interval = 1000/15;
break;
case RGBLED_MODE_PATTERN:
_should_run = true;
set_on(true);
@ -377,6 +401,13 @@ RGBLED::set_pattern(rgbled_pattern_t *pattern)
set_mode(RGBLED_MODE_PATTERN);
}
void
RGBLED::set_brightness(float brightness) {
_brightness = brightness;
set_rgb(_r, _g, _b);
}
int
RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
{
@ -413,7 +444,12 @@ RGBLED::set_on(bool on)
int
RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
{
const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)};
/* save the RGB values in case we want to change the brightness later */
_r = r;
_g = g;
_b = b;
const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)};
return transfer(msg, sizeof(msg), nullptr, 0);
}

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

@ -689,6 +689,8 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s info;
memset(&info, 0, sizeof(info));
toggle_status_leds(&status, &armed, true);
/* now initialized */
commander_initialized = true;
thread_running = true;
@ -890,7 +892,7 @@ int commander_thread_main(int argc, char *argv[])
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
}
@ -902,7 +904,7 @@ int commander_thread_main(int argc, char *argv[])
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
@ -1160,12 +1162,12 @@ int commander_thread_main(int argc, char *argv[])
if (tune_arm() == OK)
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* play tune on battery warning */
if (tune_low_bat() == OK)
battery_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
if (tune_critical_bat() == OK)
battery_tune_played = true;
@ -1252,72 +1254,45 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
if (changed) {
int i;
rgbled_pattern_t pattern;
memset(&pattern, 0, sizeof(pattern));
/* XXX TODO blink fast when armed and serious error occurs */
if (armed->armed) {
/* armed, solid */
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[0] = 1000;
rgbled_set_mode(RGBLED_MODE_ON);
} else if (armed->ready_to_arm) {
for (i = 0; i < 3; i++) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[i * 2] = 200;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 800;
}
if (status->condition_global_position_valid) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i * 2] = 1000;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 800;
} else {
for (i = 3; i < 6; i++) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i * 2] = 100;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 100;
}
pattern.color[6 * 2] = RGBLED_COLOR_OFF;
pattern.duration[6 * 2] = 700;
}
rgbled_set_mode(RGBLED_MODE_BREATHE);
} else {
for (i = 0; i < 3; i++) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
pattern.duration[i * 2] = 200;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 200;
}
/* not ready to arm, blink at 10Hz */
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
}
rgbled_set_pattern(&pattern);
}
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 */

6
src/modules/uORB/topics/vehicle_status.h
View File

@ -153,9 +153,9 @@ enum VEHICLE_TYPE {
};
enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */
VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */
VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */
};
/**