zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

merged systemstate into blinkm driver

sbg
Marco Bauer 12 years ago
parent
2f94a7a2b7
commit
41cef1d6c5
1 changed files with 472 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 492
      apps/drivers/blinkm/blinkm.cpp

492
apps/drivers/blinkm/blinkm.cpp
Unescape View File

@ -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);
return OK;
}
/* start the system monitor as a low-priority workqueue entry */ int
work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1); 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 void
BlinkM::monitor() BlinkM::led()
{ {
/* check system state, possibly update LED to suit */
/* re-queue ourselves to run again later */ static int vehicle_status_sub_fd;
work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval); 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.");
} }

Write Preview
Loading…
Cancel
Save