diff options
-rw-r--r-- | Tools/tests-host/st24_test.cpp | 24 |
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) { |