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

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;