Gather RSSI data from Spektrum Telemetry receivers

On SPM4649T receivers with firmware versions at least 1.1RC9, the
serial data will contain an rssi value in dbm, as outlined in the
Remote Receiver Interfacing document section 8.3.1.

If the value received is greater than or equal to zero, the receiver
does not support rssi data, and the incoming value will be ignored.
However, if the value is negative, we can use the rssi value.

When we have a valid rssi, it gets mapped to a percentage from 0 to
100 as expected by mavlink. This mapping is constructed as a
logarithmic function over Spektrum's published minimum and maximum
rssi values, -92dBm to -42dBm as 0 to 100:
100 Log10[1 + (x - min) * (9 / (max - min))]

This change updates all calls to the dsm input rountes to return
the rssi value.

Note that one place this doesn't work with the px4io enabled.
There is a comment left in the absence of analog rssi that:
"we do not actually get digital RSSI regs[PX4IO_P_RAW_RC_NRSSI]".
This restriction has been left in place, as removing it exposes a
problem where the frequency of the control tick is greater than
that of valid dsm frames so the rssi isn't valid every cycle.

src/drivers/px4fmu/fmu.cpp

@@ -1615,15 +1615,17 @@ PX4FMU::cycle()
 				   || _cycle_timestamp - _rc_scan_begin < rc_scan_max) {
 
 				if (newBytes > 0) {
+					int8_t dsm_rssi;
+
 					// parse new data
 					rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
-							       &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
+							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new DSM frame. Publish it.
 						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
 						fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp,
-							   false, false, frame_drops);
+							   false, false, frame_drops, dsm_rssi);
 						_rc_scan_locked = true;
 					}
 				}

src/drivers/spektrum_rc/spektrum_rc.cpp

@@ -127,20 +127,18 @@ void task_main(int argc, char *argv[])
 
 		bool dsm_11_bit;
 		unsigned frame_drops;
+		int8_t dsm_rssi;
 
 		// parse new data
 		bool rc_updated = dsm_parse(now, rx_buf, newbytes, &raw_rc_values[0], &raw_rc_count,
-					    &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
+					    &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
 		UNUSED(dsm_11_bit);
 
 		if (rc_updated) {
 
 			input_rc_s input_rc = {};
 
-			// We don't know RSSI.
-			const int rssi = -1;
-
-			fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, rssi,
+			fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, dsm_rssi,
 				      input_rc);
 
 			if (rc_pub == nullptr) {

src/lib/rc/dsm.cpp

@@ -49,6 +49,7 @@
 #include <string.h>
 
 #include "dsm.h"
+#include "spektrum_rssi.h"
 #include "common_rc.h"
 #include <drivers/drv_hrt.h>
 
@@ -78,8 +79,11 @@ static unsigned dsm_channel_shift = 0;			/**< Channel resolution, 0=unknown, 1=1
 static unsigned dsm_frame_drops = 0;			/**< Count of incomplete DSM frames */
 static uint16_t dsm_chan_count = 0;         /**< DSM channel count */
 
+static const SpektrumRssi spektrum_rssi;
+
 static bool
-dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values);
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values,
+	   int8_t *rssi_percent);
 
 /**
  * Attempt to decode a single channel raw channel datum
@@ -383,7 +387,8 @@ dsm_bind(uint16_t cmd, int pulses)
  * @return true=DSM frame successfully decoded, false=no update
  */
 bool
-dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values)
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values,
+	   int8_t *rssi_percent)
 {
 	/*
 	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
@@ -406,9 +411,45 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	}
 
 	/*
-	 * The encoding of the first two bytes is uncertain, so we're
-	 * going to ignore them for now.
+	 * The first byte represents the rssi in dBm on telemetry receivers with updated
+	 * firmware, or fades on others. If the value is less than zero, it's rssi.
+	 * We have other ways to detect bad link metrics, so ignore positive values,
+	 * but rssi dBm is a useful value.
+	 */
+
+	if (rssi_percent) {
+		if (((int8_t *)dsm_frame)[0] < 0) {
+			/*
+			 * RSSI is a signed integer between -42dBm and -92dBm
+			 * If signal is lost, the value is -128
+			 */
+			int8_t dbm = (int8_t)dsm_frame[0];
+
+			if (dbm == -128) {
+				*rssi_percent = 0;
+
+			} else {
+				*rssi_percent = spektrum_rssi.dbm_to_percent(dbm);
+			}
+
+		} else {
+			/* if we don't know the rssi, anything over 100 will invalidate it */
+			*rssi_percent = 127;
+		}
+	}
+
+	/*
+	 * The second byte indicates the protocol and frame rate. We have a
+	 * guessing state machine, so we don't need to use this. At any rate,
+	 * these are the allowable values:
 	 *
+	 * 0x01 22MS 1024 DSM2
+	 * 0x12 11MS 2048 DSM2
+	 * 0xa2 22MS 2048 DSMX
+	 * 0xb2 11MS 2048 DSMX
+	 */
+
+	/*
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
 	 * either 10 or 11 bits. The MSB may also be set to indicate the
@@ -547,10 +588,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
  * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
  * @param[out] n_butes number of bytes read
  * @param[out] bytes pointer to the buffer of read bytes
+ * @param[out] rssi value in percent, if supported, or 127
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
 dsm_input(int fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes,
+	  int8_t *rssi,
 	  unsigned max_values)
 {
 	int		ret = 1;
@@ -592,12 +635,12 @@ dsm_input(int fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint
 	/*
 	 * Try to decode something with what we got
 	 */
-	return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values);
+	return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, rssi, max_values);
 }
 
 bool
 dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t *values,
