fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: use led_control uorb topic

Browse Source
This commit is contained in:
Beat Küng 2017-02-24 10:16:54 +01:00 committed by Lorenz Meier
parent 8dae94d90a
commit 95e8e26ae0
3 changed files with 58 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;

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

@ -55,6 +55,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/led_control.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
@ -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);
}

11
src/modules/commander/commander_helper.h
View File

@ -46,7 +46,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <drivers/drv_rgbled.h>
#include <drivers/drv_led.h>
#include <drivers/drv_board_led.h>
@ -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_ */