Merge pull request #2061 from PX4/rssi_cleanup

IO RSSI handling: Fix RSSI for all protocols.

3 files changed, 24 insertions, 9 deletions

diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index d44728a71..a24d8814f 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -65,7 +65,7 @@
 /**
  * Maximum RSSI value
  */
-#define RC_INPUT_RSSI_MAX	255
+#define RC_INPUT_RSSI_MAX	100
 
 /**
  * @addtogroup topics
diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c
index a98c986bb..cea7790ec 100644
--- a/src/lib/rc/sumd.c
+++ b/src/lib/rc/sumd.c
@@ -269,7 +269,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe
 			uint8_t _cnt = *rx_count + 1;
 			*rx_count = _cnt;
 		
-			*rssi = 255;
+			*rssi = 100;
 
 			/* received Channels */
 			if ((uint16_t)_rxpacket.length > max_chan_count) {
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index e04ffc940..ac004f212 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -99,6 +99,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 
 	if (*st24_updated) {
 
+		/* ensure ADC RSSI is disabled */
+		r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+
 		*rssi = st24_rssi;
 		r_raw_rc_count = st24_channel_count;
 
@@ -116,14 +119,14 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
-		st24_rssi = RC_INPUT_RSSI_MAX;
+		sumd_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;
+		/* not setting RSSI since SUMD does not provide one */
 		r_raw_rc_count = sumd_channel_count;
 
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
@@ -187,12 +190,20 @@ controls_tick() {
 			/* use 1:1 scaling on 3.3V ADC input */
 			unsigned mV = counts * 3300 / 4096;
 
-			/* scale to 0..253 */
-			rssi = mV / 13;
+			/* scale to 0..253 and lowpass */
+			rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
+			if (rssi > RC_INPUT_RSSI_MAX) {
+				rssi = RC_INPUT_RSSI_MAX;
+			}
 		}
 	}
 #endif
 
+	/* zero RSSI if signal is lost */
+	if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) {
+		rssi = 0;
+	}
+
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated, sumd_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
@@ -215,22 +226,26 @@ controls_tick() {
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
 
-		rssi = 255;
+		unsigned sbus_rssi = RC_INPUT_RSSI_MAX;
 
 		if (sbus_frame_drop) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
-			rssi = 100;
+			sbus_rssi = RC_INPUT_RSSI_MAX / 2;
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		}
 
 		if (sbus_failsafe) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
-			rssi = 0;
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 		}
 
+		/* set RSSI to an emulated value if ADC RSSI is off */ 
+		if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+			rssi = sbus_rssi;
+		}
+
 	}
 
 	perf_end(c_gather_sbus);