diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 897c769b0b..5c0ab0ed29 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1615,15 +1615,17 @@ PX4FMU::cycle() || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { if (newBytes > 0) { + int8_t dsm_rssi; + // parse new data rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, - &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new DSM frame. Publish it. _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, - false, false, frame_drops); + false, false, frame_drops, dsm_rssi); _rc_scan_locked = true; } } diff --git a/src/drivers/spektrum_rc/spektrum_rc.cpp b/src/drivers/spektrum_rc/spektrum_rc.cpp index 899c31eea6..780c0146af 100644 --- a/src/drivers/spektrum_rc/spektrum_rc.cpp +++ b/src/drivers/spektrum_rc/spektrum_rc.cpp @@ -127,20 +127,18 @@ void task_main(int argc, char *argv[]) bool dsm_11_bit; unsigned frame_drops; + int8_t dsm_rssi; // parse new data bool rc_updated = dsm_parse(now, rx_buf, newbytes, &raw_rc_values[0], &raw_rc_count, - &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS); UNUSED(dsm_11_bit); if (rc_updated) { input_rc_s input_rc = {}; - // We don't know RSSI. - const int rssi = -1; - - fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, rssi, + fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, dsm_rssi, input_rc); if (rc_pub == nullptr) { diff --git a/src/lib/rc/dsm.cpp b/src/lib/rc/dsm.cpp index 78959a0127..e0a0b9402d 100644 --- a/src/lib/rc/dsm.cpp +++ b/src/lib/rc/dsm.cpp @@ -49,6 +49,7 @@ #include #include "dsm.h" +#include "spektrum_rssi.h" #include "common_rc.h" #include @@ -78,8 +79,11 @@ 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 const SpektrumRssi spektrum_rssi; + static bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values); +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 @@ -383,7 +387,8 @@ dsm_bind(uint16_t cmd, int pulses) * @return true=DSM frame successfully decoded, false=no update */ bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values) +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values, + int8_t *rssi_percent) { /* debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", @@ -406,9 +411,45 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } /* - * The encoding of the first two bytes is uncertain, so we're - * going to ignore them for now. + * The first byte represents the rssi in dBm on telemetry receivers with updated + * firmware, or fades on others. If the value is less than zero, it's rssi. + * We have other ways to detect bad link metrics, so ignore positive values, + * but rssi dBm is a useful value. + */ + + if (rssi_percent) { + if (((int8_t *)dsm_frame)[0] < 0) { + /* + * RSSI is a signed integer between -42dBm and -92dBm + * If signal is lost, the value is -128 + */ + int8_t dbm = (int8_t)dsm_frame[0]; + + if (dbm == -128) { + *rssi_percent = 0; + + } else { + *rssi_percent = spektrum_rssi.dbm_to_percent(dbm); + } + + } else { + /* if we don't know the rssi, anything over 100 will invalidate it */ + *rssi_percent = 127; + } + } + + /* + * 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 + */ + + /* * Each channel is a 16-bit unsigned value containing either a 10- * or 11-bit channel value and a 4-bit channel number, shifted * either 10 or 11 bits. The MSB may also be set to indicate the @@ -547,10 +588,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data * @param[out] n_butes number of bytes read * @param[out] bytes pointer to the buffer of read bytes + * @param[out] rssi value in percent, if supported, or 127 * @return true=decoded raw channel values updated, false=no update */ 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) { int ret = 1; @@ -592,12 +635,12 @@ dsm_input(int fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint /* * Try to decode something with what we got */ - return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values); + return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, rssi, max_values); } bool dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t *values, - uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels) + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, int8_t *rssi_percent, uint16_t max_channels) { /* this is set by the decoding state machine and will default to false @@ -665,7 +708,7 @@ dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t * Great, it looks like we might have a frame. Go ahead and * decode it. */ - decode_ret = dsm_decode(now, &dsm_chan_buf[0], &dsm_chan_count, dsm_11_bit, max_channels); + decode_ret = dsm_decode(now, &dsm_chan_buf[0], &dsm_chan_count, dsm_11_bit, max_channels, rssi_percent); /* we consumed the partial frame, reset */ dsm_partial_frame_count = 0; diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 4cb5daa917..8656650a87 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -58,10 +58,10 @@ __EXPORT void dsm_deinit(void); __EXPORT void dsm_proto_init(void); __EXPORT int dsm_config(int dsm_fd); __EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, - uint8_t **bytes, unsigned max_values); + uint8_t **bytes, int8_t *rssi, unsigned max_values); __EXPORT bool dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t *values, - uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels); + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, int8_t *rssi_percent, uint16_t max_channels); #ifdef SPEKTRUM_POWER __EXPORT void dsm_bind(uint16_t cmd, int pulses); diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index cff76b42e6..b224fde681 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -100,7 +100,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned // Pipe the data into the parser bool result = dsm_parse(f * 1e6f, &frame[0], len, rc_values, &num_values, - &dsm_11_bit, &dsm_frame_drops, max_channels); + &dsm_11_bit, &dsm_frame_drops, nullptr, max_channels); if (result) { ut_compare("num_values == expected_chancount", num_values, expected_chancount); diff --git a/src/lib/rc/spektrum_rssi.h b/src/lib/rc/spektrum_rssi.h new file mode 100644 index 0000000000..ff0b748267 --- /dev/null +++ b/src/lib/rc/spektrum_rssi.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file spektrum_rssi.h + * + * RSSI dBm to percentage conversion for Spektrum telemetry receivers + * + * @author Kurt Kiefer + */ + +#pragma once + +#include + +#define MIN_RSSI_DBM (-92.0f) +#define MAX_RSSI_DBM (-42.0f) + +class SpektrumRssi +{ + int8_t lu_dbm_percent[129]; + +public: + SpektrumRssi() + { + for (int i = 0; i <= 128; i++) { + float rssi_dbm = -1.0f * (float)i; + float percent; + + if (rssi_dbm > MAX_RSSI_DBM) { + percent = 100.0f; + + } else if (rssi_dbm < MIN_RSSI_DBM) { + percent = 0.0f; + + } else { + percent = 100.0f * log10f(1 + (rssi_dbm - MIN_RSSI_DBM) * (9.0f / (MAX_RSSI_DBM - MIN_RSSI_DBM))); + } + + lu_dbm_percent[i] = (int8_t)roundf(percent); + } + } + + int8_t dbm_to_percent(int8_t dbm) const + { + return lu_dbm_percent[abs(dbm)]; + } +}; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index d126f6c28f..87d56a04c3 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -78,8 +78,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint8_t n_bytes = 0; uint8_t *bytes; bool dsm_11_bit; + int8_t spektrum_rssi; *dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, - PX4IO_RC_INPUT_CHANNELS); + &spektrum_rssi, PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { @@ -93,6 +94,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + *rssi = spektrum_rssi; } perf_end(c_gather_dsm);