Merge pull request #269 from PX4/new_led_status

New led status

8 files changed, 72 insertions, 46 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index b4703839b..3006cf885 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -416,7 +416,7 @@ PX4IO::init()
 	 * already armed, and has been configured for in-air restart
 	 */
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
-	    (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
 
@@ -500,10 +500,9 @@ PX4IO::init()
 
 		/* dis-arm IO before touching anything */
 		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
-			PX4IO_P_SETUP_ARMING_ARM_OK |
+			PX4IO_P_SETUP_ARMING_FMU_ARMED |
 			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
-			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
-			PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
+			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
 
 		/* publish RC config to IO */
 		ret = io_set_rc_config();
@@ -702,16 +701,18 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (armed.armed) {
-		set |= PX4IO_P_SETUP_ARMING_ARM_OK;
+	if (armed.armed && !armed.lockdown) {
+		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
-		clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
+		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
-	if (vstatus.flag_vector_flight_mode_ok) {
-		set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
+
+	if (armed.ready_to_arm) {
+		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	} else {
-		clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;					
+		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}
+
 	if (vstatus.flag_external_manual_override_ok) {
 		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 	} else {
@@ -1277,9 +1278,9 @@ PX4IO::print_status()
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
 	printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
-	       _battery_amp_per_volt,
-	       _battery_amp_bias,
-	       _battery_mamphour_total);
+	       (double)_battery_amp_per_volt,
+	       (double)_battery_amp_bias,
+	       (double)_battery_mamphour_total);
 	printf("actuators");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@@ -1311,9 +1312,9 @@ PX4IO::print_status()
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
 	printf("arming 0x%04x%s%s%s%s\n",
 		arming,
-		((arming & PX4IO_P_SETUP_ARMING_ARM_OK)             ? " ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK)   ? " VECTOR_FLIGHT_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -1354,12 +1355,12 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 	switch (cmd) {
 	case PWM_SERVO_ARM:
 		/* set the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
 		break;
 
 	case PWM_SERVO_DISARM:
 		/* clear the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index 01ab9e3d9..aab8f3e04 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -1503,21 +1503,39 @@ int commander_thread_main(int argc, char *argv[])
 			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
 			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
 			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed */
-				led_toggle(LED_BLUE);
+				/* armed, solid */
+				led_on(LED_AMBER);
 
 			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 				/* not armed */
-				led_toggle(LED_BLUE);
+				led_toggle(LED_AMBER);
+			}
+
+			if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
+
+				/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
+				if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
+					&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
+					/* GPS lock */
+					led_on(LED_BLUE);
+
+				} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+					/* no GPS lock, but GPS module is aquiring lock */
+					led_toggle(LED_BLUE);
+				}
+
+			} else {
+				/* no GPS info, don't light the blue led */
+				led_off(LED_BLUE);
 			}
 
-			/* toggle error led at 5 Hz in HIL mode */
+			/* toggle GPS led at 5 Hz in HIL mode */
 			if (current_status.flag_hil_enabled) {
 				/* hil enabled */
-				led_toggle(LED_AMBER);
+				led_toggle(LED_BLUE);
 
 			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle error (red) at 5 Hz on low battery or error */
+				/* toggle arming (red) at 5 Hz on low battery or error */
 				led_toggle(LED_AMBER);
 
 			} else {
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
index bea388a10..ab728c7bb 100644
--- a/src/modules/commander/state_machine_helper.c
+++ b/src/modules/commander/state_machine_helper.c
@@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status)
 {
 	struct actuator_armed_s armed;
 	armed.armed = current_status->flag_system_armed;
+
+	/* XXX allow arming by external components on multicopters only if not yet armed by RC */
+	/* XXX allow arming only if core sensors are ok */
+	armed.ready_to_arm = true;
+
 	/* lock down actuators if required, only in HIL */
 	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
 	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f38593d2a..5ada1b220 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -174,7 +174,7 @@ mixer_tick(void)
 	 * here.
 	 */
 	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
 		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
 		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
@@ -246,7 +246,7 @@ void
 mixer_handle_text(const void *buffer, size_t length)
 {
 	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 		return;
 	}
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index b80551a07..e575bd841 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -145,9 +145,9 @@
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
-#define PX4IO_P_SETUP_ARMING_ARM_OK		(1 << 0) /* OK to arm */
+#define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
+#define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
-#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK	(1 << 3) /* OK to perform position / vector control (= position lock) */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 4f3addfea..61049c8b6 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -142,9 +142,10 @@ volatile uint16_t	r_page_setup[] =
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(0)
-#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | \
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
-					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
+					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -311,7 +312,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
 			}
 
@@ -362,7 +363,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 	case PX4IO_PAGE_RC_CONFIG: {
 
 		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 			break;
 		}
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 5495d5ae1..4dbecc274 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,7 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file Safety button logic.
+ * @file safety.c
+ * Safety button logic.
  */
 
 #include <nuttx/config.h>
@@ -56,11 +57,11 @@ static unsigned counter = 0;
 /*
  * Define the various LED flash sequences for each system state.
  */
-#define LED_PATTERN_SAFE 			0xffff		/**< always on 				*/
-#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0xFFFE		/**< always on with short break 	*/
-#define LED_PATTERN_FMU_ARMED 			0x4444		/**< slow blinking			*/
-#define LED_PATTERN_IO_ARMED 			0x5555		/**< fast blinking 			*/
-#define LED_PATTERN_IO_FMU_ARMED 		0x5050		/**< long off then double blink 	*/
+#define LED_PATTERN_FMU_OK_TO_ARM 		0x0003		/**< slow blinking			*/
+#define LED_PATTERN_FMU_REFUSE_TO_ARM 		0x5555		/**< fast blinking			*/
+#define LED_PATTERN_IO_ARMED 			0x5050		/**< long off, then double blink 	*/
+#define LED_PATTERN_FMU_ARMED 			0x5500		/**< long off, then quad blink 		*/
+#define LED_PATTERN_IO_FMU_ARMED 		0xffff		/**< constantly on			*/
 
 static unsigned blink_counter = 0;
 
@@ -109,7 +110,8 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
@@ -120,8 +122,6 @@ safety_check_button(void *arg)
 			counter++;
 		}
 
-		/* Disarm quickly */
-
 	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -138,21 +138,21 @@ safety_check_button(void *arg)
 	}
 
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
-	uint16_t pattern = LED_PATTERN_SAFE;
+	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
 	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
-		if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
 		} else {
 			pattern = LED_PATTERN_IO_ARMED;
 		}
 
-	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 		pattern = LED_PATTERN_FMU_ARMED;
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
+		pattern = LED_PATTERN_FMU_OK_TO_ARM;
 
-	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
-		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
 	}
 
 	/* Turn the LED on if we have a 1 at the current bit position */
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index 0b50a29ac..b7c4196c0 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -69,6 +69,7 @@ ORB_DECLARE(actuator_controls_3);
 /** global 'actuator output is live' control. */
 struct actuator_armed_s {
 	bool	armed;		/**< Set to true if system is armed */
+	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
 	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
 };