diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 0efa51b82d..feae7820fc 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -14,6 +15,7 @@ #include #include #include +#include #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);