From 41cef1d6c5420ddf1193760678a908b4e624586f Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sat, 12 Jan 2013 10:26:37 +0100 Subject: [PATCH] merged systemstate into blinkm driver --- apps/drivers/blinkm/blinkm.cpp | 496 +++++++++++++++++++++++++++++++-- 1 file changed, 474 insertions(+), 22 deletions(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index d589025cce..19f3b4d731 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -35,9 +35,57 @@ * @file blinkm.cpp * * 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 #include @@ -59,6 +107,12 @@ #include #include +#include +#include +#include +#include +#include + class BlinkM : public device::I2C { public: @@ -67,7 +121,7 @@ public: virtual int init(); virtual int probe(); - + virtual int setMode(int mode); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); static const char *script_names[]; @@ -96,10 +150,20 @@ private: }; work_s _work; - static const unsigned _monitor_interval = 250; - static void monitor_trampoline(void *arg); - void monitor(); + static int led_color_1; + 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); @@ -127,8 +191,7 @@ private: /* for now, we only support one BlinkM */ namespace { -BlinkM *g_blinkm; - + BlinkM *g_blinkm; } /* list of script names, must match script ID numbers */ @@ -155,11 +218,36 @@ const char *BlinkM::script_names[] = { 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[]); BlinkM::BlinkM(int bus) : I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000) { + memset(&_work, 0, sizeof(_work)); } BlinkM::~BlinkM() @@ -170,7 +258,6 @@ int BlinkM::init() { int ret; - ret = I2C::init(); if (ret != OK) { @@ -179,13 +266,44 @@ BlinkM::init() } /* set some sensible defaults */ - set_fade_speed(25); + set_fade_speed(255); /* turn off by default */ play_script(BLACK); + set_fade_speed(255); + stop_script(); + set_rgb(0,0,0); - /* start the system monitor as a low-priority workqueue entry */ - work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1); + return OK; +} + +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; } @@ -249,21 +367,343 @@ BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } + void -BlinkM::monitor_trampoline(void *arg) +BlinkM::led_trampoline(void *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(" 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(" 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( " 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 @@ -413,7 +853,6 @@ BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) return ret; } - int BlinkM::get_firmware_version(uint8_t version[2]) { @@ -443,9 +882,21 @@ blinkm_main(int argc, char *argv[]) exit(0); } + if (g_blinkm == nullptr) 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")) { for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) fprintf(stderr, " %s\n", BlinkM::script_names[i]); @@ -458,8 +909,9 @@ blinkm_main(int argc, char *argv[]) if (fd < 0) err(1, "can't open BlinkM device"); + g_blinkm->setMode(0); if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) exit(0); - errx(1, "missing command, try 'start', 'list' or a script name"); -} \ No newline at end of file + errx(1, "missing command, try 'start', 'systemstate', 'ledoff', 'list' or a script name."); +}