From d174998b45349348ffe41150aa1d22d7d943b790 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jan 2014 12:00:17 +0100
Subject: RSSI and concurrent S.Bus output handling

---
 src/drivers/px4io/px4io.cpp          | 6 ++++--
 src/modules/px4iofirmware/controls.c | 9 +++++++++
 src/modules/px4iofirmware/px4io.c    | 5 +++++
 src/modules/px4iofirmware/px4io.h    | 1 +
 4 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index dede5976d..8e080c043 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1453,8 +1453,10 @@ PX4IO::io_publish_raw_rc()
 
 	/* set RSSI */
 
-	// XXX the correct scaling needs to be validated here
-	rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
+	if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) {
+		// XXX the correct scaling needs to be validated here
+		rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
+	}
 
 	/* lazily advertise on first publication */
 	if (_to_input_rc == 0) {
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 541eed0e1..5859f768b 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -114,9 +114,18 @@ controls_tick() {
 
 	perf_begin(c_gather_sbus);
 	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
+
+	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
 	}
+
+	/* switch S.Bus output pin as needed */
+	if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) {
+		ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS));
+	}
+
 	perf_end(c_gather_sbus);
 
 	/*
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 0b8c4a6a8..150af35df 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -196,6 +196,11 @@ user_start(int argc, char *argv[])
 	POWER_SERVO(true);
 #endif
 
+	/* turn off S.Bus out (if supported) */
+#ifdef ENABLE_SBUS_OUT
+	ENABLE_SBUS_OUT(false);
+#endif
+
 	/* start the safety switch handler */
 	safety_init();
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index a0daa97ea..18c7468f6 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit;
 
 # define PX4IO_RELAY_CHANNELS		0
 # define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+# define ENABLE_SBUS_OUT(_s)		stm32_gpiowrite(GPIO_SBUS_OENABLE, (_s))
 
 # define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
 
-- 
cgit v1.2.3