ghst: keep the previous values for channels that are not updated

Keep the previous values for channels that are not updated (ghost protocol). Add new test data for ghost protocol.
latteishorse · Feb 20, 2021 · 9d65e9a · 9d65e9a
1 parent 96c72a5
commit 9d65e9a
Show file tree

Hide file tree

Showing 3 changed files with 3,370 additions and 5,612 deletions.
diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp
@@ -70,6 +70,7 @@
 #define GHST_FRAME_CRC_SIZE			(1)
 #define GHST_FRAME_TYPE_SIZE			(1)
 #define GHST_TYPE_DATA_CRC_SIZE			(12u)
+#define GHST_MAX_NUM_CHANNELS			(16)
 
 enum class ghst_parser_state_t : uint8_t {
 	unsynced = 0,
@@ -83,6 +84,8 @@ static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame;
 static uint32_t current_frame_position = 0;
 static ghst_parser_state_t parser_state = ghst_parser_state_t::unsynced;
 
+static uint16_t prev_rc_vals[GHST_MAX_NUM_CHANNELS];
+
 /**
  * parse the current ghst_frame buffer
  */
@@ -97,6 +100,7 @@ int ghst_config(int uart_fd)
 	tcgetattr(uart_fd, &t);
 	cfsetspeed(&t, GHST_BAUDRATE);
 	t.c_cflag &= ~(CSTOPB | PARENB);
+	memset(prev_rc_vals, (int)UINT16_MAX, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS);
 	ret_val = tcsetattr(uart_fd, TCSANOW, &t);
 	return ret_val;
 }
@@ -115,6 +119,8 @@ bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t
 	bool success = false;
 	uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame;
 
+	memcpy(values, prev_rc_vals, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS);
+
 	while (len > 0) {
 
 		// fill in the ghst_buffer, as much as we can
@@ -247,7 +253,7 @@ static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_valu
 
 		if (crc == ghst_frame_CRC(ghst_frame)) {
 			const ghstPayloadData_t *const rcChannels = (ghstPayloadData_t *)&ghst_frame.payload;
-			*num_values = MIN(max_channels, 16);
+			*num_values = MIN(max_channels, GHST_MAX_NUM_CHANNELS);
 
 			// all frames contain data from chan1to4
 			if (max_channels > 0) { values[0] = convert_channel_value(rcChannels->chan1to4.chan1 >> 1); }
@@ -297,6 +303,8 @@ static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_valu
 
 			*rssi = ghst_rssi;
 
+			memcpy(prev_rc_vals, values, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS);
+
 			GHST_VERBOSE("Got Channels");
 
 			ret = true;

diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp
@@ -5,7 +5,6 @@
 #include <stdio.h>
 #include <string.h>
 #include <unistd.h>
-#include <algorithm>
 
 #include <drivers/drv_hrt.h>
 
@@ -150,6 +149,7 @@ bool RCTest::ghstTest()
 
 	ut_test(fp);
 
+	int uart_fd = -1;
 	const int line_size = 500;
 	char line[line_size];
 	bool has_decoded_values = false;
@@ -158,6 +158,7 @@ bool RCTest::ghstTest()
 	uint16_t num_values = 0;
 	int line_counter = 1;
 	int8_t ghst_rssi = -1;
+	ghst_config(uart_fd);
 
 	while (fgets(line, line_size, fp) != nullptr)  {
 
@@ -168,8 +169,6 @@ bool RCTest::ghstTest()
 				return false;
 			}
 
-			std::fill_n(rc_values, max_channels, UINT16_MAX);
-
 			// read the values
 			const char *file_buffer = line + 6;
 			int frame_len = 0;