forked from Archive/PX4-Autopilot
merged systemstate into blinkm driver
This commit is contained in:
Marco Bauer
parent
2f94a7a2b7
commit
41cef1d6c5
|
|
@ -35,9 +35,57 @@
|
||||||
* @file blinkm.cpp
|
* @file blinkm.cpp
|
||||||
*
|
*
|
||||||
* Driver for the BlinkM LED controller connected via I2C.
|
* Driver for the BlinkM LED controller connected via I2C.
|
||||||
|
*
|
||||||
|
* Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
|
||||||
|
* blinkm start
|
||||||
|
*
|
||||||
|
* To start the system monitor put in the next line after the blinm start:
|
||||||
|
* blinkm systemmonitor
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* Description:
|
||||||
|
* After startup, the Application checked how many lipo cells are connected to the System.
|
||||||
|
* The recognized number off cells, will be blinked 5 times in purple color.
|
||||||
|
* 2 Cells = 2 blinks
|
||||||
|
* ...
|
||||||
|
* 5 Cells = 5 blinks
|
||||||
|
* Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
|
||||||
|
*
|
||||||
|
* System disarmed:
|
||||||
|
* The BlinkM should lit solid red.
|
||||||
|
*
|
||||||
|
* System armed:
|
||||||
|
* One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
|
||||||
|
*
|
||||||
|
* X-X-X-X-_-_-_-_-
|
||||||
|
* -------------------------
|
||||||
|
* G G G M
|
||||||
|
* P P P O
|
||||||
|
* S S S D
|
||||||
|
* E
|
||||||
|
*
|
||||||
|
* (X = on, _=off)
|
||||||
|
*
|
||||||
|
* The first 3 Blinks indicates the status of the GPS-Signal:
|
||||||
|
* 0-4 satellites = X-X-X-X-_-_-_-_-
|
||||||
|
* 5 satellites = X-X-_-X-_-_-_-_-
|
||||||
|
* 6 satellites = X-_-_-X-_-_-_-_-
|
||||||
|
* >=7 satellites = _-_-_-X-_-_-_-_-
|
||||||
|
*
|
||||||
|
* The fourth Blink indicates the Flightmode:
|
||||||
|
* MANUAL : off
|
||||||
|
* STABILIZED : yellow
|
||||||
|
* HOLD : blue
|
||||||
|
* AUTO : green
|
||||||
|
*
|
||||||
|
* Battery Warning (low Battery Level):
|
||||||
|
* Continuously blinking in yellow X-X-X-X-X-X-X-X
|
||||||
|
*
|
||||||
|
* Battery Alert (critical Battery Level)
|
||||||
|
* Continuously blinking in red X-X-X-X-X-X-X-X
|
||||||
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#include <nuttx/config.h>
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
#include <drivers/device/i2c.h>
|
#include <drivers/device/i2c.h>
|
||||||
|
|
@ -59,6 +107,12 @@
|
||||||
#include <systemlib/perf_counter.h>
|
#include <systemlib/perf_counter.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <poll.h>
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/vehicle_gps_position.h>
|
||||||
|
|
||||||
class BlinkM : public device::I2C
|
class BlinkM : public device::I2C
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
|
@ -67,7 +121,7 @@ public:
|
||||||
|
|
||||||
virtual int init();
|
virtual int init();
|
||||||
virtual int probe();
|
virtual int probe();
|
||||||
|
virtual int setMode(int mode);
|
||||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
static const char *script_names[];
|
static const char *script_names[];
|
||||||
|
|
@ -96,10 +150,20 @@ private:
|
||||||
};
|
};
|
||||||
|
|
||||||
work_s _work;
|
work_s _work;
|
||||||
static const unsigned _monitor_interval = 250;
|
|
||||||
|
|
||||||
static void monitor_trampoline(void *arg);
|
static int led_color_1;
|
||||||
void monitor();
|
static int led_color_2;
|
||||||
|
static int led_color_3;
|
||||||
|
static int led_color_4;
|
||||||
|
static int led_color_5;
|
||||||
|
static int led_color_6;
|
||||||
|
static int led_blink;
|
||||||
|
|
||||||
|
static int systemstate_run;
|
||||||
|
|
||||||
|
void setLEDColor(int ledcolor);
|
||||||
|
static void led_trampoline(void *arg);
|
||||||
|
void led();
|
||||||
|
|
||||||
int set_rgb(uint8_t r, uint8_t g, uint8_t b);
|
int set_rgb(uint8_t r, uint8_t g, uint8_t b);
|
||||||
|
|
||||||
|
|
@ -127,8 +191,7 @@ private:
|
||||||
/* for now, we only support one BlinkM */
|
/* for now, we only support one BlinkM */
|
||||||
namespace
|
namespace
|
||||||
{
|
{
|
||||||
BlinkM *g_blinkm;
|
BlinkM *g_blinkm;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* list of script names, must match script ID numbers */
|
/* list of script names, must match script ID numbers */
|
||||||
|
|
@ -155,11 +218,36 @@ const char *BlinkM::script_names[] = {
|
||||||
nullptr
|
nullptr
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#define MAX_CELL_VOLTAGE 43 /* cell voltage if full charged */
|
||||||
|
|
||||||
|
#define LED_OFF 0
|
||||||
|
#define LED_RED 1
|
||||||
|
#define LED_YELLOW 2
|
||||||
|
#define LED_PURPLE 3
|
||||||
|
#define LED_GREEN 4
|
||||||
|
#define LED_BLUE 5
|
||||||
|
#define LED_WHITE 6
|
||||||
|
#define LED_ONTIME 100
|
||||||
|
#define LED_OFFTIME 100
|
||||||
|
#define LED_BLINK 1
|
||||||
|
#define LED_NOBLINK 0
|
||||||
|
|
||||||
|
int BlinkM::led_color_1 = LED_OFF;
|
||||||
|
int BlinkM::led_color_2 = LED_OFF;
|
||||||
|
int BlinkM::led_color_3 = LED_OFF;
|
||||||
|
int BlinkM::led_color_4 = LED_OFF;
|
||||||
|
int BlinkM::led_color_5 = LED_OFF;
|
||||||
|
int BlinkM::led_color_6 = LED_OFF;
|
||||||
|
int BlinkM::led_blink = LED_NOBLINK;
|
||||||
|
|
||||||
|
int BlinkM::systemstate_run = 0;
|
||||||
|
|
||||||
extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
|
extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
|
||||||
|
|
||||||
BlinkM::BlinkM(int bus) :
|
BlinkM::BlinkM(int bus) :
|
||||||
I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000)
|
I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000)
|
||||||
{
|
{
|
||||||
|
memset(&_work, 0, sizeof(_work));
|
||||||
}
|
}
|
||||||
|
|
||||||
BlinkM::~BlinkM()
|
BlinkM::~BlinkM()
|
||||||
|
|
@ -170,7 +258,6 @@ int
|
||||||
BlinkM::init()
|
BlinkM::init()
|
||||||
{
|
{
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
ret = I2C::init();
|
ret = I2C::init();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
|
|
@ -179,13 +266,44 @@ BlinkM::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* set some sensible defaults */
|
/* set some sensible defaults */
|
||||||
set_fade_speed(25);
|
set_fade_speed(255);
|
||||||
|
|
||||||
/* turn off by default */
|
/* turn off by default */
|
||||||
play_script(BLACK);
|
play_script(BLACK);
|
||||||
|
set_fade_speed(255);
|
||||||
|
stop_script();
|
||||||
|
set_rgb(0,0,0);
|
||||||
|
|
||||||
/* start the system monitor as a low-priority workqueue entry */
|
return OK;
|
||||||
work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1);
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
BlinkM::setMode(int mode)
|
||||||
|
{
|
||||||
|
if(mode == 1) {
|
||||||
|
if(BlinkM::systemstate_run == 0) {
|
||||||
|
/* set some sensible defaults */
|
||||||
|
set_fade_speed(255);
|
||||||
|
|
||||||
|
/* turn off by default */
|
||||||
|
play_script(BLACK);
|
||||||
|
set_fade_speed(255);
|
||||||
|
stop_script();
|
||||||
|
set_rgb(0,0,0);
|
||||||
|
BlinkM::systemstate_run = 1;
|
||||||
|
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
BlinkM::systemstate_run = 0;
|
||||||
|
usleep(1000000);
|
||||||
|
/* set some sensible defaults */
|
||||||
|
set_fade_speed(255);
|
||||||
|
/* turn off by default */
|
||||||
|
play_script(BLACK);
|
||||||
|
set_fade_speed(255);
|
||||||
|
stop_script();
|
||||||
|
set_rgb(0,0,0);
|
||||||
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
@ -249,21 +367,343 @@ BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void
|
void
|
||||||
BlinkM::monitor_trampoline(void *arg)
|
BlinkM::led_trampoline(void *arg)
|
||||||
{
|
{
|
||||||
BlinkM *bm = (BlinkM *)arg;
|
BlinkM *bm = (BlinkM *)arg;
|
||||||
|
|
||||||
bm->monitor();
|
bm->led();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
|
||||||
BlinkM::monitor()
|
|
||||||
{
|
|
||||||
/* check system state, possibly update LED to suit */
|
|
||||||
|
|
||||||
/* re-queue ourselves to run again later */
|
|
||||||
work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval);
|
void
|
||||||
|
BlinkM::led()
|
||||||
|
{
|
||||||
|
|
||||||
|
static int vehicle_status_sub_fd;
|
||||||
|
static int vehicle_gps_position_sub_fd;
|
||||||
|
|
||||||
|
static int num_of_cells = 0;
|
||||||
|
static int detected_cells_runcount = 0;
|
||||||
|
static int t_led_color_1 = 0;
|
||||||
|
static int t_led_color_2 = 0;
|
||||||
|
static int t_led_color_3 = 0;
|
||||||
|
static int t_led_color_4 = 0;
|
||||||
|
static int t_led_color_5 = 0;
|
||||||
|
static int t_led_color_6 = 0;
|
||||||
|
static int t_led_blink = 0;
|
||||||
|
static int led_thread_runcount=1;
|
||||||
|
static int led_interval = 1000;
|
||||||
|
|
||||||
|
static bool topic_initialized = false;
|
||||||
|
static bool detected_cells_blinked = false;
|
||||||
|
static bool led_thread_ready = true;
|
||||||
|
|
||||||
|
int system_voltage = 0;
|
||||||
|
int num_of_used_sats = 0;
|
||||||
|
int poll_ret;
|
||||||
|
|
||||||
|
if(!topic_initialized) {
|
||||||
|
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
|
||||||
|
orb_set_interval(vehicle_status_sub_fd, 1000);
|
||||||
|
|
||||||
|
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||||
|
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
|
||||||
|
|
||||||
|
topic_initialized = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
pollfd fds[2];
|
||||||
|
fds[0].fd = vehicle_status_sub_fd;
|
||||||
|
fds[0].events = POLLIN;
|
||||||
|
fds[1].fd = vehicle_gps_position_sub_fd;
|
||||||
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
|
if(led_thread_ready == true) {
|
||||||
|
if(!detected_cells_blinked) {
|
||||||
|
if(num_of_cells > 0) {
|
||||||
|
t_led_color_1 = LED_PURPLE;
|
||||||
|
}
|
||||||
|
if(num_of_cells > 1) {
|
||||||
|
t_led_color_2 = LED_PURPLE;
|
||||||
|
}
|
||||||
|
if(num_of_cells > 2) {
|
||||||
|
t_led_color_3 = LED_PURPLE;
|
||||||
|
}
|
||||||
|
if(num_of_cells > 3) {
|
||||||
|
t_led_color_4 = LED_PURPLE;
|
||||||
|
}
|
||||||
|
if(num_of_cells > 4) {
|
||||||
|
t_led_color_5 = LED_PURPLE;
|
||||||
|
}
|
||||||
|
t_led_color_6 = LED_OFF;
|
||||||
|
t_led_blink = LED_BLINK;
|
||||||
|
} else {
|
||||||
|
t_led_color_1 = BlinkM::led_color_1;
|
||||||
|
t_led_color_2 = BlinkM::led_color_2;
|
||||||
|
t_led_color_3 = BlinkM::led_color_3;
|
||||||
|
t_led_color_4 = BlinkM::led_color_4;
|
||||||
|
t_led_color_5 = BlinkM::led_color_5;
|
||||||
|
t_led_color_6 = BlinkM::led_color_6;
|
||||||
|
t_led_blink = BlinkM::led_blink;
|
||||||
|
}
|
||||||
|
led_thread_ready = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch(led_thread_runcount) {
|
||||||
|
case 1: // 1. LED on
|
||||||
|
BlinkM::setLEDColor(t_led_color_1);
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_ONTIME;
|
||||||
|
break;
|
||||||
|
case 2: // 1. LED off
|
||||||
|
if(t_led_blink == LED_BLINK) {
|
||||||
|
BlinkM::setLEDColor(LED_OFF);
|
||||||
|
}
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_OFFTIME;
|
||||||
|
break;
|
||||||
|
case 3: // 2. LED on
|
||||||
|
BlinkM::setLEDColor(t_led_color_2);
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_ONTIME;
|
||||||
|
break;
|
||||||
|
case 4: // 2. LED off
|
||||||
|
if(t_led_blink == LED_BLINK) {
|
||||||
|
BlinkM::setLEDColor(LED_OFF);
|
||||||
|
}
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_OFFTIME;
|
||||||
|
break;
|
||||||
|
case 5: // 3. LED on
|
||||||
|
BlinkM::setLEDColor(t_led_color_3);
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_ONTIME;
|
||||||
|
break;
|
||||||
|
case 6: // 3. LED off
|
||||||
|
if(t_led_blink == LED_BLINK) {
|
||||||
|
BlinkM::setLEDColor(LED_OFF);
|
||||||
|
}
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_OFFTIME;
|
||||||
|
break;
|
||||||
|
case 7: // 4. LED on
|
||||||
|
BlinkM::setLEDColor(t_led_color_4);
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_ONTIME;
|
||||||
|
break;
|
||||||
|
case 8: // 4. LED off
|
||||||
|
if(t_led_blink == LED_BLINK) {
|
||||||
|
BlinkM::setLEDColor(LED_OFF);
|
||||||
|
}
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_OFFTIME;
|
||||||
|
break;
|
||||||
|
case 9: // 5. LED on
|
||||||
|
BlinkM::setLEDColor(t_led_color_5);
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_ONTIME;
|
||||||
|
break;
|
||||||
|
case 10: // 5. LED off
|
||||||
|
if(t_led_blink == LED_BLINK) {
|
||||||
|
BlinkM::setLEDColor(LED_OFF);
|
||||||
|
}
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_OFFTIME;
|
||||||
|
break;
|
||||||
|
case 11: // 6. LED on
|
||||||
|
BlinkM::setLEDColor(t_led_color_6);
|
||||||
|
led_thread_runcount++;
|
||||||
|
led_interval = LED_ONTIME;
|
||||||
|
break;
|
||||||
|
case 12: // 6. LED off
|
||||||
|
if(t_led_blink == LED_BLINK) {
|
||||||
|
BlinkM::setLEDColor(LED_OFF);
|
||||||
|
}
|
||||||
|
|
||||||
|
//poll_ret = ::poll(&fds[0],1,1000);
|
||||||
|
poll_ret = ::poll(fds, 2, 1000);
|
||||||
|
|
||||||
|
if (poll_ret == 0) {
|
||||||
|
/* this means none of our providers is giving us data */
|
||||||
|
printf("[blinkm_systemstate_sensor] Got no data within a second\n");
|
||||||
|
} else if (poll_ret < 0) {
|
||||||
|
/* this is seriously bad - should be an emergency */
|
||||||
|
log("poll error %d", errno);
|
||||||
|
usleep(1000000);
|
||||||
|
} else {
|
||||||
|
if (fds[0].revents & POLLIN) {
|
||||||
|
/* obtained data for the first file descriptor */
|
||||||
|
struct vehicle_status_s vehicle_status_raw;
|
||||||
|
struct vehicle_gps_position_s vehicle_gps_position_raw;
|
||||||
|
/* copy sensors raw data into local buffer */
|
||||||
|
|
||||||
|
/* vehicle_status */
|
||||||
|
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
|
||||||
|
|
||||||
|
/* vehicle_gps_position */
|
||||||
|
orb_copy(ORB_ID(vehicle_gps_position), vehicle_status_sub_fd, &vehicle_gps_position_raw);
|
||||||
|
|
||||||
|
/* get actual battery voltage */
|
||||||
|
system_voltage = (int)vehicle_status_raw.voltage_battery*10;
|
||||||
|
|
||||||
|
/* get number of used satellites in navigation */
|
||||||
|
num_of_used_sats = 0;
|
||||||
|
for(int satloop=0; satloop<20; satloop++) {
|
||||||
|
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
|
||||||
|
num_of_used_sats++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(num_of_cells == 0) {
|
||||||
|
/* looking for lipo cells that are connected */
|
||||||
|
printf("<blinkm> checking cells\n");
|
||||||
|
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
|
||||||
|
if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
|
||||||
|
}
|
||||||
|
printf("<blinkm> cells found:%u\n", num_of_cells);
|
||||||
|
} else {
|
||||||
|
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
|
||||||
|
/* LED Pattern for battery low warning */
|
||||||
|
BlinkM::led_color_1 = LED_YELLOW;
|
||||||
|
BlinkM::led_color_2 = LED_YELLOW;
|
||||||
|
BlinkM::led_color_3 = LED_YELLOW;
|
||||||
|
BlinkM::led_color_4 = LED_YELLOW;
|
||||||
|
BlinkM::led_color_5 = LED_YELLOW;
|
||||||
|
BlinkM::led_color_6 = LED_YELLOW;
|
||||||
|
BlinkM::led_blink = LED_BLINK;
|
||||||
|
|
||||||
|
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
|
||||||
|
/* LED Pattern for battery critical alerting */
|
||||||
|
BlinkM::led_color_1 = LED_RED;
|
||||||
|
BlinkM::led_color_2 = LED_RED;
|
||||||
|
BlinkM::led_color_3 = LED_RED;
|
||||||
|
BlinkM::led_color_4 = LED_RED;
|
||||||
|
BlinkM::led_color_5 = LED_RED;
|
||||||
|
BlinkM::led_color_6 = LED_RED;
|
||||||
|
BlinkM::led_blink = LED_BLINK;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
/* no battery warnings here */
|
||||||
|
|
||||||
|
if(vehicle_status_raw.flag_system_armed == false) {
|
||||||
|
/* system not armed */
|
||||||
|
BlinkM::led_color_1 = LED_RED;
|
||||||
|
BlinkM::led_color_2 = LED_RED;
|
||||||
|
BlinkM::led_color_3 = LED_RED;
|
||||||
|
BlinkM::led_color_4 = LED_RED;
|
||||||
|
BlinkM::led_color_5 = LED_RED;
|
||||||
|
BlinkM::led_color_6 = LED_RED;
|
||||||
|
BlinkM::led_blink = LED_NOBLINK;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
/* armed system - initial led pattern */
|
||||||
|
BlinkM::led_color_1 = LED_RED;
|
||||||
|
BlinkM::led_color_2 = LED_RED;
|
||||||
|
BlinkM::led_color_3 = LED_RED;
|
||||||
|
BlinkM::led_color_4 = LED_OFF;
|
||||||
|
BlinkM::led_color_5 = LED_OFF;
|
||||||
|
BlinkM::led_color_6 = LED_OFF;
|
||||||
|
BlinkM::led_blink = LED_BLINK;
|
||||||
|
|
||||||
|
/* handle 4th led - flightmode indicator */
|
||||||
|
switch((int)vehicle_status_raw.flight_mode) {
|
||||||
|
case VEHICLE_FLIGHT_MODE_MANUAL:
|
||||||
|
BlinkM::led_color_4 = LED_OFF;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case VEHICLE_FLIGHT_MODE_STAB:
|
||||||
|
BlinkM::led_color_4 = LED_YELLOW;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case VEHICLE_FLIGHT_MODE_HOLD:
|
||||||
|
BlinkM::led_color_4 = LED_BLUE;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case VEHICLE_FLIGHT_MODE_AUTO:
|
||||||
|
BlinkM::led_color_4 = LED_GREEN;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* handling used sat´s */
|
||||||
|
if(num_of_used_sats >= 7) {
|
||||||
|
BlinkM::led_color_1 = LED_OFF;
|
||||||
|
BlinkM::led_color_2 = LED_OFF;
|
||||||
|
BlinkM::led_color_3 = LED_OFF;
|
||||||
|
} else if(num_of_used_sats == 6) {
|
||||||
|
BlinkM::led_color_2 = LED_OFF;
|
||||||
|
BlinkM::led_color_3 = LED_OFF;
|
||||||
|
} else if(num_of_used_sats == 5) {
|
||||||
|
BlinkM::led_color_3 = LED_OFF;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
|
||||||
|
vehicle_status_raw.voltage_battery,
|
||||||
|
vehicle_status_raw.flag_system_armed,
|
||||||
|
vehicle_status_raw.flight_mode,
|
||||||
|
num_of_cells,
|
||||||
|
vehicle_status_raw.battery_warning,
|
||||||
|
num_of_used_sats,
|
||||||
|
vehicle_gps_position_raw.fix_type,
|
||||||
|
vehicle_gps_position_raw.satellites_visible);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
led_thread_runcount=1;
|
||||||
|
led_thread_ready = true;
|
||||||
|
led_interval = LED_OFFTIME;
|
||||||
|
|
||||||
|
if(detected_cells_runcount < 5){
|
||||||
|
detected_cells_runcount++;
|
||||||
|
} else {
|
||||||
|
detected_cells_blinked = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
led_thread_runcount=1;
|
||||||
|
t_led_blink = 0;
|
||||||
|
led_thread_ready = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(BlinkM::systemstate_run == 1) {
|
||||||
|
/* re-queue ourselves to run again later */
|
||||||
|
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void BlinkM::setLEDColor(int ledcolor) {
|
||||||
|
switch (ledcolor) {
|
||||||
|
case LED_OFF: // off
|
||||||
|
BlinkM::set_rgb(0,0,0);
|
||||||
|
break;
|
||||||
|
case LED_RED: // red
|
||||||
|
BlinkM::set_rgb(255,0,0);
|
||||||
|
break;
|
||||||
|
case LED_YELLOW: // yellow
|
||||||
|
BlinkM::set_rgb(255,70,0);
|
||||||
|
break;
|
||||||
|
case LED_PURPLE: // purple
|
||||||
|
BlinkM::set_rgb(255,0,255);
|
||||||
|
break;
|
||||||
|
case LED_GREEN: // green
|
||||||
|
BlinkM::set_rgb(0,255,0);
|
||||||
|
break;
|
||||||
|
case LED_BLUE: // blue
|
||||||
|
BlinkM::set_rgb(0,0,255);
|
||||||
|
break;
|
||||||
|
case LED_WHITE: // white
|
||||||
|
BlinkM::set_rgb(255,255,255);
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
|
@ -413,7 +853,6 @@ BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int
|
int
|
||||||
BlinkM::get_firmware_version(uint8_t version[2])
|
BlinkM::get_firmware_version(uint8_t version[2])
|
||||||
{
|
{
|
||||||
|
|
@ -443,9 +882,21 @@ blinkm_main(int argc, char *argv[])
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
if (g_blinkm == nullptr)
|
if (g_blinkm == nullptr)
|
||||||
errx(1, "not started");
|
errx(1, "not started");
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "systemstate")) {
|
||||||
|
g_blinkm->setMode(1);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "ledoff")) {
|
||||||
|
g_blinkm->setMode(0);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
if (!strcmp(argv[1], "list")) {
|
if (!strcmp(argv[1], "list")) {
|
||||||
for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
|
for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
|
||||||
fprintf(stderr, " %s\n", BlinkM::script_names[i]);
|
fprintf(stderr, " %s\n", BlinkM::script_names[i]);
|
||||||
|
|
@ -458,8 +909,9 @@ blinkm_main(int argc, char *argv[])
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "can't open BlinkM device");
|
err(1, "can't open BlinkM device");
|
||||||
|
|
||||||
|
g_blinkm->setMode(0);
|
||||||
if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
|
if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
|
||||||
exit(0);
|
exit(0);
|
||||||
|
|
||||||
errx(1, "missing command, try 'start', 'list' or a script name");
|
errx(1, "missing command, try 'start', 'systemstate', 'ledoff', 'list' or a script name.");
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue