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
Split 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 @@ @@ -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 @@ @@ -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 @@ @@ -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 <nuttx/config.h>
@ -113,6 +117,12 @@ @@ -113,6 +117,12 @@
#include <uORB/topics/vehicle_status.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
{
public:
@ -150,19 +160,14 @@ private: @@ -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[] = { @@ -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() @@ -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 @@ -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() @@ -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() @@ -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("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%u\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
/* LED Pattern for battery low warning */
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( "<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;
led_thread_ready = true;
break;
}
/*
printf( "<blinkm> 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;
}
}

Write Preview
Loading…
Cancel
Save