rc/dsm: remove system field check, add new validity checks

- unfortunately we can't depend on the system field due to potential
external binding and non-genuine Spektrum equipment
 - reject any DSM frame with duplicate channels
 - add 16 channel mask
 - tighten valid PWM range 990-2010us (±100% travel is 1102-1898µs)
 - update RCTest rejected frame count

src/lib/rc/dsm.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 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
@@ -44,6 +44,7 @@
 #include <px4_platform_common/defines.h>
 
 #include <fcntl.h>
+#include <math.h>
 #include <unistd.h>
 #include <termios.h>
 #include <string.h>
@@ -53,6 +54,8 @@
 #include "common_rc.h"
 #include <drivers/drv_hrt.h>
 
+#include <include/containers/Bitset.hpp>
+
 #if defined(__PX4_NUTTX)
 #include <nuttx/arch.h>
 #define dsm_udelay(arg)    up_udelay(arg)
@@ -106,7 +109,7 @@ static uint16_t dsm_chan_count = 0;         /**< DSM channel count */
  */
 static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, uint16_t &value)
 {
-	if (raw == 0xffff) {
+	if (raw == 0 || raw == 0xffff) {
 		return false;
 	}
 
@@ -118,8 +121,26 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 		static constexpr uint16_t MASK_1024_SXPOS = 0x03FF;
 
 		channel = (raw & MASK_1024_CHANID) >> 10; // 6 bits
-		uint16_t servo_position = (raw & MASK_1024_SXPOS); // 10 bits
+
-		value = servo_position * 2; // scale to be equivalent to 2048 mode
+		const uint16_t servo_position = (raw & MASK_1024_SXPOS); // 10 bits
+
+		if (channel > DSM_MAX_CHANNEL_COUNT) {
+			PX4_DEBUG("invalid channel: %d\n", channel);
+			return false;
+		}
+
+		// PWM_OUT = (ServoPosition x 1.166μs) + Offset
+		static constexpr uint16_t offset = 903; // microseconds
+		value = roundf(servo_position * 1.166f) + offset;
+
+		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
+		//  ±100% travel is 1102µs to 1898 µs
+		if (value < 990 || value > 2010) {
+			// if the value is unrealistic, fail the parsing entirely
+			PX4_DEBUG("channel %d invalid range %d", channel, value);
+			return false;
+		}
+
 		return true;
 
 	} else if (shift == 11) {
@@ -127,32 +148,54 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 		//  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;
+		uint16_t servo_position = 0;
 
 		// from Spektrum Remote Receiver Interfacing Rev G Page 6
-		uint8_t chan = (raw & MASK_2048_CHANID) >> 11;
+		const bool phase = raw & (2 >> 15); // the phase is part of the X-Plus address (bit 15)
-		uint16_t servo_position = 0;
+		uint8_t chan = (raw >> 11) & 0x0F;
 
 		if (chan < 12) {
 			// Normal channels
+			static constexpr uint16_t MASK_2048_SXPOS = 0x07FF;
 			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;
 			}
 
+			if (chan > DSM_MAX_CHANNEL_COUNT) {
+				PX4_DEBUG("invalid channel: %d\n", chan);
+				return false;
+			}
+
 			servo_position = (raw & 0x01FF) << 2;
+
+			channel = chan;
+
+		} else {
+			PX4_DEBUG("invalid channel: %d\n", chan);
+			return false;
 		}
 
 		channel = chan;
-		value = servo_position;
+
+		// PWM_OUT = (ServoPosition x 0.583μs) + Offset
+		static constexpr uint16_t offset = 903; // microseconds
+		value = roundf(servo_position * 0.583f) + offset;
+
+		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
+		//  ±100% travel is 1102µs to 1898 µs
+		if (value < 990 || value > 2010) {
+			// if the value is unrealistic, fail the parsing entirely
+			PX4_DEBUG("channel %d invalid range %d", channel, value);
+			return false;
+		}
+
 		return true;
 	}
 
@@ -179,6 +222,12 @@ static bool dsm_guess_format(bool reset)
 		return false;
 	}
 
+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found_10;
+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found_11;
+
+	bool cs10_frame_valid = true;
+	bool cs11_frame_valid = true;
+
 	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
@@ -189,15 +238,43 @@ static bool dsm_guess_format(bool reset)
 		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)) {
-			cs10 |= (1 << channel);
+			// invalidate entire frame (for 1024) if channel already found, no duplicate channels per DSM frame
+			if (channels_found_10[channel]) {
+				cs10_frame_valid = false;
+
+			} else {
+				channels_found_10.set(channel);
+			}
 		}
 
