From 7d1cd7b994240d441a37b36f74598980f3f460db Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 11 Aug 2014 19:28:14 +0200
Subject: [PATCH] Fix NO RC handling (fixes #1263)

---
 src/drivers/px4io/px4io.cpp | 36 +++++++++++++++++++++++++++++-------
 1 file changed, 29 insertions(+), 7 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index d93009c47d..bc0e6a221c 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -135,6 +135,15 @@ public:
 	 */
 	virtual int		init();
 
+	/**
+	 * Initialize the PX4IO class.
+	 *
+	 * Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 *
+	 * @param disable_rc_handling set to true to forbid override / RC handling on IO
+	 */
+	virtual int		init(bool disable_rc_handling);
+
 	/**
 	 * Detect if a PX4IO is connected.
 	 *
@@ -579,6 +588,12 @@ PX4IO::detect()
 	return 0;
 }
 
+int
+PX4IO::init(bool rc_handling_disabled) {
+	_rc_handling_disabled = rc_handling_disabled;
+	init();
+}
+
 int
 PX4IO::init()
 {
@@ -778,6 +793,11 @@ PX4IO::init()
 		if (_rc_handling_disabled) {
 			ret = io_disable_rc_handling();
 
+			if (ret != OK) {
+				log("failed disabling RC handling");
+				return ret;
+			}
+
 		} else {
 			/* publish RC config to IO */
 			ret = io_set_rc_config();
@@ -1175,6 +1195,7 @@ PX4IO::io_set_arming_state()
 int
 PX4IO::disable_rc_handling()
 {
+	_rc_handling_disabled = true;
 	return io_disable_rc_handling();
 }
 
@@ -2613,24 +2634,25 @@ start(int argc, char *argv[])
 		errx(1, "driver alloc failed");
 	}
 
-	if (OK != g_dev->init()) {
-		delete g_dev;
-		g_dev = nullptr;
-		errx(1, "driver init failed");
-	}
+	bool rc_handling_disabled = false;
 
 	/* disable RC handling on request */
 	if (argc > 1) {
 		if (!strcmp(argv[1], "norc")) {
 
-			if (g_dev->disable_rc_handling())
-				warnx("Failed disabling RC handling");
+			rc_handling_disabled = true;
 
 		} else {
 			warnx("unknown argument: %s", argv[1]);
 		}
 	}
 
+	if (OK != g_dev->init(rc_handling_disabled)) {
+		delete g_dev;
+		g_dev = nullptr;
+		errx(1, "driver init failed");
+	}
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	int dsm_vcc_ctl;