Added required additional fields: If system is ok to launch (required for LED indicator), if system is ok to override fully by RC (required for multirotors which should not support this), desired PWM output rate in Hz (again required for some multirotors).

7 files changed, 118 insertions, 27 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 40ea38cf7..c522325dd 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -130,15 +130,10 @@ comms_main(void)
 			last_report_time = now;
 
 			/* populate the report */
-			for (int 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];
-
-			if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) {
-				report.channel_count = system_state.rc_channels;
-			} else {
-				report.channel_count = 0;
 			}
-			
+			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
 			/* and send it */
@@ -174,26 +169,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 the FMU gets disarmed, the 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.mixer_use_fmu = true;
+	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 d4eace3df..03c8182b2 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -78,7 +78,43 @@ controls_main(void)
 		if (fds[0].revents & POLLIN)
 			dsm_input();
 		if (fds[1].revents & POLLIN)
+<<<<<<< Updated upstream
 			sbus_input();
+=======
+			locked |= sbus_input();
+
+		/*
+		 * If we don't have lock from one of the serial receivers,
+		 * look for PPM. It shares an input with S.bus, so there's
+		 * a possibility it will mis-parse an S.bus frame.
+		 *
+		 * XXX each S.bus frame will cause a PPM decoder interrupt
+		 * storm (lots of edges).  It might be sensible to actually
+		 * disable the PPM decoder completely if we have an alternate
+		 * receiver lock.
+		 */
+		if (!locked)
+			ppm_input();
+
+		/* check for manual override status */
+		if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
+			/* force manual input override */
+			system_state.mixer_manual_override = true;
+		} else {
+			/* override not engaged, use FMU */
+			system_state.mixer_manual_override = false;
+		}
+
+		/*
+		 * If we haven't seen any new control data in 200ms, assume we
+		 * have lost input and tell FMU.
+		 */
+		if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+
+			/* set the number of channels to zero - no inputs */
+			system_state.rc_channels = 0;
+			system_state.rc_lost = true;
+>>>>>>> Stashed changes
 
 		/* XXX do ppm processing, bypass mode, etc. here */
 
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 483e9fe4d..4e0aecdd7 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -97,7 +97,11 @@ mixer_tick(void)
 	/*
 	 * Decide which set of inputs we're using.
 	 */
+<<<<<<< Updated upstream
 	if (system_state.mixer_use_fmu) {
+=======
+	if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
+>>>>>>> Stashed changes
 		/* we have recent control data from the FMU */
 		control_count = PX4IO_OUTPUT_CHANNELS;
 		control_values = &system_state.fmu_channel_data[0];
@@ -108,20 +112,24 @@ 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 */
+<<<<<<< Updated upstream
+=======
+		// XXX builtin failsafe would activate here 
+>>>>>>> Stashed changes
 		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 77524797f..0eed5b011 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -60,6 +60,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 483b9bcc8..8a6cb48a9 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -69,8 +69,14 @@
 struct sys_state_s 
 {
 
+<<<<<<< Updated upstream
 	bool		armed;		/* IO armed */
 	bool		arm_ok;		/* FMU says OK to arm */
+=======
+	bool		armed;			/* IO armed */
+	bool		arm_ok;			/* FMU says OK to arm */
+	uint16_t	servo_rate;		/* output rate of servos in Hz */
+>>>>>>> Stashed changes
 
 	bool		ppm_input_ok;	/* valid PPM input data */
 	bool		dsm_input_ok;	/* valid Spektrum DSM data */
@@ -96,7 +102,7 @@ struct sys_state_s
 	/*
 	 * If true, we are using the FMU controls.
 	 */
-	bool		mixer_use_fmu;
+	bool		mixer_manual_override;
 
 	/*
 	 * If true, state that should be reported to FMU has been updated.
@@ -113,6 +119,29 @@ struct sys_state_s
 	 * in the config packet.
 	 */
 	uint8_t		serial_rx_mode;
+<<<<<<< Updated upstream
+=======
+
+	/**
+	 * If true, the RC signal has been lost for more than a timeout interval
+	 */
+	bool		rc_lost;
+
+	/**
+	 * 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;
+>>>>>>> Stashed changes
 };
 
 extern struct sys_state_s system_state;
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 60d20905a..52facfb61 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -63,10 +63,11 @@ static unsigned counter;
 /*
  * 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;
 
@@ -139,6 +140,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 +168,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;