rc/dsm: decode improvements

- always check system field for validity
 - reject any data outside of "servo position" valid range from Spektrum specification
 - properly support XPlus channels (12+)
 - debug message if channel count changes

msg/input_rc.msg

@@ -19,12 +19,17 @@ uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14
 uint8 RC_INPUT_MAX_CHANNELS = 18 	# Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
 
 uint64 timestamp_last_signal		# last valid reception time
-uint32 channel_count			# number of channels actually being seen
+
+uint8 channel_count			# number of channels actually being seen
+
 int32 rssi				# receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception
+
 bool rc_failsafe			# explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly.
 bool rc_lost				# RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usually means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
+
 uint16 rc_lost_frame_count		# Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality.
 uint16 rc_total_frame_count		# Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality.
 uint16 rc_ppm_frame_length		# Length of a single PPM frame. Zero for non-PPM systems
+
 uint8 input_source			# Input source
 uint16[18] values			# measured pulse widths for each of the supported channels

src/drivers/rc_input/RCInput.cpp

@@ -38,7 +38,7 @@
 using namespace time_literals;
 
 #if defined(SPEKTRUM_POWER)
-static bool bind_spektrum(int arg);
+static bool bind_spektrum(int arg = DSMX8_BIND_PULSES);
 #endif /* SPEKTRUM_POWER */
 
 constexpr char const *RCInput::RC_SCAN_STRING[];
@@ -652,7 +652,8 @@ int RCInput::custom_command(int argc, char *argv[])
 	const char *verb = argv[0];
 
 	if (!strcmp(verb, "bind")) {
-		bind_spektrum(DSMX8_BIND_PULSES);
+		// TODO: publish vehicle_command
+		bind_spektrum();
 		return 0;
 	}
 

src/lib/rc/dsm.cpp

@@ -79,10 +79,6 @@ 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 bool
-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
  *
@@ -108,22 +104,59 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
  * @param[out] value pointer to returned channel value
  * @return true=raw value successfully decoded
  */
-static bool
+static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, uint16_t &value)
-dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
 {
-
 	if (raw == 0xffff) {
 		return false;
 	}
 
-	*channel = (raw >> shift) & 0xf;
+	if (shift == 10) {
+		// 1024 Mode: This format is used only by DSM2/22ms mode. All other modes use 2048 data.
+		//  Bits 15-10 Channel ID
+		//  Bits 9-0   Servo Position
+		static constexpr uint16_t MASK_1024_CHANID = 0xFC00;
+		static constexpr uint16_t MASK_1024_SXPOS = 0x03FF;
 
-	uint16_t data_mask = (1 << shift) - 1;
+		channel = (raw & MASK_1024_CHANID) >> 10; // 6 bits
-	*value = raw & data_mask;
+		uint16_t servo_position = (raw & MASK_1024_SXPOS); // 10 bits
+		value = servo_position * 2; // scale to be equivalent to 2048 mode
+		return true;
 
-	//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
+	} else if (shift == 11) {
+		// 2048 Mode
+		//  Bits 15    Servo Phase
+		//  Bits 14-11 Channel ID
+		//  Bits 10-0  Servo Position
+		static constexpr uint16_t MASK_2048_CHANID = 0x7800;
+		static constexpr uint16_t MASK_2048_SXPOS = 0x07FF;
 
-	return true;
+		// from Spektrum Remote Receiver Interfacing Rev G Page 6
+		uint8_t chan = (raw & MASK_2048_CHANID) >> 11;
+		uint16_t servo_position = 0;
+
+		if (chan < 12) {
+			// Normal channels
+			servo_position = (raw & MASK_2048_SXPOS);
+
+		} else if (chan == 12) {
+			// XPlus channels
+			chan += ((raw >> 9) & 0x03);
+
+			const bool phase = raw & (2 >> 15); // the phase is part of the X-Plus address (bit 15)
+
+			if (phase) {
+				chan += 4;
+			}
+
+			servo_position = (raw & 0x01FF) << 2;
+		}
+
+		channel = chan;
+		value = servo_position;
+		return true;
+	}
+
+	return false;
 }
 
 /**
@@ -152,14 +185,16 @@ dsm_guess_format(bool reset)
 
 		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
-		unsigned channel, value;
+
+		uint8_t channel = 0;
+		uint16_t value = 0;
 
 		/* if the channel decodes, remember the assigned number */