-	  uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels)
+	  uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, int8_t *rssi_percent, uint16_t max_channels)
 {
 
 	/* this is set by the decoding state machine and will default to false
@@ -665,7 +708,7 @@ dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t
 				 * Great, it looks like we might have a frame.  Go ahead and
 				 * decode it.
 				 */
-				decode_ret = dsm_decode(now, &dsm_chan_buf[0], &dsm_chan_count, dsm_11_bit, max_channels);
+				decode_ret = dsm_decode(now, &dsm_chan_buf[0], &dsm_chan_count, dsm_11_bit, max_channels, rssi_percent);
 
 				/* we consumed the partial frame, reset */
 				dsm_partial_frame_count = 0;

src/lib/rc/dsm.h

@@ -58,10 +58,10 @@ __EXPORT void	dsm_deinit(void);
 __EXPORT void	dsm_proto_init(void);
 __EXPORT int	dsm_config(int dsm_fd);
 __EXPORT bool	dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes,
-			  uint8_t **bytes, unsigned max_values);
+			  uint8_t **bytes, int8_t *rssi, unsigned max_values);
 
 __EXPORT bool	dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t *values,
-			  uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels);
+			  uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, int8_t *rssi_percent, uint16_t max_channels);
 
 #ifdef SPEKTRUM_POWER
 __EXPORT void	dsm_bind(uint16_t cmd, int pulses);

src/lib/rc/rc_tests/RCTest.cpp

@@ -100,7 +100,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 
 		// Pipe the data into the parser
 		bool result = dsm_parse(f * 1e6f, &frame[0], len, rc_values, &num_values,
-					&dsm_11_bit, &dsm_frame_drops, max_channels);
+					&dsm_11_bit, &dsm_frame_drops, nullptr, max_channels);
 
 		if (result) {
 			ut_compare("num_values == expected_chancount", num_values, expected_chancount);

src/lib/rc/spektrum_rssi.h

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *	Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *	notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *	notice, this list of conditions and the following disclaimer in
+ *	the documentation and/or other materials provided with the
+ *	distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *	used to endorse or promote products derived from this software
+ *	without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file spektrum_rssi.h
+ *
+ * RSSI dBm to percentage conversion for Spektrum telemetry receivers
+ *
+ * @author Kurt Kiefer <kekiefer@gmail.com>
+ */
+
+#pragma once
+
+#include <math.h>
+
+#define MIN_RSSI_DBM (-92.0f)
+#define MAX_RSSI_DBM (-42.0f)
+
+class SpektrumRssi
+{
+	int8_t lu_dbm_percent[129];
+
+public:
+	SpektrumRssi()
+	{
+		for (int i = 0; i <= 128; i++) {
+			float rssi_dbm = -1.0f * (float)i;
+			float percent;
+
+			if (rssi_dbm > MAX_RSSI_DBM) {
+				percent = 100.0f;
+
+			} else if (rssi_dbm < MIN_RSSI_DBM) {
+				percent = 0.0f;
+
+			} else {
+				percent = 100.0f * log10f(1 + (rssi_dbm - MIN_RSSI_DBM) * (9.0f / (MAX_RSSI_DBM - MIN_RSSI_DBM)));
+			}
+
+			lu_dbm_percent[i] = (int8_t)roundf(percent);
+		}
+	}
+
+	int8_t dbm_to_percent(int8_t dbm) const
+	{
+		return lu_dbm_percent[abs(dbm)];
+	}
+};

src/modules/px4iofirmware/controls.c

@@ -78,8 +78,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
 	bool dsm_11_bit;
+	int8_t spektrum_rssi;
 	*dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
-				 PX4IO_RC_INPUT_CHANNELS);
+				 &spektrum_rssi, PX4IO_RC_INPUT_CHANNELS);
 
 	if (*dsm_updated) {
 
@@ -93,6 +94,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 
+		*rssi = spektrum_rssi;
 	}
 
 	perf_end(c_gather_dsm);