fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add RSSI in dBm support, plus LQ, to GHST protocol (#24351)

Browse Source
This commit is contained in:
Tony Cake 2025-02-28 10:53:32 +01:00 committed by GitHub
parent 93b8bc1515
commit ba31054992
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 35 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/drivers/rc/ghst_rc/GhstRc.cpp
View File

@ -34,6 +34,7 @@
#include "GhstRc.hpp"
#include <termios.h>
#include <math.h>
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);

7
src/drivers/rc_input/RCInput.cpp
View File

@ -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

22
src/lib/rc/ghst.cpp
View File

@ -56,6 +56,7 @@
#include <termios.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
// 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<uint8_t>(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<int8_t>(rssiValues->rssidBm));
last_link_stats.rssi_pct = spek_dbm_to_percent(static_cast<int8_t>
(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);

9
src/lib/rc/ghst.hpp
View File

@ -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);
/**

4
src/lib/rc/rc_tests/RCTest.cpp
View File

@ -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;