-		if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) {
+		if (dsm_decode_channel(raw, 10, channel, value) && (channel < 31)) {
 			cs10 |= (1 << channel);
 		}
 
-		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) {
+		if (dsm_decode_channel(raw, 11, channel, value) && (channel < 31)) {
 			cs11 |= (1 << channel);
 		}
 
@@ -186,8 +221,6 @@ dsm_guess_format(bool reset)
 	 *     stream, we may want to sniff for longer in some cases when we think we
 	 *     are talking to a DSM2 receiver in high-resolution mode (so that we can
 	 *     reject it, ideally).
-	 *     See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
-	 *     of this issue.
 	 */
 	static uint32_t masks[] = {
 		0x3f,	/* 6 channels (DX6) */
@@ -415,6 +448,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	 * but rssi dBm is a useful value.
 	 */
 
+	// The SPM4649T with firmware version 1.1RC9 or later will have RSSI in place of fades
 	if (rssi_percent) {
 		if (((int8_t *)dsm_frame)[0] < 0) {
 			/*
@@ -446,6 +480,80 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	 * 0xa2 22MS 2048 DSMX
 	 * 0xb2 11MS 2048 DSMX
 	 */
+	const uint8_t system = dsm_frame[1];
+
+	switch (system) {
+	case 0x00: // SURFACE DSM1
+		// unsupported
+		PX4_DEBUG("ERROR system: SURFACE DSM1 (%X) unsupported\n", system);
+		return false;
+
+	case 0x01: // DSM2 1024 22ms
+		if (dsm_channel_shift != 10) {
+			dsm_guess_format(true);
+			return false;
+		}
+
+		break;
+
+	case 0x02: // DSM2 1024 (MC24)
+		// unsupported
+		PX4_DEBUG("ERROR system: DSM2 1024 (MC24) (%X) unsupported\n", system);
+		return false;
+
+	case 0x12: // DSM2 2048 11ms
+		if (dsm_channel_shift != 11) {
+			dsm_guess_format(true);
+			return false;
+		}
+
+		break;
+
+	case 0x23: // SURFACE DSM2 16.5ms
+		// unsupported
+		PX4_DEBUG("ERROR system: DSM2 16.5ms (%X) unsupported\n", system);
+		return false;
+
+	case 0x50: // DSM MARINE
+		// unsupported
+		PX4_DEBUG("ERROR system: DSM MARINE (%X) unsupported\n", system);
+		return false;
+
+	case 0x92: // DSMJ
+		// unsupported
+		PX4_DEBUG("ERROR system: DSMJ (%X) unsupported\n", system);
+		return false;
+
+	case 0xA2: // DSMX 22ms OR DSMR 11ms or DSMR 22ms
+		if (dsm_channel_shift != 11) {
+			dsm_guess_format(true);
+			return false;
+		}
+
+		break;
+
+	case 0xA4: // DSMR 5.5ms
+		// unsupported
+		PX4_DEBUG("ERROR system: DSMR 5.5ms (%X) unsupported\n", system);
+		return false;
+
+	case 0xAE: // NOT_BOUND
+		PX4_DEBUG("ERROR system: NOT_BOUND (%X) unsupported\n", system);
+		return false;
+
+	case 0xB2: // DSMX 11ms
+		if (dsm_channel_shift != 11) {
+			dsm_guess_format(true);
+			return false;
+		}
+
+		break;
+
+	default:
+		// ERROR
+		PX4_DEBUG("ERROR system: %X unsupported\n", system);
+		return false;
+	}
 
 	/*
 	 * Each channel is a 16-bit unsigned value containing either a 10-
@@ -459,9 +567,11 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 
 		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
-		unsigned channel, value;
 
-		if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) {
+		uint8_t channel = 0;
+		uint16_t value = 0;
+
+		if (!dsm_decode_channel(raw, dsm_channel_shift, channel, value)) {
 			continue;
 		}
 
@@ -481,27 +591,6 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 			*num_values = channel + 1;
 		}
 
-		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
-		if (dsm_channel_shift == 10) {
-			value *= 2;
-		}
-
-		/*
-		 * Spektrum scaling is special. There are these basic considerations
-		 *
-		 *   * Midpoint is 1520 us
-		 *   * 100% travel channels are +- 400 us
-		 *
-		 * We obey the original Spektrum scaling (so a default setup will scale from
-		 * 1100 - 1900 us), but we do not obey the weird 1520 us center point
-		 * and instead (correctly) center the center around 1500 us. This is in order
-		 * to get something useful without requiring the user to calibrate on a digital
-		 * link for no reason.
-		 */
-
-		/* scaled integer for decent accuracy while staying efficient */
-		value = ((((int)value - 1024) * 1000) / 1700) + 1500;
-
 		/*
 		 * Store the decoded channel into the R/C input buffer, taking into
 		 * account the different ideas about channel assignement that we have.
@@ -511,21 +600,25 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 		 */
 		switch (channel) {
 		case 0:
-			channel = 2;
+			channel = 2; // Spektrum Throttle (0) -> 2
 			break;
 
 		case 1:
-			channel = 0;
+			channel = 0; // Spektrum Aileron (1) -> 0
 			break;
 
 		case 2:
-			channel = 1;
+			channel = 1; // Spektrum Elevator (2) -> 1
 
 		default:
 			break;
 		}
 
-		values[channel] = value;
+		// Scaling
+		//  See Specification for Spektrum Remote Receiver Interfacing Rev G 2019 January 22
+		//  2048 Mode Scaling: PWM_OUT = (ServoPosition x 0.583μs) + Offset (903μs)
+		//  scaled integer for decent accuracy while staying efficient (0.583us ~= 1194/2048)
+		values[channel] = (value * 1194) / 2048 + 903;
 	}
 
 	/*
@@ -554,8 +647,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 
 	/* check all values */
 	for (unsigned i = 0; i < *num_values; i++) {
-		/* if the value is unrealistic, fail the parsing entirely */
+		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
-		if (values[i] < 600 || values[i] > 2400) {
+		if (values[i] < 903 || values[i] > 2097) {
+			// if the value is unrealistic, fail the parsing entirely
 #ifdef DSM_DEBUG
 			printf("DSM: VALUE RANGE FAIL: %d: %d\n", (int)i, (int)values[i]);
 #endif
@@ -591,12 +685,8 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
  */
 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,
+	  int8_t *rssi, unsigned max_values)
-	  unsigned max_values)
 {
-	int		ret = 1;
-	hrt_abstime	now;
-
 	/*
 	 * The S.BUS protocol doesn't provide reliable framing,
 	 * so we detect frame boundaries by the inter-frame delay.
@@ -612,14 +702,14 @@ dsm_input(int fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint
 	 * provides a degree of protection. Of course, it would be better
 	 * if we didn't drop bytes...
 	 */
-	now = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
 
 	/*
 	 * Fetch bytes, but no more than we would need to complete
 	 * a complete frame.
 	 */
 
-	ret = read(fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0]));
+	int ret = read(fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0]));
 
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1) {

src/lib/rc/dsm.h

@@ -87,8 +87,8 @@ enum DSM_CMD {							/* DSM bind states */
 	DSM_CMD_BIND_REINIT_UART
 };
 
-#define DSM2_BIND_PULSES 3	/* DSM_BIND_START parameter, pulses required to start dsm2 pairing */
+#define DSM2_BIND_PULSES 3	/* DSM_BIND_START parameter, pulses required to start pairing DSM2 22ms */
-#define DSMX_BIND_PULSES 7	/* DSM_BIND_START parameter, pulses required to start dsmx pairing */
+#define DSMX_BIND_PULSES 7	/* DSM_BIND_START parameter, pulses required to start pairing DSMx 22ms */
-#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START parameter, pulses required to start 8 or more channel dsmx pairing */
+#define DSMX8_BIND_PULSES 9     /* DSM_BIND_START parameter, pulses required to start pairing DSMx 11ms */
 
 __END_DECLS

src/lib/rc/rc_tests/RCTest.cpp

@@ -138,17 +138,16 @@ bool RCTest::crsfTest()
 
 bool RCTest::dsmTest10Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 64, 1500);
 }
 
 bool RCTest::dsmTest12Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 6, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 454, 1500);
 }
 
 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
 {
-
 	FILE *fp;
 	fp = fopen(filepath, "rt");
 
@@ -205,7 +204,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 		}
 
 		if (last_drop != (dsm_frame_drops)) {
-			PX4_INFO("frame dropped, now #%d", (dsm_frame_drops));
+			//PX4_INFO("frame dropped, now #%d", (dsm_frame_drops));
 			last_drop = dsm_frame_drops;
 		}
 
@@ -216,7 +215,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 
 	ut_test(ret == EOF);
 	PX4_INFO("drop: %d", (int)last_drop);
-	ut_test(last_drop == expected_dropcount);
+	ut_compare("last_drop == expected_dropcount", last_drop, expected_dropcount);
 
 	return true;
 }

src/modules/rc_update/rc_update.cpp

@@ -320,6 +320,13 @@ RCUpdate::Run()
 
 	if (_input_rc_sub.copy(&rc_input)) {
 
+		// warn if the channel count is changing (possibly indication of error)
+		if (!rc_input.rc_lost && (_channel_count_previous != rc_input.channel_count) && (_channel_count_previous > 0)) {
+			PX4_DEBUG("RC channel count changed %d -> %d", _channel_count_previous, rc_input.channel_count);
+		}
+
+		_channel_count_previous = rc_input.channel_count;
+
 		/* detect RC signal loss */
 		bool signal_lost = true;
 

src/modules/rc_update/rc_update.h

@@ -166,6 +166,8 @@ private:
 
 	hrt_abstime _last_rc_to_param_map_time = 0;
 
+	uint8_t _channel_count_previous{0};
+
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 
 	DEFINE_PARAMETERS(