diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-23 07:20:54 -0800 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-28 19:19:50 +0200 |
commit | c179863b1e35628a75efef624d2df0b0e85ad923 (patch) | |
tree | b4fcf77a10faf49d3c4614e6d662f493faeb1ef6 /Tools/tests-host/sbus2_test.cpp | |
parent | e134537ae8f892ba3ae4a2f9c771bcfa62f905c8 (diff) | |
download | px4-firmware-c179863b1e35628a75efef624d2df0b0e85ad923.tar.gz px4-firmware-c179863b1e35628a75efef624d2df0b0e85ad923.tar.bz2 px4-firmware-c179863b1e35628a75efef624d2df0b0e85ad923.zip |
Improve testing infrastructure for mixer and SBUS2
Diffstat (limited to 'Tools/tests-host/sbus2_test.cpp')
-rw-r--r-- | Tools/tests-host/sbus2_test.cpp | 75 |
1 files changed, 75 insertions, 0 deletions
diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp new file mode 100644 index 000000000..e3b012226 --- /dev/null +++ b/Tools/tests-host/sbus2_test.cpp @@ -0,0 +1,75 @@ + +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <systemlib/mixer/mixer.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <px4iofirmware/px4io.h> +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("SBUS2 test started"); + + if (argc < 2) + errx(1, "Need a filename for the input file"); + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1],"rt"); + + if (!fp) + errx(1, "failed opening file"); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + (void)fscanf(fp, "%f,%x,,", &f, &x); + } + + // Init the parser + uint8_t frame[30]; + unsigned partial_frame_count = 0; + uint16_t rc_values[18]; + uint16_t num_values; + bool sbus_failsafe; + bool sbus_frame_drop; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + partial_frame_count = 0; + warnx("FRAME RESET\n\n"); + } + + frame[partial_frame_count] = x; + partial_frame_count++; + + //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); + + if (partial_frame_count == sizeof(frame)) + partial_frame_count = 0; + + 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); + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + +}
\ No newline at end of file |