Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

Added dim RGB led support, not operational yet

Browse Source
This commit is contained in:
Lorenz Meier
2013-08-17 15:47:42 +02:00
parent 6c45d9cb5c
commit 6ff3f51f88
1 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+25 -10
View File
@@ -52,6 +52,7 @@
#include <errno.h>
#include <debug.h>
#include <sys/prctl.h>
#include <sys/stat.h>
#include <string.h>
#include <math.h>
#include <poll.h>
@@ -153,6 +154,8 @@ static unsigned int failsafe_lowlevel_timeout_ms;
static unsigned int leds_counter;
/* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0;
/* if connected via USB */
static bool on_usb_power = false;
/* tasks waiting for low prio thread */
@@ -205,6 +208,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st
void print_reject_mode(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
/**
@@ -244,6 +249,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\tcommander is running\n");
print_status();
} else {
warnx("\tcommander not started\n");
@@ -265,6 +271,10 @@ void usage(const char *reason)
exit(1);
}
void print_status() {
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
}
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
@@ -865,9 +875,14 @@ int commander_thread_main(int argc, char *argv[])
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
last_idle_time = system_load.tasks[0].total_runtime;
/* check if board is connected via USB */
struct stat statbuf;
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
warnx("bat remaining: %2.2f", status.battery_remaining);
// XXX remove later
//warnx("bat remaining: %2.2f", status.battery_remaining);
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
@@ -1362,22 +1377,22 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
if (armed->armed) {
/* armed, solid */
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[0] = RGBLED_COLOR_AMBER;
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] = RGBLED_COLOR_RED;
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[0] = RGBLED_COLOR_GREEN;
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN;
}
pattern.duration[0] = 1000;
} else if (armed->ready_to_arm) {
for (i=0; i<3; i++) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[i*2] = RGBLED_COLOR_AMBER;
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] = RGBLED_COLOR_RED;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[i*2] = RGBLED_COLOR_GREEN;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[i*2] = 200;
@@ -1385,13 +1400,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
pattern.duration[i*2+1] = 800;
}
if (status->condition_global_position_valid) {
pattern.color[i*2] = RGBLED_COLOR_BLUE;
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] = RGBLED_COLOR_BLUE;
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;
@@ -1402,7 +1417,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
} else {
for (i=0; i<3; i++) {
pattern.color[i*2] = RGBLED_COLOR_RED;
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;