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

Author: tonycake

src/drivers/rc/ghst_rc/GhstRc.cpp

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

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

src/lib/rc/ghst.cpp

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

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

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;