diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 2469cfa46f..8857e4e6b8 100644 --- a/msg/input_rc.msg +++ b/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 diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 2d7546a33b..74aabb21f5 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/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; } diff --git a/src/lib/rc/dsm.cpp b/src/lib/rc/dsm.cpp index 05f3b36673..746981c3ce 100644 --- a/src/lib/rc/dsm.cpp +++ b/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 -dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) +static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, uint16_t &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; - *value = raw & data_mask; + 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 + 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 */ - if (values[i] < 600 || values[i] > 2400) { + // Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1) + 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, - unsigned max_values) + int8_t *rssi, 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) { diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 95f3323b97..874a760152 100644 --- a/src/lib/rc/dsm.h +++ b/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 DSMX_BIND_PULSES 7 /* DSM_BIND_START parameter, pulses required to start dsmx pairing */ -#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START parameter, pulses required to start 8 or more channel dsmx 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 pairing DSMx 22ms */ +#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START parameter, pulses required to start pairing DSMx 11ms */ __END_DECLS diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index ab5cf3fdc4..aca8684a5c 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/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; } diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index e4fd5c8daa..9c9469e4f1 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/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; diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 39b6b91e87..fdd4ef8cac 100644 --- a/src/modules/rc_update/rc_update.h +++ b/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(