Fixed ST24 test

1 files changed, 19 insertions, 5 deletions

diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp
index 680791ca8..e1134bba6 100644
--- a/Tools/tests-host/st24_test.cpp
+++ b/Tools/tests-host/st24_test.cpp
@@ -4,7 +4,7 @@
 #include <string.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
-#include <px4iofirmware/px4io.h>
+#include <rc/st24.h>
 #include "../../src/systemcmds/tests/tests.h"
 
 int main(int argc, char *argv[]) {
@@ -28,7 +28,8 @@ int main(int argc, char *argv[]) {
 
 	// Trash the first 20 lines
 	for (unsigned i = 0; i < 20; i++) {
-		(void)fscanf(fp, "%f,%x,,", &f, &x);
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
 	}
 
 	float last_time = 0;
@@ -38,15 +39,28 @@ int main(int argc, char *argv[]) {
 			warnx("FRAME RESET\n\n");
 		}
 
-		warnx("%f: 0x%02x", (double)f, x);
+		uint8_t b = static_cast<uint8_t>(x);
 
 		last_time = f;
 
 		// Pipe the data into the parser
 		hrt_abstime now = hrt_absolute_time();
 
-		//if (partial_frame_count % 25 == 0)
-			//sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels);
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		int16_t channels[20];
+
+
+		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0])))
+		{
+			//warnx("decoded: %u channels", (unsigned)channel_count);
+			for (unsigned i = 0; i < channel_count; i++) {
+				warnx("channel %u: %d", i, static_cast<int>(channels[i]));
+			}
+		}
+
+		//warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast<unsigned>(rssi), static_cast<unsigned>(rx_count));
 	}
 
 	if (ret == EOF) {