From 41cef1d6c5420ddf1193760678a908b4e624586f Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sat, 12 Jan 2013 10:26:37 +0100 Subject: merged systemstate into blinkm driver --- apps/drivers/blinkm/blinkm.cpp | 492 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 472 insertions(+), 20 deletions(-) (limited to 'apps') diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index d589025cc..19f3b4d73 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); + + return OK; +} - /* start the system monitor as a low-priority workqueue entry */ - 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; } @@ -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() +BlinkM::led() { - /* 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); + 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."); +} -- cgit v1.2.3 From 8ec566d0cb0078e027355798d2ed2670ee23d928 Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sat, 12 Jan 2013 21:54:39 +0100 Subject: fix number of satellites --- apps/drivers/blinkm/blinkm.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 19f3b4d73..ebe38ca43 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -544,7 +544,7 @@ BlinkM::led() 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); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); /* get actual battery voltage */ system_voltage = (int)vehicle_status_raw.voltage_battery*10; @@ -642,7 +642,7 @@ BlinkM::led() } } - +/* 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, @@ -652,7 +652,7 @@ BlinkM::led() num_of_used_sats, vehicle_gps_position_raw.fix_type, vehicle_gps_position_raw.satellites_visible); - +*/ } } -- cgit v1.2.3 From edc3ae7b6cbbbd5d61837cbd44a12d9c22770faf Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sun, 13 Jan 2013 13:50:07 +0100 Subject: some changes in structure --- Makefile | 3 +- apps/drivers/blinkm/blinkm.cpp | 299 ++++++++++++++++++++--------------------- 2 files changed, 145 insertions(+), 157 deletions(-) (limited to 'apps') diff --git a/Makefile b/Makefile index d9469bb49..d3f859a25 100644 --- a/Makefile +++ b/Makefile @@ -109,7 +109,8 @@ ifeq ($(SYSTYPE),Linux) SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" endif ifeq ($(SERIAL_PORTS),) -SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +#SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" endif upload: $(FIRMWARE_BUNDLE) $(UPLOADER) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index ebe38ca43..90dfa3eea 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -149,6 +149,21 @@ private: MORSE_CODE }; + enum systemDefines { + LED_OFF, + LED_RED, + LED_YELLOW, + LED_PURPLE, + LED_GREEN, + LED_BLUE, + LED_WHITE, + LED_ONTIME=100, + LED_OFFTIME=100, + LED_NOBLINK=0, + LED_BLINK=1, + MAX_CELL_VOLTAGE=43 + }; + work_s _work; static int led_color_1; @@ -159,7 +174,7 @@ private: static int led_color_6; static int led_blink; - static int systemstate_run; + static bool systemstate_run; void setLEDColor(int ledcolor); static void led_trampoline(void *arg); @@ -218,19 +233,6 @@ 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; @@ -240,7 +242,8 @@ 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; +bool BlinkM::systemstate_run = false; + extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); @@ -281,28 +284,26 @@ int BlinkM::setMode(int mode) { if(mode == 1) { - if(BlinkM::systemstate_run == 0) { + if(BlinkM::systemstate_run == false) { /* 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; + //set_rgb(0,0,0); + BlinkM::systemstate_run = true; work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); } } else { - BlinkM::systemstate_run = 0; + BlinkM::systemstate_run = false; 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); + //set_rgb(0,0,0); } return OK; @@ -403,7 +404,6 @@ BlinkM::led() 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)); @@ -415,12 +415,6 @@ BlinkM::led() 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) { @@ -523,138 +517,131 @@ BlinkM::led() BlinkM::setLEDColor(LED_OFF); } - //poll_ret = ::poll(&fds[0],1,1000); - poll_ret = ::poll(fds, 2, 1000); + /* obtained data for the first file descriptor */ + struct vehicle_status_s vehicle_status_raw; + struct vehicle_gps_position_s vehicle_gps_position_raw; - 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_gps_position_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++; - } - } + bool new_data; + orb_check(vehicle_status_sub_fd, &new_data); + + if (new_data) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); + } + + orb_check(vehicle_gps_position_sub_fd, &new_data); + + if (new_data) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_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; - 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; - } - } + /* 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; } - } -/* - 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); -*/ + /* 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; @@ -674,7 +661,7 @@ BlinkM::led() break; } - if(BlinkM::systemstate_run == 1) { + if(BlinkM::systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); } -- cgit v1.2.3 From 825012b029a09e51828136deed3a24fab95c4780 Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sun, 13 Jan 2013 18:04:07 +0100 Subject: switched to initialiser list and member variables --- apps/drivers/blinkm/blinkm.cpp | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) (limited to 'apps') diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 90dfa3eea..b1e20d4f8 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -119,6 +119,7 @@ public: BlinkM(int bus); ~BlinkM(); + virtual int init(); virtual int probe(); virtual int setMode(int mode); @@ -166,15 +167,15 @@ private: work_s _work; - 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; + int led_color_1; + int led_color_2; + int led_color_3; + int led_color_4; + int led_color_5; + int led_color_6; + int led_blink; - static bool systemstate_run; + bool systemstate_run; void setLEDColor(int ledcolor); static void led_trampoline(void *arg); @@ -233,7 +234,7 @@ const char *BlinkM::script_names[] = { nullptr }; - +/* int BlinkM::led_color_1 = LED_OFF; int BlinkM::led_color_2 = LED_OFF; int BlinkM::led_color_3 = LED_OFF; @@ -243,12 +244,20 @@ int BlinkM::led_color_6 = LED_OFF; int BlinkM::led_blink = LED_NOBLINK; bool BlinkM::systemstate_run = false; - +*/ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus) : - I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000) + I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000), + led_color_1(LED_OFF), + led_color_2(LED_OFF), + led_color_3(LED_OFF), + led_color_4(LED_OFF), + led_color_5(LED_OFF), + led_color_6(LED_OFF), + led_blink(LED_NOBLINK), + systemstate_run(false) { memset(&_work, 0, sizeof(_work)); } -- cgit v1.2.3 From f0edb59d7e8ef3aa23ae14a47bacc8276c4cdf0b Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Mon, 14 Jan 2013 21:58:42 +0100 Subject: some major changes --- apps/drivers/blinkm/blinkm.cpp | 459 ++++++++++++++++++----------------------- 1 file changed, 203 insertions(+), 256 deletions(-) (limited to 'apps') diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index b1e20d4f8..aeee80905 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -39,7 +39,7 @@ * 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: + * To start the system monitor put in the next line after the blinkm start: * blinkm systemmonitor * * @@ -66,11 +66,12 @@ * * (X = on, _=off) * - * The first 3 Blinks indicates the status of the GPS-Signal: + * The first 3 blinks indicates the status of the GPS-Signal (red): * 0-4 satellites = X-X-X-X-_-_-_-_- * 5 satellites = X-X-_-X-_-_-_-_- * 6 satellites = X-_-_-X-_-_-_-_- * >=7 satellites = _-_-_-X-_-_-_-_- + * If no GPS is found the first 3 blinks are white * * The fourth Blink indicates the Flightmode: * MANUAL : off @@ -84,6 +85,9 @@ * Battery Alert (critical Battery Level) * Continuously blinking in red X-X-X-X-X-X-X-X * + * General Error (no uOrb Data) + * Continuously blinking in white X-X-X-X-X-X-X-X + * */ #include @@ -113,6 +117,12 @@ #include #include +static const float MAX_CELL_VOLTAGE = 4.3f; +static const int LED_ONTIME = 100; +static const int LED_OFFTIME = 100; +static const int LED_BLINK = 1; +static const int LED_NOBLINK = 0; + class BlinkM : public device::I2C { public: @@ -150,19 +160,14 @@ private: MORSE_CODE }; - enum systemDefines { + enum ledColors { LED_OFF, LED_RED, LED_YELLOW, LED_PURPLE, LED_GREEN, LED_BLUE, - LED_WHITE, - LED_ONTIME=100, - LED_OFFTIME=100, - LED_NOBLINK=0, - LED_BLINK=1, - MAX_CELL_VOLTAGE=43 + LED_WHITE }; work_s _work; @@ -234,17 +239,6 @@ const char *BlinkM::script_names[] = { nullptr }; -/* -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; - -bool BlinkM::systemstate_run = false; -*/ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); @@ -277,12 +271,6 @@ BlinkM::init() return ret; } - /* 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); @@ -293,26 +281,14 @@ int BlinkM::setMode(int mode) { if(mode == 1) { - if(BlinkM::systemstate_run == false) { - /* set some sensible defaults */ - set_fade_speed(255); - - /* turn off by default */ - play_script(BLACK); + if(systemstate_run == false) { stop_script(); - //set_rgb(0,0,0); - BlinkM::systemstate_run = true; + set_rgb(0,0,0); + systemstate_run = true; work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); } } else { - BlinkM::systemstate_run = false; - usleep(1000000); - /* set some sensible defaults */ - set_fade_speed(255); - /* turn off by default */ - play_script(BLACK); - stop_script(); - //set_rgb(0,0,0); + systemstate_run = false; } return OK; @@ -397,21 +373,19 @@ BlinkM::led() 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_color[6] = { 0, 0, 0, 0, 0, 0}; static int t_led_blink = 0; - static int led_thread_runcount=1; + static int led_thread_runcount=0; static int led_interval = 1000; + static int no_data_vehicle_status = 0; + static int no_data_vehicle_gps_position = 0; + 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; if(!topic_initialized) { @@ -427,277 +401,250 @@ BlinkM::led() if(led_thread_ready == true) { if(!detected_cells_blinked) { if(num_of_cells > 0) { - t_led_color_1 = LED_PURPLE; + t_led_color[0] = LED_PURPLE; } if(num_of_cells > 1) { - t_led_color_2 = LED_PURPLE; + t_led_color[1] = LED_PURPLE; } if(num_of_cells > 2) { - t_led_color_3 = LED_PURPLE; + t_led_color[2] = LED_PURPLE; } if(num_of_cells > 3) { - t_led_color_4 = LED_PURPLE; + t_led_color[3] = LED_PURPLE; } if(num_of_cells > 4) { - t_led_color_5 = LED_PURPLE; + t_led_color[4] = LED_PURPLE; } - t_led_color_6 = LED_OFF; + t_led_color[5] = 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; + t_led_color[0] = led_color_1; + t_led_color[1] = led_color_2; + t_led_color[2] = led_color_3; + t_led_color[3] = led_color_4; + t_led_color[4] = led_color_5; + t_led_color[5] = led_color_6; + t_led_blink = 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); - } + if (led_thread_runcount & 1) { + if (t_led_blink) + setLEDColor(LED_OFF); + led_interval = LED_OFFTIME; + } else { + setLEDColor(t_led_color[(led_thread_runcount / 2) % 6]); + //led_interval = (led_thread_runcount & 1) : LED_ONTIME; + led_interval = LED_ONTIME; + } - /* obtained data for the first file descriptor */ - struct vehicle_status_s vehicle_status_raw; - struct vehicle_gps_position_s vehicle_gps_position_raw; + if (led_thread_runcount == 11) { + /* obtained data for the first file descriptor */ + struct vehicle_status_s vehicle_status_raw; + struct vehicle_gps_position_s vehicle_gps_position_raw; - bool new_data; - orb_check(vehicle_status_sub_fd, &new_data); + bool new_data_vehicle_status; + bool new_data_vehicle_gps_position; - if (new_data) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); - } + orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); - orb_check(vehicle_gps_position_sub_fd, &new_data); + if (new_data_vehicle_status) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); + no_data_vehicle_status = 0; + } else { + no_data_vehicle_status++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } - if (new_data) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); - } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); - /* get actual battery voltage */ - system_voltage = (int)vehicle_status_raw.voltage_battery*10; + if (new_data_vehicle_gps_position) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); + no_data_vehicle_gps_position = 0; + } else { + no_data_vehicle_gps_position++; + if(no_data_vehicle_gps_position >= 3) + no_data_vehicle_gps_position = 3; + } - /* 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++; - } + + + /* 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(new_data_vehicle_status || no_data_vehicle_status < 3){ if(num_of_cells == 0) { - /* looking for lipo cells that are connected */ + /* 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; + if(vehicle_status_raw.voltage_battery < 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 */ + led_color_1 = LED_YELLOW; + led_color_2 = LED_YELLOW; + led_color_3 = LED_YELLOW; + led_color_4 = LED_YELLOW; + led_color_5 = LED_YELLOW; + led_color_6 = LED_YELLOW; + led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + /* LED Pattern for battery critical alerting */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_RED; + led_color_5 = LED_RED; + led_color_6 = LED_RED; + led_blink = LED_BLINK; + } 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; + /* no battery warnings here */ + + if(vehicle_status_raw.flag_system_armed == false) { + /* system not armed */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_RED; + led_color_5 = LED_RED; + led_color_6 = LED_RED; + led_blink = LED_NOBLINK; } 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; - } + /* armed system - initial led pattern */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_OFF; + led_color_5 = LED_OFF; + led_color_6 = LED_OFF; + led_blink = LED_BLINK; + + /* handle 4th led - flightmode indicator */ + switch((int)vehicle_status_raw.flight_mode) { + case VEHICLE_FLIGHT_MODE_MANUAL: + led_color_4 = LED_OFF; + break; + + case VEHICLE_FLIGHT_MODE_STAB: + led_color_4 = LED_YELLOW; + break; + + case VEHICLE_FLIGHT_MODE_HOLD: + led_color_4 = LED_BLUE; + break; + + case VEHICLE_FLIGHT_MODE_AUTO: + led_color_4 = LED_GREEN; + break; + } + if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* 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; + led_color_1 = LED_OFF; + led_color_2 = LED_OFF; + led_color_3 = LED_OFF; } else if(num_of_used_sats == 6) { - BlinkM::led_color_2 = LED_OFF; - BlinkM::led_color_3 = LED_OFF; + led_color_2 = LED_OFF; + led_color_3 = LED_OFF; } else if(num_of_used_sats == 5) { - BlinkM::led_color_3 = LED_OFF; + led_color_3 = LED_OFF; } + + } else { + /* no vehicle_gps_position data */ + led_color_1 = LED_WHITE; + led_color_2 = LED_WHITE; + led_color_3 = LED_WHITE; + } + } } - - /* - 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; } + } else { + /* LED Pattern for general Error - no vehicle_status can retrieved */ + led_color_1 = LED_WHITE; + led_color_2 = LED_WHITE; + led_color_3 = LED_WHITE; + led_color_4 = LED_WHITE; + led_color_5 = LED_WHITE; + led_color_6 = LED_WHITE; + led_blink = LED_BLINK; - break; - default: - led_thread_runcount=1; - t_led_blink = 0; - led_thread_ready = true; - break; + } + + /* + printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%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, + no_data_vehicle_status, + no_data_vehicle_gps_position, + num_of_used_sats, + vehicle_gps_position_raw.fix_type, + vehicle_gps_position_raw.satellites_visible); + */ + + led_thread_runcount=0; + led_thread_ready = true; + led_interval = LED_OFFTIME; + + if(detected_cells_runcount < 4){ + detected_cells_runcount++; + } else { + detected_cells_blinked = true; + } + + } else { + led_thread_runcount++; } - if(BlinkM::systemstate_run == true) { + if(systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); + } else { + stop_script(); + set_rgb(0,0,0); } } void BlinkM::setLEDColor(int ledcolor) { switch (ledcolor) { case LED_OFF: // off - BlinkM::set_rgb(0,0,0); + set_rgb(0,0,0); break; case LED_RED: // red - BlinkM::set_rgb(255,0,0); + set_rgb(255,0,0); break; case LED_YELLOW: // yellow - BlinkM::set_rgb(255,70,0); + set_rgb(255,70,0); break; case LED_PURPLE: // purple - BlinkM::set_rgb(255,0,255); + set_rgb(255,0,255); break; case LED_GREEN: // green - BlinkM::set_rgb(0,255,0); + set_rgb(0,255,0); break; case LED_BLUE: // blue - BlinkM::set_rgb(0,0,255); + set_rgb(0,0,255); break; case LED_WHITE: // white - BlinkM::set_rgb(255,255,255); + set_rgb(255,255,255); break; } } -- cgit v1.2.3