From 95dd2f7e8dfca35219fc4336f4f909bcbc3d461a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 31 Dec 2020 21:48:46 -0500 Subject: [PATCH] rc/dsm: remove system field check, add new validity checks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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 | 230 +++++++++++++++++---------------- src/lib/rc/rc_tests/RCTest.cpp | 20 +-- 2 files changed, 133 insertions(+), 117 deletions(-) diff --git a/src/lib/rc/dsm.cpp b/src/lib/rc/dsm.cpp index 852dbbf393..95aa4efb46 100644 --- a/src/lib/rc/dsm.cpp +++ b/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 #include +#include #include #include #include @@ -53,6 +54,8 @@ #include "common_rc.h" #include +#include + #if defined(__PX4_NUTTX) #include #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; - uint16_t servo_position = 0; + const bool phase = raw & (2 >> 15); // the phase is part of the X-Plus address (bit 15) + 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 channels_found_10; + px4::Bitset 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)) { - cs10 |= (1 << channel); + if (dsm_decode_channel(raw, 10, channel, value)) { + // 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)) { - cs11 |= (1 << channel); - } + if (dsm_decode_channel(raw, 11, channel, value)) { + // invalidate entire frame (for 2048) if channel already found, no duplicate channels per DSM frame + if (channels_found_11[channel]) { + cs11_frame_valid = false; - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ + } else { + channels_found_11.set(channel); + } + } + } + + // 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 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 - // 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; + values[channel] = value; } /* diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index aca8684a5c..e65d4d6f72 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/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) { - ut_compare("num_values == expected_chancount", num_values, expected_chancount); + 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;