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/drivers') 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