Merged IO feature branch

7 files changed, 64 insertions, 30 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 5033aeb98..0fcf952ab 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -130,8 +130,10 @@ comms_main(void)
 			last_report_time = now;
 
 			/* populate the report */
-			for (unsigned i = 0; i < system_state.rc_channels; i++)
+			for (unsigned i = 0; i < system_state.rc_channels; i++) {
 				report.rc_channel[i] = system_state.rc_channel_data[i];
+			}
+
 			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
@@ -168,26 +170,41 @@ comms_handle_command(const void *buffer, size_t length)
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
-	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
+	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
+	}
 
-	/* if the IO is armed and FMU gets disarmed, IO must also disarm */
-	if(system_state.arm_ok && !cmd->arm_ok) {
+	/* if IO is armed and FMU gets disarmed, IO must also disarm */
+	if (system_state.arm_ok && !cmd->arm_ok) {
 		system_state.armed = false;
 	}
 
 	system_state.arm_ok = cmd->arm_ok;
+	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
+	system_state.manual_override_ok = cmd->manual_override_ok;
 	system_state.mixer_fmu_available = true;
 	system_state.fmu_data_received = true;
 
+	/* set PWM update rate if changed (after limiting) */
+	uint16_t new_servo_rate = cmd->servo_rate;
 
-	/* handle changes signalled by FMU */
-//	if (!system_state.arm_ok && system_state.armed)
-//		system_state.armed = false;
+	/* reject faster than 500 Hz updates */
+	if (new_servo_rate > 500) {
+		new_servo_rate = 500;
+	}
+	/* reject slower than 50 Hz updates */
+	if (new_servo_rate < 50) {
+		new_servo_rate = 50;
+	}
+	if (system_state.servo_rate != new_servo_rate) {
+		up_pwm_servo_set_rate(new_servo_rate);
+		system_state.servo_rate = new_servo_rate;
+	}
 
 	/* XXX do relay changes here */	
-	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
+	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
 		system_state.relays[i] = cmd->relay_state[i];
+	}
 
 	irqrestore(flags);
 }
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index e64f8b204..9db9a0fa5 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -110,12 +110,13 @@ controls_main(void)
 		if (!locked)
 			ppm_input();
 
-		/* force manual input override */
+		/* check for manual override status */
 		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
-			system_state.mixer_use_fmu = false;
+			/* force manual input override */
+			system_state.mixer_manual_override = true;
 		} else {
 			/* override not engaged, use FMU */
-			system_state.mixer_use_fmu = true;
+			system_state.mixer_manual_override = false;
 		}
 
 		/*
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 4fa3e1d5d..5a644906b 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -85,7 +85,7 @@ mixer_tick(void)
 	/*
 	 * Decide which set of inputs we're using.
 	 */
-	if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) {
+	if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
 		/* we have recent control data from the FMU */
 		control_count = PX4IO_OUTPUT_CHANNELS;
 		control_values = &system_state.fmu_channel_data[0];
@@ -96,21 +96,22 @@ mixer_tick(void)
 
 			/* too many frames without FMU input, time to go to failsafe */
 			if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
-				system_state.mixer_use_fmu = false;
+				system_state.mixer_manual_override = true;
+				system_state.mixer_fmu_available = false;
 			}
 		} else {
 			fmu_input_drops = 0;
 			system_state.fmu_data_received = false;
 		}
 
