Graupner HoTT SUMD/SUMH Receiver Protocol added

3 files changed, 38 insertions, 8 deletions

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index d20f776d6..e04ffc940 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -45,6 +45,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
 #include <rc/st24.h>
+#include <rc/sumd.h>
 
 #include "px4io.h"
 
@@ -53,7 +54,7 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
-static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated);
+static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -63,7 +64,7 @@ static int _dsm_fd;
 
 static uint16_t rc_value_override = 0;
 
-bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
+bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
 {
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
@@ -106,7 +107,31 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
 
-	return (*dsm_updated | *st24_updated);
+
+	/* get data from FD and attempt to parse with SUMD libs */
+	uint8_t sumd_rssi, sumd_rx_count;
+	uint16_t sumd_channel_count = 0;
+
+	*sumd_updated = false;
+
+	for (unsigned i = 0; i < n_bytes; i++) {
+		/* set updated flag if one complete packet was parsed */
+		st24_rssi = RC_INPUT_RSSI_MAX;
+		*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
+					&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+	}
+
+	if (*sumd_updated) {
+
+		*rssi = sumd_rssi;
+		r_raw_rc_count = sumd_channel_count;
+
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	}
+
+	return (*dsm_updated | *st24_updated | *sumd_updated);
 }
 
 void
@@ -169,14 +194,17 @@ controls_tick() {
 #endif
 
 	perf_begin(c_gather_dsm);
-	bool dsm_updated, st24_updated;
-	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
+	bool dsm_updated, st24_updated, sumd_updated;
+	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
 	if (dsm_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 	}
 	if (st24_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
 	}
+	if (sumd_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -238,7 +266,7 @@ controls_tick() {
 	/*
 	 * If we received a new frame from any of the RC sources, process it.
 	 */
-	if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
+	if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
 
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
@@ -438,7 +466,7 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
+			if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated)
 				mixer_tick();
 
 		} else {
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 844e493cd..34c231174 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -14,7 +14,8 @@ SRCS		= adc.c \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
 		  ../systemlib/pwm_limit/pwm_limit.c \
-		  ../../lib/rc/st24.c
+		  ../../lib/rc/st24.c \
+		  ../../lib/rc/sumd.c
 
 ifeq ($(BOARD),px4io-v1)
 SRCS		+= i2c.c
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index bd777428f..874dc0c39 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -115,6 +115,7 @@
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
 #define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* ST24 input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SUMD		(1 << 15) /* SUMD input is valid */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */