some major changes

1 files changed, 203 insertions, 256 deletions

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 <nuttx/config.h>
@@ -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:
 		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("<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;
 	}
 }