From 0f3878a48a457f0d9dcda6801a6a2c9ead7e0558 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 8 Jan 2016 12:27:40 -0700
Subject: [PATCH] DSM input tested OK with DX7

---
 src/drivers/boards/px4fmu-v4/board_config.h |  2 +-
 src/drivers/px4fmu/fmu.cpp                  | 62 +++++++++++++++++++--
 2 files changed, 57 insertions(+), 7 deletions(-)

diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index d464c8af2a..0655868c57 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -238,7 +238,7 @@ __BEGIN_DECLS
 
 /* Power switch controls ******************************************************/
 
-#define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+#define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s))
 #define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
 #define SPEKTRUM_RX_HIGH(_s)	stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
 #define SPEKTRUM_RX_AS_UART()		stm32_configgpio(GPIO_USART1_RX)
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 9747cd660c..d89d0cfbd4 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -134,8 +134,7 @@ private:
 	orb_advert_t	_outputs_pub;
 	unsigned	_num_outputs;
 	int		_class_instance;
-	int		_sbus_fd;
-	int		_dsm_fd;
+	int		_rcs_fd;
 
 	volatile bool	_initialized;
 	bool		_throttle_armed;
@@ -280,8 +279,7 @@ PX4FMU::PX4FMU() :
 	_outputs_pub(nullptr),
 	_num_outputs(0),
 	_class_instance(0),
-	_sbus_fd(-1),
-	_dsm_fd(-1),
+	_rcs_fd(-1),
 	_initialized(false),
 	_throttle_armed(false),
 	_pwm_on(false),
@@ -801,13 +799,65 @@ PX4FMU::cycle()
 
 	bool rc_updated = false;
 
-#ifdef RC_SERIAL_PORT
+#ifdef DSM_SERIAL_PORT
+	uint8_t n_bytes = 0;
+	uint8_t *bytes;
+	bool dsm_11_bit;
+	uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+	uint16_t raw_rc_count;
+
+	bool dsm_updated = dsm_input(_rcs_fd, raw_rc_values, &raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
+			input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+	if (dsm_updated) {
+
+		warnx("%llu, %u, %u, %u, %u, %u\n", hrt_absolute_time(), dsm_updated,
+				raw_rc_values[0], raw_rc_values[1], raw_rc_values[2],
+				raw_rc_values[3]);
+
+//		if (dsm_11_bit) {
+//			raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+//
+//		} else {
+//			raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+//		}
+//
+//		raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+//		raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+
+		// we have a new SBUS frame. Publish it.
+		_rc_in.channel_count = raw_rc_count;
+
+		if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+			_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+		}
+
+		for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
+			_rc_in.values[i] = raw_rc_values[i];
+		}
+
+		_rc_in.timestamp_publication = hrt_absolute_time();
+		_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
+
+		_rc_in.rc_ppm_frame_length = 0;
+//		_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
+//		_rc_in.rc_failsafe = sbus_failsafe;
+		_rc_in.rc_lost = false;
+//		_rc_in.rc_lost_frame_count = sbus_dropped_frames();
+		_rc_in.rc_total_frame_count = 0;
+
+		rc_updated = true;
+	}
+
+#endif
+
+#ifdef SBUS_SERIAL_PORT
 	bool sbus_failsafe, sbus_frame_drop;
 	uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
 	uint16_t raw_rc_count;
 
 	// read port
-	bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count,
+	bool sbus_updated = sbus_input(_rcs_fd, &raw_rc_values[0], &raw_rc_count,
 				       &sbus_failsafe, &sbus_frame_drop,
 				       input_rc_s::RC_INPUT_MAX_CHANNELS);