From 5fbee2394522d8b0c7a78d2751783845d011b56d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Aug 2013 11:17:29 +0200
Subject: [PATCH] Added flag to disable RC evaluation onboard of IO (raw values
 still forwarded)

---
 src/drivers/px4io/px4io.cpp           | 31 ++++++++++++++++++++-------
 src/modules/px4iofirmware/mixer.cpp   |  3 ++-
 src/modules/px4iofirmware/protocol.h  |  3 ++-
 src/modules/px4iofirmware/registers.c | 10 ++++++++-
 4 files changed, 36 insertions(+), 11 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index cebe339967..afbd4e6959 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -206,7 +206,8 @@ private:
 	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
 	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval;	///<Subscription interval limiting send rate
+	unsigned 		_update_interval;	///< Subscription interval limiting send rate
+	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
 
 	volatile int		_task;			///<worker task id
 	volatile bool		_task_should_exit;	///<worker terminate flag
@@ -393,6 +394,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_max_relays(0),
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
+	_rc_handling_disabled(false),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
@@ -588,12 +590,16 @@ PX4IO::init()
 			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
 			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
 
-		/* publish RC config to IO */
-		ret = io_set_rc_config();
-		if (ret != OK) {
-			log("failed to update RC input config");
-			mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
-			return ret;
+		if (_rc_handling_disabled) {
+			ret = io_disable_rc_handling();
+		} else {
+			/* publish RC config to IO */
+			ret = io_set_rc_config();
+			if (ret != OK) {
+				log("failed to update RC input config");
+				mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
+				return ret;
+			}
 		}
 
 	}
@@ -847,6 +853,15 @@ PX4IO::io_set_arming_state()
 	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
 }
 
+int
+PX4IO::io_disable_rc_handling()
+{
+	uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED;
+	uint16_t clear = 0;
+
+	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+}
+
 int
 PX4IO::io_set_rc_config()
 {
@@ -1365,7 +1380,7 @@ PX4IO::print_status()
 		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
 		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index d8c0e58bad..3ddad9ef7c 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -132,7 +132,8 @@ mixer_tick(void)
 
 		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 3c59a75a79..2dda59124b 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -74,7 +74,7 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
-#define PX4IO_PROTOCOL_VERSION		2
+#define PX4IO_PROTOCOL_VERSION		3
 
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
@@ -157,6 +157,7 @@
 #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_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 5) /* Disable the IO-internal evaluation of the RC */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3f241d29c9..7cae09fef1 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -158,7 +158,8 @@ volatile uint16_t	r_page_setup[] =
 #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_IO_ARM_OK)
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -335,6 +336,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
 			}
 
+			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+			}
+
 			r_setup_arming = value;
 
 			break;
@@ -427,6 +432,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 
+			/* clear any existing RC disabled flag */
+			r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
 			/* set all options except the enabled option */
 			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;