Merge branch 'fmuv2_bringup' into fmuv2_bringup_io2

6 files changed, 49 insertions, 52 deletions

diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c
index 670b8d635..f744698be 100644
--- a/src/modules/px4iofirmware/adc.c
+++ b/src/modules/px4iofirmware/adc.c
@@ -135,6 +135,9 @@ adc_init(void)
 	return 0;
 }
 
+/*
+  return one measurement, or 0xffff on error
+ */
 uint16_t
 adc_measure(unsigned channel)
 {
@@ -154,9 +157,10 @@ adc_measure(unsigned channel)
 	while (!(rSR & ADC_SR_EOC)) {
 
 		/* never spin forever - this will give a bogus result though */
-		if (hrt_elapsed_time(&now) > 1000) {
+		if (hrt_elapsed_time(&now) > 100) {
 			debug("adc timeout");
-			break;
+			perf_end(adc_perf);
+			return 0xffff;
 		}
 	}
 
@@ -165,4 +169,4 @@ adc_measure(unsigned channel)
 
 	perf_end(adc_perf);
 	return result;
-}
\ No newline at end of file
+}
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index dc36f6c93..3cf9ca149 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -217,9 +217,8 @@ controls_tick() {
 		if (assigned_channels == 0) {
 			rc_input_lost = true;
 		} else {
-			/* set RC OK flag and clear RC lost alarm */
+			/* set RC OK flag */
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
-			r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
 		}
 
 		/*
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f38593d2a..1234e2eea 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -100,7 +100,6 @@ mixer_tick(void)
 
 	} else {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
-		r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
 	}
 
 	source = MIX_FAILSAFE;
@@ -174,7 +173,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 +245,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 1a687bd2a..29107f79f 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -138,7 +138,7 @@
 #define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
 
 #define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT			5	/* battery current in cA */
+#define PX4IO_P_STATUS_IBATT			5	/* battery current (raw ADC) */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -168,9 +168,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 */
@@ -178,8 +178,6 @@
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_IBATT_SCALE		7	/* battery current scaling factor (float) */
-#define PX4IO_P_SETUP_IBATT_BIAS		8	/* battery current bias value */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 6c09def9e..9698a0f2f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -138,15 +138,14 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 	[PX4IO_P_SETUP_RELAYS]			= 0,
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
-	[PX4IO_P_SETUP_IBATT_SCALE]		= 0,
-	[PX4IO_P_SETUP_IBATT_BIAS]		= 0,
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 };
 
 #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)
 
@@ -204,7 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		system_state.fmu_data_received_time = hrt_absolute_time();
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
-        	r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
 		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
 		
 		break;
@@ -313,7 +311,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;
 			}
 
@@ -364,7 +362,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;
 		}
@@ -414,18 +412,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
 					count++;
 				}
-
 				/* assert deadzone is sane */
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				// The following check isn't needed as constraint checks in controls.c will catch this.
-				//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-				//	count++;
-				//}
-				//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-				//	count++;
-				//}
 				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
 					count++;
 				}
@@ -508,20 +498,27 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			 * Intercept corrected for best results @ 12V.
 			 */
 			unsigned counts = adc_measure(ADC_VBATT);
-			unsigned mV = (4150 + (counts * 46)) / 10 - 200;
-			unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+			if (counts != 0xffff) {
+				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
 
-			r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+			}
 		}
 
 		/* PX4IO_P_STATUS_IBATT */
 		{
-			unsigned counts = adc_measure(ADC_VBATT);
-			unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
-			int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
-			if (corrected < 0)
-				corrected = 0;
-			r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
+			/*
+			  note that we have no idea what sort of
+			  current sensor is attached, so we just
+			  return the raw 12 bit ADC value and let the
+			  FMU sort it out, with user selectable
+			  configuration for their sensor
+			 */
+			unsigned counts = adc_measure(ADC_IN5);
+			if (counts != 0xffff) {
+				r_page_status[PX4IO_P_STATUS_IBATT] = counts;
+			}
 		}
 
 		SELECT_PAGE(r_page_status);
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 */