Skip to content

Commit

Permalink
rc/dsm: remove system field check, add new validity checks
Browse files Browse the repository at this point in the history 
 - 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
  • Loading branch information
@dagar @LorenzMeier
dagar authored and LorenzMeier committed Jan 1, 2021
1 parent 4b0038b commit a6274bc
Show file tree
Hide file tree
Showing 2 changed files with 132 additions and 116 deletions.
228 changes: 121 additions & 107 deletions src/lib/rc/dsm.cpp
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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>
Expand All @@ -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)
Expand Down Expand Up @@ -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;
}

Expand All @@ -118,41 +121,81 @@ 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) {
// 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;

// from Spektrum Remote Receiver Interfacing Rev G Page 6
uint8_t chan = (raw & MASK_2048_CHANID) >> 11;
uint16_t servo_position = 0;

// from Spektrum Remote Receiver Interfacing Rev G Page 6
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;
}

Expand Down Expand Up @@ -180,6 +223,12 @@ 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++) {

Expand All @@ -190,15 +239,43 @@ 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)) {
// 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);
}
}
}

if (dsm_decode_channel(raw, 11, channel, value) && (channel < 31)) {
cs11 |= (1 << 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;
}
}
}

/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
// 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++;
Expand Down Expand Up @@ -229,8 +306,10 @@ 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;

Expand Down Expand Up @@ -470,91 +549,6 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
}
}

/*
* 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
Expand All @@ -563,18 +557,42 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
* 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);
Expand Down Expand Up @@ -614,11 +632,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
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;
}

/*
Expand Down

4 comments on commit a6274bc

@mengchaoheng
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why can't the v1.11.x version be used for SITL debugging with vscode, but the current master branch version can. I want to use a tag on the stable branch for secondary development instead of master, because I found that the current master branch upload to CUAV nano is abnormal, and the initial ringtone is incomplete. So, I need the stability of the stable branch, and I need to perform single-step debugging in vscode. What should I do?

@mcsauder
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This isn the conundrum of continuous development.

@mengchaoheng
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This isn the conundrum of continuous development.

What do you mean？This isn't the conundrum of continuous development？

@mcsauder
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fix it and your will be the answer.

Please sign in to comment.