Merge pull request #835 from TickTock-/blinkm_fix

Blinkm fix

src/drivers/blinkm/blinkm.cpp

@@ -48,11 +48,14 @@
  * The recognized number off cells, will be blinked 5 times in purple color.
  * 2 Cells = 2 blinks
  * ...
- * 5 Cells = 5 blinks
+ * 6 Cells = 6 blinks
  * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
  *
- * System disarmed:
+ * System disarmed and safe:
- * The BlinkM should lit solid red.
+ * The BlinkM should light solid cyan.
+ *
+ * System safety off but not armed:
+ * The BlinkM should light flashing orange
  *
  * System armed:
  * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
@@ -67,10 +70,10 @@
  * (X = on, _=off)
  *
  * 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-X-_-_-_-_-_-
- *   5 satellites = X-X-_-X-_-_-_-_-_-_-
+ *   5 satellites = X-X-_-X-X-_-_-_-_-_-
- *   6 satellites = X-_-_-X-_-_-_-_-_-_-
+ *   6 satellites = X-_-_-X-X-_-_-_-_-_-
- * >=7 satellites = _-_-_-X-_-_-_-_-_-_-
+ * >=7 satellites = _-_-_-X-X-_-_-_-_-_-
  * If no GPS is found the first 3 blinks are white
  *
  * The fourth Blink indicates the Flightmode:
@@ -119,6 +122,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/safety.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
 static const int LED_ONTIME = 120;
@@ -166,10 +170,12 @@ private:
 	enum ledColors {
 		LED_OFF,
 		LED_RED,
+		LED_ORANGE,
 		LED_YELLOW,
 		LED_PURPLE,
 		LED_GREEN,
 		LED_BLUE,
+		LED_CYAN,
 		LED_WHITE,
 		LED_AMBER
 	};
@@ -380,6 +386,7 @@ BlinkM::led()
 	static int vehicle_control_mode_sub_fd;
 	static int vehicle_gps_position_sub_fd;
 	static int actuator_armed_sub_fd;
+	static int safety_sub_fd;
 
 	static int num_of_cells = 0;
 	static int detected_cells_runcount = 0;
@@ -402,16 +409,20 @@ BlinkM::led()
 
 	if(!topic_initialized) {
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
-		orb_set_interval(vehicle_status_sub_fd, 1000);
+		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, 1000);
+		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, 1000);
+		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, 1000);
+		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;
 	}
@@ -433,7 +444,9 @@ BlinkM::led()
 			if(num_of_cells > 4) {
 				t_led_color[4] = LED_PURPLE;
 			}
-			t_led_color[5] = LED_OFF;
+			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;
@@ -467,14 +480,17 @@ BlinkM::led()
 		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);
 
@@ -520,7 +536,12 @@ BlinkM::led()
 				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 = 0;
@@ -541,19 +562,7 @@ BlinkM::led()
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
-					/* 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_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -565,21 +574,56 @@ BlinkM::led()
 					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_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 */
-						led_color_1 = LED_RED;
+						if(safety.safety_off){
-						led_color_2 = LED_RED;
+							led_color_1 = LED_ORANGE;
-						led_color_3 = LED_RED;
+							led_color_2 = LED_ORANGE;
-						led_color_4 = LED_RED;
+							led_color_3 = LED_ORANGE;
-						led_color_5 = LED_RED;
+							led_color_4 = LED_ORANGE;
-						led_color_6 = LED_RED;
+							led_color_5 = LED_ORANGE;
-						led_color_7 = LED_RED;
+							led_color_6 = LED_ORANGE;
-						led_color_8 = LED_RED;
+							led_color_7 = LED_ORANGE;
-						led_blink = LED_NOBLINK;
+							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;
@@ -593,23 +637,22 @@ BlinkM::led()
 						led_blink = LED_BLINK;
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
-
+							/* indicate main control state */
-							//XXX please check
+							if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
-							if (vehicle_control_mode.flag_control_position_enabled)
 								led_color_4 = LED_GREEN;
-							else if (vehicle_control_mode.flag_control_velocity_enabled)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_control_mode.flag_control_attitude_enabled)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_control_mode.flag_control_manual_enabled)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
-								led_color_4 = LED_AMBER;
+								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 sat<EFBFBD>s */
+							/* handling used satus */
 							if(num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
@@ -690,8 +733,11 @@ void BlinkM::setLEDColor(int ledcolor) {
 		case LED_RED:	// red
 			set_rgb(255,0,0);
 			break;
+		case LED_ORANGE:	// orange
+			set_rgb(255,150,0);
+			break;
 		case LED_YELLOW:	// yellow
-			set_rgb(255,70,0);
+			set_rgb(200,200,0);
 			break;
 		case LED_PURPLE:	// purple
 			set_rgb(255,0,255);
@@ -702,11 +748,14 @@ void BlinkM::setLEDColor(int ledcolor) {
 		case LED_BLUE:	// blue
 			set_rgb(0,0,255);
 			break;
+		case LED_CYAN:	// cyan
+			set_rgb(0,128,128);
+			break;
 		case LED_WHITE:	// white
 			set_rgb(255,255,255);
 			break;
 		case LED_AMBER:	// amber
-			set_rgb(255,20,0);
+			set_rgb(255,65,0);
 			break;
 	}
 }