mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 00:51:36 +08:00
some major changes
This commit is contained in:
Marco Bauer
+231
-284
@@ -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 */
|
|
||||||
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);
|
|
||||||
|
|
||||||
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("<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;
|
|
||||||
}
|
|
||||||
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 */
|
|
||||||
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( "<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;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
led_thread_runcount=1;
|
|
||||||
t_led_blink = 0;
|
|
||||||
led_thread_ready = true;
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if(BlinkM::systemstate_run == true) {
|
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_vehicle_status;
|
||||||
|
bool new_data_vehicle_gps_position;
|
||||||
|
|
||||||
|
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
|
||||||
|
|
||||||
|
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++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(new_data_vehicle_status || no_data_vehicle_status < 3){
|
||||||
|
if(num_of_cells == 0) {
|
||||||
|
/* 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(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 {
|
||||||
|
/* 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 {
|
||||||
|
/* 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) {
|
||||||
|
led_color_1 = LED_OFF;
|
||||||
|
led_color_2 = LED_OFF;
|
||||||
|
led_color_3 = LED_OFF;
|
||||||
|
} else if(num_of_used_sats == 6) {
|
||||||
|
led_color_2 = LED_OFF;
|
||||||
|
led_color_3 = LED_OFF;
|
||||||
|
} else if(num_of_used_sats == 5) {
|
||||||
|
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;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} 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;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
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(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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user