Merge pull request #1420 from PX4/rctype

Encode RC type in RSSI field for GCS

2 files changed, 55 insertions, 2 deletions

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index bed915bbe..978aee118 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1719,7 +1719,53 @@ protected:
 			msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
 			msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
 			msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
-			msg.rssi = rc.rssi;
+
+			/* RSSI has a max value of 100, and when Spektrum or S.BUS are
+			 * available, the RSSI field is invalid, as they do not provide
+			 * an RSSI measurement. Use an out of band magic value to signal
+			 * these digital ports. XXX revise MAVLink spec to address this.
+			 * One option would be to use the top bit to toggle between RSSI
+			 * and input source mode.
+			 *
+			 * Full RSSI field: 0b 1 111 1111
+			 *
+			 *                     ^ If bit is set, RSSI encodes type + RSSI
+			 *
+			 *                       ^ These three bits encode a total of 8
+			 *                         digital RC input types.
+			 *                         0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
+			 *                           ^ These four bits encode a total of
+			 *                             16 RSSI levels. 15 = full, 0 = no signal
+			 *
+			 */
+
+			/* Initialize RSSI with the special mode level flag */
+			msg.rssi = (1 << 7);
+
+			/* Set RSSI */
+			msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
+
+			switch (rc.input_source) {
+				case RC_INPUT_SOURCE_PX4FMU_PPM:
+				/* fallthrough */
+				case RC_INPUT_SOURCE_PX4IO_PPM:
+					msg.rssi |= (0 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
+					msg.rssi |= (1 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SBUS:
+					msg.rssi |= (2 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_ST24:
+					msg.rssi |= (3 << 4);
+					break;
+			}
+
+			if (rc.rc_lost) {
+				/* RSSI is by definition zero */
+				msg.rssi = 0;
+			}
 
 			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index e3cb8d820..58c9429b6 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -41,6 +41,7 @@
 #include <stdbool.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
 #include <rc/st24.h>
@@ -70,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 	uint8_t *bytes;
 	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
 	if (*dsm_updated) {
-		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
@@ -91,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
+		st24_rssi = RC_INPUT_RSSI_MAX;
 		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
 					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
@@ -170,6 +171,12 @@ controls_tick() {
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_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;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);