RCTest: add tests for ghost protocol

Add tests for ghost protocol

src/lib/rc/rc_tests/RCTest.cpp

@@ -5,6 +5,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <unistd.h>
+#include <algorithm>
 
 #include <drivers/drv_hrt.h>
 
@@ -14,6 +15,7 @@
 #include <lib/rc/st24.h>
 #include <lib/rc/sumd.h>
 #include <lib/rc/crsf.h>
+#include <lib/rc/ghst.h>
 
 #if defined(CONFIG_ARCH_BOARD_PX4_SITL)
 #define TEST_DATA_PATH "./test_data/"
@@ -30,6 +32,7 @@ public:
 
 private:
 	bool crsfTest();
+	bool ghstTest();
 	bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0);
 	bool dsmTest10Ch();
 	bool dsmTest16Ch();
@@ -42,6 +45,7 @@ private:
 bool RCTest::run_tests()
 {
 	ut_run_test(crsfTest);
+	ut_run_test(ghstTest);
 	ut_run_test(dsmTest10Ch);
 	ut_run_test(dsmTest16Ch);
 	ut_run_test(dsmTest22msDSMX16Ch);
@@ -138,6 +142,94 @@ bool RCTest::crsfTest()
 	return true;
 }
 
+bool RCTest::ghstTest()
+{
+	const char *filepath = TEST_DATA_PATH "ghst_rc_channels.txt";
+
+	FILE *fp = fopen(filepath, "rt");
+
+	ut_test(fp);
+
+	const int line_size = 500;
+	char line[line_size];
+	bool has_decoded_values = false;
+	const int max_channels = 16;
+	uint16_t rc_values[max_channels];
+	uint16_t num_values = 0;
+	int line_counter = 1;
+	int8_t ghst_rssi = -1;
+
+	while (fgets(line, line_size, fp) != nullptr)  {
+
+		if (strncmp(line, "INPUT ", 6) == 0) {
+
+			if (has_decoded_values) {
+				PX4_ERR("Parser decoded values that are not in the test file (line=%i)", line_counter);
+				return false;
+			}
+
+			std::fill_n(rc_values, max_channels, UINT16_MAX);
+
+			// read the values
+			const char *file_buffer = line + 6;
+			int frame_len = 0;
+			uint8_t frame[300];
+			int offset;
+			int number;
+
+			while (sscanf(file_buffer, "%x, %n", &number, &offset) > 0) {
+				frame[frame_len++] = number;
+				file_buffer += offset;
+			}
+
+			// 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);
+
+			if (result) {
+				has_decoded_values = true;
+			}
+
+		} else if (strncmp(line, "DECODED ", 8) == 0) {
+
+			if (!has_decoded_values) {
+				PX4_ERR("Test file contains decoded values but the parser did not decode anything (line=%i)", line_counter);
+				return false;
+			}
+
+			// read the values
+			const char *file_buffer = line + 8;
+			int offset;
+			int expected_rc_value;
+			int expected_num_channels = 0;
+
+			while (sscanf(file_buffer, "%x, %n", &expected_rc_value, &offset) > 0) {
+
+				// allow a small difference
+				if (abs(expected_rc_value - (int)rc_values[expected_num_channels]) > 10) {
+					PX4_ERR("File line: %i, channel: %i", line_counter, expected_num_channels);
+					ut_compare("Wrong decoded channel", expected_rc_value, rc_values[expected_num_channels]);
+				}
+
+				file_buffer += offset;
+				++expected_num_channels;
+			}
+
+			if (expected_num_channels != num_values) {
+				PX4_ERR("File line: %d", line_counter);
+				ut_compare("Unexpected number of decoded channels", expected_num_channels, num_values);
+			}
+
+			has_decoded_values = false;
+		}
+
+		++line_counter;
+	}
+
+	return true;
+}
+
 bool RCTest::dsmTest10Ch()
 {
 	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);