diff --git a/src/drivers/rc/ghst_rc/GhstRc.cpp b/src/drivers/rc/ghst_rc/GhstRc.cpp index 1960c84ce6..1aa3c23e89 100644 --- a/src/drivers/rc/ghst_rc/GhstRc.cpp +++ b/src/drivers/rc/ghst_rc/GhstRc.cpp @@ -34,6 +34,7 @@ #include "GhstRc.hpp" #include +#include GhstRc::GhstRc(const char *device) : ModuleParams(nullptr), @@ -174,16 +175,18 @@ void GhstRc::Run() if (newBytes > 0) { uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; uint16_t raw_rc_count = 0; - int8_t ghst_rssi = -1; + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; - if (ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, + if (ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &link_stats, &raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS) ) { // we have a new GHST frame. Publish it. input_rc_s input_rc{}; input_rc.timestamp_last_signal = cycle_timestamp; input_rc.channel_count = math::constrain(raw_rc_count, (uint16_t)0, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); - input_rc.rssi = ghst_rssi; + input_rc.rssi = link_stats.rssi_pct; + input_rc.link_quality = link_stats.link_quality; + input_rc.rssi_dbm = link_stats.rssi_dbm; input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; unsigned valid_chans = 0; @@ -200,13 +203,11 @@ void GhstRc::Run() if (valid_chans == 0) { input_rc.rssi = 0; + // can't force link quality to zero here, receiver takes care of this } input_rc.rc_lost = (valid_chans == 0); - input_rc.link_quality = -1; - input_rc.rssi_dbm = NAN; - input_rc.timestamp = hrt_absolute_time(); _input_rc_pub.publish(input_rc); perf_count(_publish_interval_perf); diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 12cd1ab0bf..538cb9d8ed 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -762,14 +762,15 @@ void RCInput::Run() // parse new data if (newBytes > 0) { - int8_t ghst_rssi = -1; - rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi, + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; + + rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &link_stats, &_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new GHST frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; - int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, link_stats.rssi_pct); // ghst telemetry works on fmu-v5 // on other Pixhawk (-related) boards we cannot write to the RC UART diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp index a0bce68718..306004059c 100644 --- a/src/lib/rc/ghst.cpp +++ b/src/lib/rc/ghst.cpp @@ -56,6 +56,7 @@ #include #include #include +#include // TODO: include RSSI dBm to percentage conversion for ghost receiver #include "spektrum_rssi.h" @@ -77,8 +78,8 @@ enum class ghst_parser_state_t : uint8_t { synced }; -// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI -static int8_t ghst_rssi = -1; +// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI/LQ +static ghstLinkStatistics_t last_link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame; static uint32_t current_frame_position = 0U; @@ -89,7 +90,8 @@ static uint16_t prev_rc_vals[GHST_MAX_NUM_CHANNELS]; /** * parse the current ghst_frame buffer */ -static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels); +static bool ghst_parse_buffer(uint16_t *values, ghstLinkStatistics_t *link_stats, uint16_t *num_values, + uint16_t max_channels); int ghst_config(int uart_fd) { @@ -114,7 +116,7 @@ static uint16_t convert_channel_value(unsigned chan_value); bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, - int8_t *rssi, uint16_t *num_values, uint16_t max_channels) + ghstLinkStatistics_t *link_stats, uint16_t *num_values, uint16_t max_channels) { bool success = false; uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; @@ -145,7 +147,7 @@ bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t len -= current_len; frame += current_len; - if (ghst_parse_buffer(values, rssi, num_values, max_channels)) { + if (ghst_parse_buffer(values, link_stats, num_values, max_channels)) { success = true; } } @@ -182,7 +184,8 @@ static uint16_t convert_channel_value(unsigned int chan_value) return converted_chan_value; } -static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels) +static bool ghst_parse_buffer(uint16_t *values, ghstLinkStatistics_t *link_stats, uint16_t *num_values, + uint16_t max_channels) { uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; @@ -299,13 +302,16 @@ static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_valu } else if (ghst_frame.type == static_cast(ghstFrameType::frameTypeRssi)) { const ghstPayloadRssi_t *const rssiValues = (ghstPayloadRssi_t *)&ghst_frame.payload; // TODO: call function for RSSI dBm to percentage conversion for ghost receiver - ghst_rssi = spek_dbm_to_percent(static_cast(rssiValues->rssidBm)); + last_link_stats.rssi_pct = spek_dbm_to_percent(static_cast + (rssiValues->rssidBm)); // rssidBm sign inverted (90 = -90dBm) + last_link_stats.rssi_dbm = -rssiValues->rssidBm; + last_link_stats.link_quality = rssiValues->lq; // 0 - 100 } else { GHST_DEBUG("Frame type: %u", ghst_frame.type); } - *rssi = ghst_rssi; + *link_stats = last_link_stats; memcpy(prev_rc_vals, values, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS); diff --git a/src/lib/rc/ghst.hpp b/src/lib/rc/ghst.hpp index 6f5a4f4664..c8fbc407c5 100644 --- a/src/lib/rc/ghst.hpp +++ b/src/lib/rc/ghst.hpp @@ -106,6 +106,13 @@ typedef struct { int txPowerdBm: 8; // Tx power [dBm] } __attribute__((__packed__)) ghstPayloadRssi_t; +// Link statistics for internal transport +typedef struct { + int8_t rssi_pct; + float rssi_dbm; + int8_t link_quality; +} ghstLinkStatistics_t; + /** * Configure an UART port to be used for GHST * @param uart_fd UART file descriptor @@ -127,7 +134,7 @@ __EXPORT int ghst_config(int uart_fd); * @return true if channels successfully decoded */ __EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, - int8_t *rssi, uint16_t *num_values, uint16_t max_channels); + ghstLinkStatistics_t *link_stats, uint16_t *num_values, uint16_t max_channels); /** diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 1997c19e85..4612673b92 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -159,7 +159,7 @@ bool RCTest::ghstTest() uint16_t rc_values[max_channels]; uint16_t num_values = 0; int line_counter = 1; - int8_t ghst_rssi = -1; + ghstLinkStatistics_t link_stats; ghst_config(uart_fd); while (fgets(line, line_size, fp) != nullptr) { @@ -186,7 +186,7 @@ bool RCTest::ghstTest() // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - bool result = ghst_parse(now, frame, frame_len, rc_values, &ghst_rssi, &num_values, max_channels); + bool result = ghst_parse(now, frame, frame_len, rc_values, &link_stats, &num_values, max_channels); if (result) { has_decoded_values = true;