Merge branch 'master' into mpc_yaw_fix

4 files changed, 101 insertions, 72 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a2443b0f6..1e86e8e24 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -5,6 +5,7 @@
  *           Lorenz Meier <lm@inf.ethz.ch>
  *           Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
  */
 int commander_thread_main(int argc, char *argv[]);
 
-void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
@@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool critical_battery_voltage_actions_done = false;
 
 	uint64_t last_idle_time = 0;
-
 	uint64_t start_time = 0;
 
 	bool status_changed = true;
@@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
-	toggle_status_leds(&status, &armed, true);
+	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
 	commander_initialized = true;
@@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[])
 				battery_tune_played = false;
 
 				if (armed.armed) {
-					// XXX not sure what should happen when voltage is low in flight
-					//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 				} else {
-					// XXX should we still allow to arm with critical battery?
-					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
 
 				status_changed = true;
@@ -1166,9 +1164,6 @@ int commander_thread_main(int argc, char *argv[])
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
-
-		} else {
-			status_changed = false;
 		}
 
 		hrt_abstime t1 = hrt_absolute_time();
@@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[])
 		fflush(stdout);
 		counter++;
 
-		toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+		int blink_state = blink_msg_state();
+		if (blink_state > 0) {
+			/* blinking LED message, don't touch LEDs */
+			if (blink_state == 2) {
+				/* blinking LED message completed, restore normal state */
+				control_status_leds(&status, &armed, true);
+			}
+		} else {
+			/* normal state */
+			control_status_leds(&status, &armed, status_changed);
+		}
+
+		status_changed = false;
 
 		usleep(COMMANDER_MONITORING_INTERVAL);
 	}
@@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
 {
+	/* driving rgbled */
+	if (changed) {
+		bool set_normal_color = false;
+		/* set mode */
+		if (status->arming_state == ARMING_STATE_ARMED) {
+			rgbled_set_mode(RGBLED_MODE_ON);
+			set_normal_color = true;
+
+		} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+			rgbled_set_color(RGBLED_COLOR_RED);
+
+		} else if (status->arming_state == ARMING_STATE_STANDBY) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+			set_normal_color = true;
+
+		} else {	// STANDBY_ERROR and other states
+			rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
+			rgbled_set_color(RGBLED_COLOR_RED);
+		}
+
+		if (set_normal_color) {
+			/* set color */
+			if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
+				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+					rgbled_set_color(RGBLED_COLOR_AMBER);
+				}
+				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
+
+			} else {
+				if (status->condition_local_position_valid) {
+					rgbled_set_color(RGBLED_COLOR_GREEN);
+
+				} else {
+					rgbled_set_color(RGBLED_COLOR_BLUE);
+				}
+			}
+		}
+	}
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
@@ -1298,54 +1345,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 #endif
 
-	if (changed) {
-		/* XXX TODO blink fast when armed and serious error occurs */
-
-		if (armed->armed) {
-			rgbled_set_mode(RGBLED_MODE_ON);
-
-		} else if (armed->ready_to_arm) {
-			rgbled_set_mode(RGBLED_MODE_BREATHE);
-
-		} else {
-			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-		}
-	}
-
-	if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
-		switch (status->battery_warning) {
-		case VEHICLE_BATTERY_WARNING_LOW:
-			rgbled_set_color(RGBLED_COLOR_YELLOW);
-			break;
-
-		case VEHICLE_BATTERY_WARNING_CRITICAL:
-			rgbled_set_color(RGBLED_COLOR_AMBER);
-			break;
-
-		default:
-			break;
-		}
-
-	} else {
-		switch (status->main_state) {
-		case MAIN_STATE_MANUAL:
-			rgbled_set_color(RGBLED_COLOR_WHITE);
-			break;
-
-		case MAIN_STATE_SEATBELT:
-		case MAIN_STATE_EASY:
-			rgbled_set_color(RGBLED_COLOR_GREEN);
-			break;
-
-		case MAIN_STATE_AUTO:
-			rgbled_set_color(RGBLED_COLOR_BLUE);
-			break;
-
-		default:
-			break;
-		}
-	}
-
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
 	if (status->load > 0.95f) {
 		if (leds_counter % 2 == 0)
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 017499cb5..565b4b66a 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -3,6 +3,7 @@
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -55,7 +56,6 @@
 #include <drivers/drv_led.h>
 #include <drivers/drv_rgbled.h>
 
-
 #include "commander_helper.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -64,21 +64,24 @@
 #endif
 static const int ERROR = -1;
 
+#define BLINK_MSG_TIME	700000	// 3 fast blinks
+
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
 	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
 }
 
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
 	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
-	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
 static int buzzer;
+static hrt_abstime blink_msg_end;
 
 int buzzer_init()
 {
@@ -104,16 +107,25 @@ void tune_error()
 
 void tune_positive()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_GREEN);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
 }
 
 void tune_neutral()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_WHITE);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
 }
 
 void tune_negative()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_RED);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
 }
 
@@ -132,18 +144,31 @@ int tune_critical_bat()
 	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
 }
 
-
-
 void tune_stop()
 {
 	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
 }
 
+int blink_msg_state()
+{
+	if (blink_msg_end == 0) {
+		return 0;
+
+	} else if (hrt_absolute_time() > blink_msg_end) {
+		return 2;
+
+	} else {
+		return 1;
+	}
+}
+
 static int leds;
 static int rgbleds;
 
 int led_init()
 {
+	blink_msg_end = 0;
+
 	/* first open normal LEDs */
 	leds = open(LED_DEVICE_PATH, 0);
 
@@ -159,6 +184,7 @@ int led_init()
 		warnx("Blue LED: ioctl fail\n");
 		return ERROR;
 	}
+
 #endif
 
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
@@ -168,6 +194,7 @@ int led_init()
 
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
+
 	if (rgbleds == -1) {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
@@ -203,19 +230,22 @@ int led_off(int led)
 	return ioctl(leds, LED_OFF, led);
 }
 
-void rgbled_set_color(rgbled_color_t color) {
+void rgbled_set_color(rgbled_color_t color)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
 }
 
-void rgbled_set_mode(rgbled_mode_t mode) {
+void rgbled_set_mode(rgbled_mode_t mode)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
 }
 
-void rgbled_set_pattern(rgbled_pattern_t *pattern) {
+void rgbled_set_pattern(rgbled_pattern_t *pattern)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index 027d0535f..e9514446c 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -62,6 +62,7 @@ int tune_arm(void);
 int tune_low_bat(void);
 int tune_critical_bat(void);
 void tune_stop(void);
+int blink_msg_state();
 
 int led_init(void);
 void led_deinit(void);
@@ -70,9 +71,7 @@ int led_on(int led);
 int led_off(int led);
 
 void rgbled_set_color(rgbled_color_t color);
-
 void rgbled_set_mode(rgbled_mode_t mode);
-
 void rgbled_set_pattern(rgbled_pattern_t *pattern);
 
 /**
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 222d1f45f..a3ef1d63b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -574,6 +574,7 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
+			hil_global_pos.valid = true;
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
 			hil_global_pos.alt = hil_state.alt / 1000.0f;