-	} else if (system_state.rc_channels > 0) {
+	} else if (system_state.rc_channels > 0 && system_state.manual_override_ok) {
 		/* we have control data from an R/C input */
 		control_count = system_state.rc_channels;
 		control_values = &system_state.rc_channel_data[0];
-
 	} else {
 		/* we have no control input */
-		/* XXX builtin failsafe would activate here */
+
+		// XXX builtin failsafe would activate here 
 		control_count = 0;
 	}
 
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index c704b1201..0bf6d4997 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -52,9 +52,12 @@ struct px4io_command {
 	uint16_t	f2i_magic;
 #define F2I_MAGIC	0x636d
 
-	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];
-	bool		relay_state[PX4IO_RELAY_CHANNELS];
-	bool		arm_ok;
+	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];	/**< servo output channels */
+	uint16_t	servo_rate;				/**< PWM output rate in Hz */
+	bool		relay_state[PX4IO_RELAY_CHANNELS];	/**< relay states as requested by FMU */
+	bool		arm_ok;					/**< FMU allows full arming */
+	bool		vector_flight_mode_ok;			/**< FMU aquired a valid position lock, ready for pos control */
+	bool		manual_override_ok;			/**< if true, IO performs a direct manual override */ 
 };
 
 /* config message from FMU to IO */
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index a3ac9e3e7..4f68b3d2d 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -61,6 +61,8 @@ int user_start(int argc, char *argv[])
 {
 	/* reset all to zero */
 	memset(&system_state, 0, sizeof(system_state));
+	/* default to 50 Hz PWM outputs */
+	system_state.servo_rate = 50;
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index e0b7cdda1..84f5ba029 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -71,6 +71,7 @@ struct sys_state_s
 
 	bool		armed;			/* IO armed */
 	bool		arm_ok;			/* FMU says OK to arm */
+	uint16_t	servo_rate;		/* output rate of servos in Hz */
 
 	/**
 	 * Data from the remote control input(s)
@@ -92,7 +93,7 @@ struct sys_state_s
 	/**
 	 * If true, we are using the FMU controls, else RC input if available.
 	 */
-	bool		mixer_use_fmu;
+	bool		mixer_manual_override;
 
 	/**
 	 * If true, FMU input is available.
@@ -124,6 +125,16 @@ struct sys_state_s
 	 * If true, the connection to FMU has been lost for more than a timeout interval
 	 */
 	bool		fmu_lost;
+
+	/**
+	 * If true, FMU is ready for autonomous position control. Only used for LED indication
+	 */
+	bool vector_flight_mode_ok;
+
+	/**
+	 * If true, IO performs an on-board manual override with the RC channel values
+	 */
+	bool manual_override_ok;
 };
 
 extern struct sys_state_s system_state;
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 88e7aeac6..9ce4589b7 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -63,15 +63,16 @@ static unsigned counter = 0;
 /*
  * Define the various LED flash sequences for each system state.
  */
-#define LED_PATTERN_SAFE 			0xffff		// always on
-#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_SAFE 			0xffff		/**< always on 				*/
+#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0x3000		/**< 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 	*/
 
 static unsigned blink_counter = 0;
 
 #define ARM_COUNTER_THRESHOLD	10
-#define DISARM_COUNTER_THRESHOLD	2
+#define DISARM_COUNTER_THRESHOLD	4
 
 static bool safety_button_pressed;
 
@@ -101,10 +102,6 @@ safety_check_button(void *arg)
 	 */
 	safety_button_pressed = BUTTON_SAFETY;
 
-	if(safety_button_pressed) {
-		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
-	}
-
 	/* Keep pressed for a while to arm */
 	if (safety_button_pressed && !system_state.armed) {
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -139,6 +136,8 @@ safety_check_button(void *arg)
 		}
 	} else if (system_state.arm_ok) {
 		pattern = LED_PATTERN_FMU_ARMED;
+	} else if (system_state.vector_flight_mode_ok) {
+		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
 	}
 
 	/* Turn the LED on if we have a 1 at the current bit position */
@@ -165,7 +164,7 @@ failsafe_blink(void *arg)
 	static bool failsafe = false;
 
 	/* blink the failsafe LED if we don't have FMU input */
-	if (!system_state.mixer_use_fmu) {
+	if (!system_state.mixer_fmu_available) {
 		failsafe = !failsafe;
 	} else {
 		failsafe = false;