-		if (dsm_decode_channel(raw, 11, channel, value) && (channel < 31)) {
+		if (dsm_decode_channel(raw, 11, channel, value)) {
-			cs11 |= (1 << channel);
+			// invalidate entire frame (for 2048) if channel already found, no duplicate channels per DSM frame
+			if (channels_found_11[channel]) {
+				cs11_frame_valid = false;
+
+			} else {
+				channels_found_11.set(channel);
+			}
+		}
 	}
 
-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
+	// add valid cs10 channels
+	if (cs10_frame_valid) {
+		for (unsigned channel = 0; channel < DSM_FRAME_CHANNELS; channel++) {
+			if (channels_found_10[channel]) {
+				cs10 |= 1 << channel;
+			}
+		}
+	}
+
+	// add valid cs11 channels
+	if (cs11_frame_valid) {
+		for (unsigned channel = 0; channel < DSM_FRAME_CHANNELS; channel++) {
+			if (channels_found_11[channel]) {
+				cs11 |= 1 << channel;
+			}
+		}
 	}
 
 	samples++;
@@ -227,8 +304,10 @@ static bool dsm_guess_format(bool reset)
 		0x1ff,	/* 9 channels (DX9, etc.) */
 		0x3ff,	/* 10 channels (DX10) */
 		0x1fff,	/* 13 channels (DX10t) */
-		0x3fff	/* 18 channels (DX10) */
+		0xffff,	/* 16 channels */
+		0x3ffff,/* 18 channels (DX10) */
 	};
+
 	unsigned votes10 = 0;
 	unsigned votes11 = 0;
 
@@ -480,91 +559,6 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 		}
 	}
 
-	/*
-	 * 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
-	 */
-	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-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
@@ -573,18 +567,42 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 	 * seven channels are being transmitted.
 	 */
 
+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found;
+
+	unsigned channel_decode_failures = 0;
+
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
 		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 
+		// ignore
+		if (raw == 0 || raw == 0xffff) {
+			continue;
+		}
+
 		uint8_t channel = 0;
 		uint16_t value = 0;
 
 		if (!dsm_decode_channel(raw, dsm_channel_shift, channel, value)) {
+			channel_decode_failures++;
 			continue;
 		}
 
+		// discard entire frame if at least half of it (4 channels) failed to decode
+		if (channel_decode_failures >= 4) {
+			return false;
+		}
+
+		// abort if channel already found, no duplicate channels per DSM frame
+		if (channels_found[channel]) {
+			PX4_DEBUG("duplicate channel %d\n\n", channel);
+			return false;
+
+		} else {
+			channels_found.set(channel);
+		}
+
 		/* reset bit guessing state machine if the channel index is out of bounds */
 		if (channel > DSM_MAX_CHANNEL_COUNT) {
 			dsm_guess_format(true);
@@ -624,11 +642,7 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 			break;
 		}
 
-		// Scaling
+		values[channel] = value;
-		//  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;
 	}
 
 	/*

src/lib/rc/rc_tests/RCTest.cpp

@@ -32,7 +32,7 @@ private:
 	bool crsfTest();
 	bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0);
 	bool dsmTest10Ch();
-	bool dsmTest12Ch();
+	bool dsmTest16Ch();
 	bool sbus2Test();
 	bool st24Test();
 	bool sumdTest();
@@ -42,7 +42,7 @@ bool RCTest::run_tests()
 {
 	ut_run_test(crsfTest);
 	ut_run_test(dsmTest10Ch);
-	ut_run_test(dsmTest12Ch);
+	ut_run_test(dsmTest16Ch);
 	ut_run_test(sbus2Test);
 	ut_run_test(st24Test);
 	ut_run_test(sumdTest);
@@ -138,12 +138,12 @@ bool RCTest::crsfTest()
 
 bool RCTest::dsmTest10Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 64, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
 }
 
-bool RCTest::dsmTest12Ch()
+bool RCTest::dsmTest16Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 454, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 3, 1500);
 }
 
 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
@@ -172,7 +172,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 	unsigned dsm_frame_drops = 0;
 	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
 
-	int rate_limiter = 0;
+	int count = 0;
 	unsigned last_drop = 0;
 
 	dsm_proto_init();
@@ -192,7 +192,9 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 					&dsm_11_bit, &dsm_frame_drops, nullptr, max_channels);
 
 		if (result) {
+			if (count > (16 * 10)) { // need to process enough data to have full channel count
 				ut_compare("num_values == expected_chancount", num_values, expected_chancount);
+			}
 
 			ut_test(abs((int)chan0 - (int)rc_values[0]) < 30);
 
@@ -208,13 +210,13 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 			last_drop = dsm_frame_drops;
 		}
 
-		rate_limiter++;
+		count++;
 	}
 
 	fclose(fp);
 
 	ut_test(ret == EOF);
-	PX4_INFO("drop: %d", (int)last_drop);
+	//PX4_INFO("drop: %d", (int)last_drop);
 	ut_compare("last_drop == expected_dropcount", last_drop, expected_dropcount);
 
 	return true;