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

some major changes

sbg
Marco Bauer 12 years ago
parent
825012b029
commit
f0edb59d7e
1 changed files with 203 additions and 256 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. 459
      apps/drivers/blinkm/blinkm.cpp

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

@ -39,7 +39,7 @@
* Connect the BlinkM to I2C3 and put the following line to the rc startup-script: * Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
* blinkm start * 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 * blinkm systemmonitor
* *
* *
@ -66,11 +66,12 @@
* *
* (X = on, _=off) * (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-_-_-_-_- * 0-4 satellites = X-X-X-X-_-_-_-_-
* 5 satellites = X-X-_-X-_-_-_-_- * 5 satellites = X-X-_-X-_-_-_-_-
* 6 satellites = X-_-_-X-_-_-_-_- * 6 satellites = X-_-_-X-_-_-_-_-
* >=7 satellites = _-_-_-X-_-_-_-_- * >=7 satellites = _-_-_-X-_-_-_-_-
* If no GPS is found the first 3 blinks are white
* *
* The fourth Blink indicates the Flightmode: * The fourth Blink indicates the Flightmode:
* MANUAL : off * MANUAL : off
@ -84,6 +85,9 @@
* Battery Alert (critical Battery Level) * Battery Alert (critical Battery Level)
* Continuously blinking in red X-X-X-X-X-X-X-X * 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 <nuttx/config.h> #include <nuttx/config.h>
@ -113,6 +117,12 @@
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
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 class BlinkM : public device::I2C
{ {
public: public:
@ -150,19 +160,14 @@ private:
MORSE_CODE MORSE_CODE
}; };
enum systemDefines { enum ledColors {
LED_OFF, LED_OFF,
LED_RED, LED_RED,
LED_YELLOW, LED_YELLOW,
LED_PURPLE, LED_PURPLE,
LED_GREEN, LED_GREEN,
LED_BLUE, LED_BLUE,
LED_WHITE, LED_WHITE
LED_ONTIME=100,
LED_OFFTIME=100,
LED_NOBLINK=0,
LED_BLINK=1,
MAX_CELL_VOLTAGE=43
}; };
work_s _work; work_s _work;
@ -234,17 +239,6 @@ const char *BlinkM::script_names[] = {
nullptr 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[]); extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
@ -277,12 +271,6 @@ BlinkM::init()
return ret; return ret;
} }
/* set some sensible defaults */
set_fade_speed(255);
/* turn off by default */
play_script(BLACK);
set_fade_speed(255);
stop_script(); stop_script();
set_rgb(0,0,0); set_rgb(0,0,0);
@ -293,26 +281,14 @@ int
BlinkM::setMode(int mode) BlinkM::setMode(int mode)
{ {
if(mode == 1) { if(mode == 1) {
if(BlinkM::systemstate_run == false) { if(systemstate_run == false) {
/* set some sensible defaults */
set_fade_speed(255);
/* turn off by default */
play_script(BLACK);
stop_script(); stop_script();
//set_rgb(0,0,0); set_rgb(0,0,0);
BlinkM::systemstate_run = true; systemstate_run = true;
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
} }
} else { } else {
BlinkM::systemstate_run = false; 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);
} }
return OK; return OK;
@ -397,21 +373,19 @@ BlinkM::led()
static int num_of_cells = 0; static int num_of_cells = 0;
static int detected_cells_runcount = 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[6] = { 0, 0, 0, 0, 0, 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 t_led_blink = 0;
static int led_thread_runcount=1; static int led_thread_runcount=0;
static int led_interval = 1000; 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 topic_initialized = false;
static bool detected_cells_blinked = false; static bool detected_cells_blinked = false;
static bool led_thread_ready = true; static bool led_thread_ready = true;
int system_voltage = 0;
int num_of_used_sats = 0; int num_of_used_sats = 0;
if(!topic_initialized) { if(!topic_initialized) {
@ -427,277 +401,250 @@ BlinkM::led()
if(led_thread_ready == true) { if(led_thread_ready == true) {
if(!detected_cells_blinked) { if(!detected_cells_blinked) {
if(num_of_cells > 0) { if(num_of_cells > 0) {
t_led_color_1 = LED_PURPLE; t_led_color[0] = LED_PURPLE;
} }
if(num_of_cells > 1) { if(num_of_cells > 1) {
t_led_color_2 = LED_PURPLE; t_led_color[1] = LED_PURPLE;
} }
if(num_of_cells > 2) { if(num_of_cells > 2) {
t_led_color_3 = LED_PURPLE; t_led_color[2] = LED_PURPLE;
} }
if(num_of_cells > 3) { if(num_of_cells > 3) {
t_led_color_4 = LED_PURPLE; t_led_color[3] = LED_PURPLE;
} }
if(num_of_cells > 4) { 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; t_led_blink = LED_BLINK;
} else { } else {
t_led_color_1 = BlinkM::led_color_1; t_led_color[0] = led_color_1;
t_led_color_2 = BlinkM::led_color_2; t_led_color[1] = led_color_2;
t_led_color_3 = BlinkM::led_color_3; t_led_color[2] = led_color_3;
t_led_color_4 = BlinkM::led_color_4; t_led_color[3] = led_color_4;
t_led_color_5 = BlinkM::led_color_5; t_led_color[4] = led_color_5;
t_led_color_6 = BlinkM::led_color_6; t_led_color[5] = led_color_6;
t_led_blink = BlinkM::led_blink; t_led_blink = led_blink;
} }
led_thread_ready = false; led_thread_ready = false;
} }
switch(led_thread_runcount) { if (led_thread_runcount & 1) {
case 1: // 1. LED on if (t_led_blink)
BlinkM::setLEDColor(t_led_color_1); setLEDColor(LED_OFF);
led_thread_runcount++; led_interval = LED_OFFTIME;
led_interval = LED_ONTIME; } else {
break; setLEDColor(t_led_color[(led_thread_runcount / 2) % 6]);
case 2: // 1. LED off //led_interval = (led_thread_runcount & 1) : LED_ONTIME;
if(t_led_blink == LED_BLINK) { led_interval = LED_ONTIME;
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);
}
/* obtained data for the first file descriptor */ if (led_thread_runcount == 11) {
struct vehicle_status_s vehicle_status_raw; /* obtained data for the first file descriptor */
struct vehicle_gps_position_s vehicle_gps_position_raw; struct vehicle_status_s vehicle_status_raw;
struct vehicle_gps_position_s vehicle_gps_position_raw;
bool new_data; bool new_data_vehicle_status;
orb_check(vehicle_status_sub_fd, &new_data); bool new_data_vehicle_gps_position;
if (new_data) { orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
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_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_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
}
/* get actual battery voltage */ if (new_data_vehicle_gps_position) {
system_voltage = (int)vehicle_status_raw.voltage_battery*10; 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++) { /* get number of used satellites in navigation */
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { num_of_used_sats = 0;
num_of_used_sats++; 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) { if(num_of_cells == 0) {
/* looking for lipo cells that are connected */ /* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n"); printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { 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("<blinkm> cells found:%u\n", num_of_cells); 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 */
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 { } else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { /* no battery warnings here */
/* LED Pattern for battery low warning */
BlinkM::led_color_1 = LED_YELLOW; if(vehicle_status_raw.flag_system_armed == false) {
BlinkM::led_color_2 = LED_YELLOW; /* system not armed */
BlinkM::led_color_3 = LED_YELLOW; led_color_1 = LED_RED;
BlinkM::led_color_4 = LED_YELLOW; led_color_2 = LED_RED;
BlinkM::led_color_5 = LED_YELLOW; led_color_3 = LED_RED;
BlinkM::led_color_6 = LED_YELLOW; led_color_4 = LED_RED;
BlinkM::led_blink = LED_BLINK; led_color_5 = LED_RED;
led_color_6 = LED_RED;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { led_blink = LED_NOBLINK;
/* 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 { } else {
/* no battery warnings here */ /* armed system - initial led pattern */
led_color_1 = LED_RED;
if(vehicle_status_raw.flag_system_armed == false) { led_color_2 = LED_RED;
/* system not armed */ led_color_3 = LED_RED;
BlinkM::led_color_1 = LED_RED; led_color_4 = LED_OFF;
BlinkM::led_color_2 = LED_RED; led_color_5 = LED_OFF;
BlinkM::led_color_3 = LED_RED; led_color_6 = LED_OFF;
BlinkM::led_color_4 = LED_RED; led_blink = LED_BLINK;
BlinkM::led_color_5 = LED_RED;
BlinkM::led_color_6 = LED_RED; /* handle 4th led - flightmode indicator */
BlinkM::led_blink = LED_NOBLINK; switch((int)vehicle_status_raw.flight_mode) {
case VEHICLE_FLIGHT_MODE_MANUAL:
} else { led_color_4 = LED_OFF;
/* armed system - initial led pattern */ break;
BlinkM::led_color_1 = LED_RED;
BlinkM::led_color_2 = LED_RED; case VEHICLE_FLIGHT_MODE_STAB:
BlinkM::led_color_3 = LED_RED; led_color_4 = LED_YELLOW;
BlinkM::led_color_4 = LED_OFF; break;
BlinkM::led_color_5 = LED_OFF;
BlinkM::led_color_6 = LED_OFF; case VEHICLE_FLIGHT_MODE_HOLD:
BlinkM::led_blink = LED_BLINK; led_color_4 = LED_BLUE;
break;
/* handle 4th led - flightmode indicator */
switch((int)vehicle_status_raw.flight_mode) { case VEHICLE_FLIGHT_MODE_AUTO:
case VEHICLE_FLIGHT_MODE_MANUAL: led_color_4 = LED_GREEN;
BlinkM::led_color_4 = LED_OFF; break;
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;
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used sat´s */ /* handling used sat´s */
if(num_of_used_sats >= 7) { if(num_of_used_sats >= 7) {
BlinkM::led_color_1 = LED_OFF; led_color_1 = LED_OFF;
BlinkM::led_color_2 = LED_OFF; led_color_2 = LED_OFF;
BlinkM::led_color_3 = LED_OFF; led_color_3 = LED_OFF;
} else if(num_of_used_sats == 6) { } else if(num_of_used_sats == 6) {
BlinkM::led_color_2 = LED_OFF; led_color_2 = LED_OFF;
BlinkM::led_color_3 = LED_OFF; led_color_3 = LED_OFF;
} else if(num_of_used_sats == 5) { } 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( "<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;
} }
} 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; printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
led_thread_ready = true; vehicle_status_raw.voltage_battery,
break; 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 */ /* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); 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) { void BlinkM::setLEDColor(int ledcolor) {
switch (ledcolor) { switch (ledcolor) {
case LED_OFF: // off case LED_OFF: // off
BlinkM::set_rgb(0,0,0); set_rgb(0,0,0);
break; break;
case LED_RED: // red case LED_RED: // red
BlinkM::set_rgb(255,0,0); set_rgb(255,0,0);
break; break;
case LED_YELLOW: // yellow case LED_YELLOW: // yellow
BlinkM::set_rgb(255,70,0); set_rgb(255,70,0);
break; break;
case LED_PURPLE: // purple case LED_PURPLE: // purple
BlinkM::set_rgb(255,0,255); set_rgb(255,0,255);
break; break;
case LED_GREEN: // green case LED_GREEN: // green
BlinkM::set_rgb(0,255,0); set_rgb(0,255,0);
break; break;
case LED_BLUE: // blue case LED_BLUE: // blue
BlinkM::set_rgb(0,0,255); set_rgb(0,0,255);
break; break;
case LED_WHITE: // white case LED_WHITE: // white
BlinkM::set_rgb(255,255,255); set_rgb(255,255,255);
break; break;
} }
} }

Write Preview
Loading…
Cancel
Save