void
RGBLED::led()
{
static int led_thread_runcount=0;
static int led_interval = 1000;
switch (mode) {
case LED_MODE_TEST:
/* Demo LED pattern for now */
led_colors[0] = LED_COLOR_YELLOW;
led_colors[1] = LED_COLOR_AMBER;
led_colors[2] = LED_COLOR_RED;
led_colors[3] = LED_COLOR_PURPLE;
led_colors[4] = LED_COLOR_BLUE;
led_colors[5] = LED_COLOR_GREEN;
led_colors[6] = LED_COLOR_WHITE;
led_colors[7] = LED_COLOR_OFF;
led_blink = LED_BLINK_ON;
break;
case LED_MODE_SYSTEMSTATE:
/* XXX TODO set pattern */
led_colors[0] = LED_COLOR_OFF;
led_colors[1] = LED_COLOR_OFF;
led_colors[2] = LED_COLOR_OFF;
led_colors[3] = LED_COLOR_OFF;
led_colors[4] = LED_COLOR_OFF;
led_colors[5] = LED_COLOR_OFF;
led_colors[6] = LED_COLOR_OFF;
led_colors[7] = LED_COLOR_OFF;
led_blink = LED_BLINK_OFF;
break;
case LED_MODE_OFF:
default:
return;
break;
}
if (led_thread_runcount & 1) {
if (led_blink == LED_BLINK_ON)
setLEDColor(LED_COLOR_OFF);
led_interval = LED_OFFTIME;
} else {
setLEDColor(led_colors[(led_thread_runcount/2) % 8]);
led_interval = LED_ONTIME;
}
led_thread_runcount++;
if(running) {
/* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval);
} else {
set_on(false);
}
}
task main()
{
// Clear all text from the debugstream window
clearDebugStream();
//sets LED to flash to show that the printer is printing
setLEDColor(ledRed);
//credits
displayCenteredTextLine(2, "Made by Xander Soldaat");
displayCenteredTextLine(4, "and Cyrus Cuenca");
//verion number
displayCenteredTextLine(6, "Version 1.1");
//GitHub link
displayCenteredTextLine(8, "http://github.com/cyruscuenca/g-pars3");
//supported commands
displayCenteredTextLine(10, "Supported commands: G1");
float x, y, z, e, f = 0.0;
long fd = 0;
char buffer[128];
long lineLength = 0;
string gcmd = "G1"; // always a g1
fd = fileOpenRead(fileName); // fileName = gcode.rtf
if (fd < 0) // if file is not found/cannot open
{
writeDebugStreamLine("Could not open %s", fileName);
return;
}
while (true)
{
lineLength = readLine(fd, buffer, 128);
if (lineLength > 0)
{
readNextCommand(buffer, lineLength, x, y, z, e, f);
executeCommand(gcmd, x, y, z, e, f);
}
else
// we're done, no lines left to read from the file
return;
// Wipe the buffer by setting its contents to 0
memset(buffer, 0, sizeof(buffer));
//LED turns green to show that the print is done
setLEDColor(ledGreen);
}
}
//------------------------------------------------------------------------------
// Fill the strip with a solid color. This will update the strip.
void fillStrip(u_char r, u_char g, u_char b, u_char w){
int i;
for (i = 0; i < NUM_LEDS; i++){
setLEDColor(i, r, g, b, w); // set all LEDs to specified color
}
showStrip(); // refresh strip
}
//------------------------------------------------------------------------------
void SpinningColor(u_char r, u_char g, u_char b, u_char w)
{
static int a = 0, check = 0, counter = 0;
int i = 0;
for(i = 0; i < 7; i++)
{
if(BallBlink)
{
if(check)
setLEDColor(i, r, g, b, w);
else
setLEDColor(i, 0x00, 0x00, 0x00, 0x4F);
}
else
{
setLEDColor(i, r, g, b, w);
leds[a].red = 0x00;
leds[a].green = 0x00;
leds[a].blue = 0x00;
leds[a].white = 0x4F;
}
showStrip();
}
if(BallBlink)
{
if(counter++ < 4)
check = 0;
else
check = 1;
if(counter >= 12)
counter = 0;
}
else
{
if(a >= 6)
a = 0;
else
a++;
}
}
//int main(void)
//{
// uint32_t MCLKfreq, SMCLKfreq;
// uint16_t i;
//
// WDT_A_holdTimer();
//
// /* Configuring pins for peripheral/crystal usage */
// MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_PJ,
// GPIO_PIN3 | GPIO_PIN4, GPIO_PRIMARY_MODULE_FUNCTION);
//
// /* Setting the external clock frequency. This API is optional, but will
// * come in handy if the user ever wants to use the getMCLK/getACLK/etc
// * functions
// */
// CS_setExternalClockSourceFrequency(32000,48000000);
//
// /* Starting HFXT in non-bypass mode without a timeout. Before we start
// * we have to change VCORE to 1 to support the 48MHz frequency */
// MAP_PCM_setCoreVoltageLevel(PCM_VCORE1);
// MAP_FlashCtl_setWaitState(FLASH_BANK0, 2);
// MAP_FlashCtl_setWaitState(FLASH_BANK1, 2);
// CS_startHFXT(false);
//
// /* Initializing MCLK to HFXT (effectively 48MHz) */
// MAP_CS_initClockSignal(CS_MCLK, CS_HFXTCLK_SELECT, CS_CLOCK_DIVIDER_1);
//// CS_setDCOFrequency (16000000); // set to 16 MHz (timing is set for this freq)
//
// MCLKfreq=MAP_CS_getMCLK(); // get MCLK value
//
// MAP_CS_initClockSignal(CS_SMCLK, CS_HFXTCLK_SELECT, CS_CLOCK_DIVIDER_4); // set SMCLK to 12 MHz
// SMCLKfreq=MAP_CS_getSMCLK(); // get SMCLK value to verify it was set correctly
//
// // Set P1.0 to output direction to drive red LED
// GPIO_setAsOutputPin(GPIO_PORT_P1,GPIO_PIN0);
//
// // Configure SysTick
//
// SysTick_enableModule();
// SysTick_setPeriod(16000); // with a 68 MHz clock, this period is 1 ms
// SysTick_enableInterrupt();
//
// // MAP_Interrupt_enableSleepOnIsrExit();
// MAP_Interrupt_enableMaster();
//
// initStrip(); // ***** HAVE YOU SET YOUR NUM_LEDS DEFINE IN WS2812.C? ******
//
// // set strip color red
// fillStrip(0xFF, 0x00, 0x00, 0x00);
//
// // show the strip
// showStrip();
//
// // gradually fill for ever and ever
// u_int numLEDs = 7;
// while(1){
// gradualFill(numLEDs, 0x0F, 0x00, 0x00, 0x00); // red
// msDelay(3000);
// gradualFill(numLEDs, 0x00, 0x0F, 0x00, 0x00); // green
// msDelay(3000);
// gradualFill(numLEDs, 0x00, 0x00, 0x0F, 0x00); // blue
// msDelay(3000);
// gradualFill(numLEDs, 0x0F, 0x00, 0x0F, 0x00); // magenta
// msDelay(3000);
// gradualFill(numLEDs, 0x0F, 0x0F, 0x00, 0x00); // yellow
// msDelay(3000);
// gradualFill(numLEDs, 0x00, 0x0F, 0x0F, 0x00); // cyan
// msDelay(3000);
// gradualFill(numLEDs, 0x00, 0x00, 0x00, 0x0F); // white
// msDelay(5000);
// }
//}
//------------------------------------------------------------------------------
void gradualFill(u_int n, u_char r, u_char g, u_char b, u_char w){
int i;
for (i = 0; i < n; i++){ // n is number of LEDs
setLEDColor(i, r, g, b, w);
showStrip();
// _delay_cycles(50000); // lazy delay
msDelay(200);
}
}
bool LEDTestController::handleEvent(Ion::Events::Event event) {
if (event == Ion::Events::OK) {
setLEDColor(LEDColorAtIndex(m_LEDColorIndex++));
if (m_LEDColorIndex == k_numberOfColors) {
// Next step, see WizardViewController
return false;
}
}
return true;
}
BW_LED_Number::BW_LED_Number( QWidget *parent, const char *name )
: QFrame( parent, name ){
offcolor = QColor(100,0,0);
showOffColon(FALSE);
smallLED = false;
current_symbol = ' ';
old_symbol = ' ';
old_segments = &segs[13][0]; // nothing
current_segments = &segs[13][0]; // nothing
setLEDColor(yellow,black);
}
// Over a period of 1 second, fade to the specified brightness
void fadeWhite(unsigned int brightness){
unsigned int fadeFrequency = 0;
unsigned int subTimer = 0;
unsigned int lastTime = 0;
unsigned int fadeBrightness = 0;
// determine fade frequency
if (brightness < 1000){
fadeFrequency = 1000 / brightness;
if (1000 % brightness != 0){
fadeFrequency++; // round up to nearest whole int
}
}
else {
fadeFrequency = 1;
}
// over a period of 1 second, fade to brightness every fadeFrequency ms
subTimer = 0;
lastTime = timer1ms;
while (subTimer <= 1000){
if ((timer1ms != lastTime)) {
subTimer++;
lastTime = timer1ms;
if (subTimer % fadeFrequency == 0){
fadeBrightness++;
setLEDColor(fadeBrightness, fadeBrightness, fadeBrightness);
}
}
}
// make sure we are at the right brightness at 1 sec (we will be very close, if not exact)
if (fadeBrightness < brightness) {
setLEDColor(brightness, brightness, brightness);
}
}
void setupPWM()
{
gpio_mode_setup(LED_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, LED_R_PIN | LED_G_PIN);
gpio_set_output_options(LED_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, LED_R_PIN | LED_G_PIN);
gpio_set_af(LED_PORT, GPIO_AF1, LED_R_PIN | LED_G_PIN);
TIM_CCMR1(RGB_TIMER) = TIM_CCMR1_OC1M_PWM1 | TIM_CCMR1_OC2M_PWM1;
TIM_CCER(RGB_TIMER) = TIM_CCER_CC1E | TIM_CCER_CC2E;
TIM_PSC(RGB_TIMER) = 1000;
TIM_ARR(RGB_TIMER) = 0xff;
TIM_CR1(RGB_TIMER) = TIM_CR1_CEN;
g_isRedBlinking = false;
setLEDColor(127, 127, 0);
}
void
BlinkM::led()
{
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[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
static int t_led_blink = 0;
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 num_of_used_sats = 0;
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;
}
if(led_thread_ready == true) {
if(!detected_cells_blinked) {
if(num_of_cells > 0) {
t_led_color[0] = LED_PURPLE;
}
if(num_of_cells > 1) {
t_led_color[1] = LED_PURPLE;
}
if(num_of_cells > 2) {
t_led_color[2] = LED_PURPLE;
}
if(num_of_cells > 3) {
t_led_color[3] = LED_PURPLE;
}
if(num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
t_led_color[5] = LED_OFF;
t_led_color[6] = LED_OFF;
t_led_color[7] = LED_OFF;
t_led_blink = LED_BLINK;
} else {
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_color[6] = led_color_7;
t_led_color[7] = led_color_8;
t_led_blink = led_blink;
}
led_thread_ready = false;
}
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) % 8]);
//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
led_interval = LED_ONTIME;
}
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_gps_position_s vehicle_gps_position_raw;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(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++) {
for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); 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_color_7 = LED_YELLOW;
led_color_8 = 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_color_7 = LED_RED;
led_color_8 = 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_color_7 = LED_RED;
led_color_8 = 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_color_7 = LED_OFF;
led_color_8 = 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_AMBER;
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_color_7 = LED_WHITE;
led_color_8 = 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 */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
} else {
stop_script();
set_rgb(0,0,0);
}
}
void
BlinkM::led()
{
if (!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 250);
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(vehicle_control_mode_sub_fd, 250);
actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(actuator_armed_sub_fd, 250);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 250);
/* Subscribe to safety topic */
safety_sub_fd = orb_subscribe(ORB_ID(safety));
orb_set_interval(safety_sub_fd, 250);
topic_initialized = true;
}
if (led_thread_ready == true) {
if (!detected_cells_blinked) {
if (num_of_cells > 0) {
t_led_color[0] = LED_PURPLE;
}
if (num_of_cells > 1) {
t_led_color[1] = LED_PURPLE;
}
if (num_of_cells > 2) {
t_led_color[2] = LED_PURPLE;
}
if (num_of_cells > 3) {
t_led_color[3] = LED_PURPLE;
}
if (num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
if (num_of_cells > 5) {
t_led_color[5] = LED_PURPLE;
}
t_led_color[6] = LED_OFF;
t_led_color[7] = LED_OFF;
t_led_blink = LED_BLINK;
} else {
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_color[6] = led_color_7;
t_led_color[7] = led_color_8;
t_led_blink = led_blink;
}
led_thread_ready = false;
}
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) % 8]);
//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
led_interval = LED_ONTIME;
}
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_control_mode_s vehicle_control_mode;
struct actuator_armed_s actuator_armed;
struct vehicle_gps_position_s vehicle_gps_position_raw;
struct safety_s safety;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
memset(&safety, 0, sizeof(safety));
bool new_data_vehicle_status;
bool new_data_vehicle_control_mode;
bool new_data_actuator_armed;
bool new_data_vehicle_gps_position;
bool new_data_safety;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
int no_data_vehicle_status = 0;
int no_data_vehicle_control_mode = 0;
int no_data_actuator_armed = 0;
int no_data_vehicle_gps_position = 0;
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_control_mode_sub_fd, &new_data_vehicle_control_mode);
if (new_data_vehicle_control_mode) {
orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
no_data_vehicle_control_mode = 0;
} else {
no_data_vehicle_control_mode++;
if (no_data_vehicle_control_mode >= 3) {
no_data_vehicle_control_mode = 3;
}
}
orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
if (new_data_actuator_armed) {
orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
no_data_actuator_armed = 0;
} else {
no_data_actuator_armed++;
if (no_data_actuator_armed >= 3) {
no_data_actuator_armed = 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;
}
}
/* update safety topic */
orb_check(safety_sub_fd, &new_data_safety);
if (new_data_safety) {
orb_copy(ORB_ID(safety), safety_sub_fd, &safety);
}
/* get number of used satellites in navigation */
num_of_used_sats = vehicle_gps_position_raw.satellites_used;
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.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) { break; }
}
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* 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_color_7 = LED_RED;
led_color_8 = LED_RED;
led_blink = LED_BLINK;
} else if (vehicle_status_raw.rc_signal_lost) {
/* LED Pattern for FAILSAFE */
led_color_1 = LED_BLUE;
led_color_2 = LED_BLUE;
led_color_3 = LED_BLUE;
led_color_4 = LED_BLUE;
led_color_5 = LED_BLUE;
led_color_6 = LED_BLUE;
led_color_7 = LED_BLUE;
led_color_8 = LED_BLUE;
led_blink = LED_BLINK;
} else if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
/* 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_color_7 = LED_YELLOW;
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if (actuator_armed.armed == false) {
/* system not armed */
if (safety.safety_off) {
led_color_1 = LED_ORANGE;
led_color_2 = LED_ORANGE;
led_color_3 = LED_ORANGE;
led_color_4 = LED_ORANGE;
led_color_5 = LED_ORANGE;
led_color_6 = LED_ORANGE;
led_color_7 = LED_ORANGE;
led_color_8 = LED_ORANGE;
led_blink = LED_BLINK;
} else {
led_color_1 = LED_CYAN;
led_color_2 = LED_CYAN;
led_color_3 = LED_CYAN;
led_color_4 = LED_CYAN;
led_color_5 = LED_CYAN;
led_color_6 = LED_CYAN;
led_color_7 = LED_CYAN;
led_color_8 = LED_CYAN;
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_color_7 = LED_OFF;
led_color_8 = LED_OFF;
led_blink = LED_BLINK;
if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) {
led_color_4 = LED_GREEN;
}
/* TODO: add other Auto modes */
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) {
led_color_4 = LED_BLUE;
} else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) {
led_color_4 = LED_YELLOW;
} else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) {
led_color_4 = LED_WHITE;
} else {
led_color_4 = LED_OFF;
}
led_color_5 = led_color_4;
}
if (new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used satus */
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_color_7 = LED_WHITE;
led_color_8 = 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 */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
} else {
stop_script();
set_rgb(0, 0, 0);
}
}
void LEDTestController::viewWillAppear() {
m_LEDColorIndex = 0;
setLEDColor(LEDColorAtIndex(m_LEDColorIndex++));
}
