From 3701a02a37ddd9e78fa2ddcb7b9e9ec0d136ae79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 Nov 2013 10:00:33 +0100 Subject: Tests for all PWM pins --- src/systemcmds/tests/module.mk | 5 +- src/systemcmds/tests/test_file.c | 335 ++++++++++++++++++++++++++++++ src/systemcmds/tests/test_param.c | 78 +++++++ src/systemcmds/tests/test_ppm_loopback.c | 154 ++++++++++++++ src/systemcmds/tests/test_sensors.c | 92 ++++++-- src/systemcmds/tests/test_servo.c | 66 +++--- src/systemcmds/tests/test_uart_loopback.c | 70 ++----- src/systemcmds/tests/tests.h | 9 +- src/systemcmds/tests/tests_file.c | 335 ------------------------------ src/systemcmds/tests/tests_main.c | 14 +- src/systemcmds/tests/tests_param.c | 78 ------- 11 files changed, 704 insertions(+), 532 deletions(-) create mode 100644 src/systemcmds/tests/test_file.c create mode 100644 src/systemcmds/tests/test_param.c create mode 100644 src/systemcmds/tests/test_ppm_loopback.c delete mode 100644 src/systemcmds/tests/tests_file.c delete mode 100644 src/systemcmds/tests/tests_param.c (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6729ce4ae..5d5fe50d3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,7 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ - tests_file.c \ + test_file.c \ tests_main.c \ - tests_param.c + test_param.c \ + test_ppm_loopback.c diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c new file mode 100644 index 000000000..798724cf1 --- /dev/null +++ b/src/systemcmds/tests/test_file.c @@ -0,0 +1,335 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_file.c + * + * File write test. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tests.h" + +int +test_file(int argc, char *argv[]) +{ + const unsigned iterations = 100; + const unsigned alignments = 65; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + for (unsigned a = 0; a < alignments; a++) { + + printf("\n"); + warnx("----- alignment test: %u bytes -----", a); + + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing unaligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + errx(1, "memory is unaligned, align shift: %d", a); + + } + + fsync(fd); + //perf_end(wperf); + + } + end = hrt_absolute_time(); + + //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + + //perf_print_counter(wperf); + //perf_free(wperf); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + errx(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + /* + * ALIGNED WRITES AND UNALIGNED READS + */ + + close(fd); + int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing aligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + err(1, "WRITE ERROR!"); + } + + } + + fsync(fd); + + warnx("reading data aligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool align_read_ok = true; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } + + if (!align_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + warnx("reading data unaligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool unalign_read_ok = true; + int unalign_read_err_count = 0; + + memset(read_buf, 0, sizeof(read_buf)); + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf + a, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + for (int j = 0; j < chunk_sizes[c]; j++) { + + if ((read_buf + a)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + + if (!unalign_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) + err(1, "UNLINKING FILE FAILED"); + + } + } + + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); + } + + return 0; +} +#if 0 +int +test_file(int argc, char *argv[]) +{ + const iterations = 1024; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + uint8_t buf[512]; + hrt_abstime start, end; + perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + memset(buf, 0, sizeof(buf)); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + perf_begin(wperf); + write(fd, buf, sizeof(buf)); + perf_end(wperf); + } + end = hrt_absolute_time(); + + close(fd); + + unlink("/fs/microsd/testfile"); + + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + perf_print_counter(wperf); + perf_free(wperf); + + warnx("running unlink test"); + + /* ensure that common commands do not run against file count limits */ + for (unsigned i = 0; i < 64; i++) { + + warnx("unlink iteration #%u", i); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file before unlink()"); + int ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file before unlink()"); + close(fd); + + ret = unlink("/fs/microsd/testfile"); + if (ret != OK) + errx(1, "failed unlinking test file"); + + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file after unlink()"); + ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file after unlink()"); + close(fd); + } + + return 0; +} +#endif diff --git a/src/systemcmds/tests/test_param.c b/src/systemcmds/tests/test_param.c new file mode 100644 index 000000000..318d2505b --- /dev/null +++ b/src/systemcmds/tests/test_param.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_param.c + * + * Tests related to the parameter system. + */ + +#include +#include "systemlib/err.h" + +#include "systemlib/param/param.h" +#include "tests.h" + +PARAM_DEFINE_INT32(test, 0x12345678); + +int +test_param(int argc, char *argv[]) +{ + param_t p; + + p = param_find("test"); + if (p == PARAM_INVALID) + errx(1, "test parameter not found"); + + param_type_t t = param_type(p); + if (t != PARAM_TYPE_INT32) + errx(1, "test parameter type mismatch (got %u)", (unsigned)t); + + int32_t val; + if (param_get(p, &val) != 0) + errx(1, "failed to read test parameter"); + if (val != 0x12345678) + errx(1, "parameter value mismatch"); + + val = 0xa5a5a5a5; + if (param_set(p, &val) != 0) + errx(1, "failed to write test parameter"); + if (param_get(p, &val) != 0) + errx(1, "failed to re-read test parameter"); + if ((uint32_t)val != 0xa5a5a5a5) + errx(1, "parameter value mismatch after write"); + + warnx("parameter test PASS"); + + return 0; +} diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c new file mode 100644 index 000000000..6d4e45c4c --- /dev/null +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_ppm_loopback.c + * Tests the PWM outputs and PPM input + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "tests.h" + +#include +#include + +int test_ppm_loopback(int argc, char *argv[]) +{ + + int servo_fd, result; + servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; + servo_position_t pos; + + servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); + + if (servo_fd < 0) { + printf("failed opening /dev/pwm_servo\n"); + } + + result = read(servo_fd, &data, sizeof(data)); + + if (result != sizeof(data)) { + printf("failed bulk-reading channel values\n"); + } + + printf("Servo readback, pairs of values should match defaults\n"); + + unsigned servo_count; + result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { + warnx("PWM_SERVO_GET_COUNT"); + return ERROR; + } + + for (unsigned i = 0; i < servo_count; i++) { + result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos); + + if (result < 0) { + printf("failed reading channel %u\n", i); + } + + printf("%u: %u %u\n", i, pos, data[i]); + + } + + /* tell safety that its ok to disable it with the switch */ + result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + /* tell output device that the system is armed (it will output values if safety is off) */ + result = ioctl(servo_fd, PWM_SERVO_ARM, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_ARM"); + + int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; + + + printf("Advancing channel 0 to 1100\n"); + result = ioctl(servo_fd, PWM_SERVO_SET(0), 1100); + printf("Advancing channel 1 to 1900\n"); + result = ioctl(servo_fd, PWM_SERVO_SET(1), 1900); + printf("Advancing channel 2 to 1200\n"); + result = ioctl(servo_fd, PWM_SERVO_SET(2), 1200); + + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + if (result) { + (void)close(servo_fd); + return ERROR; + } + } + + /* give driver 10 ms to propagate */ + usleep(10000); + + /* open PPM input and expect values close to the output values */ + + int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); + + struct rc_input_values rc; + result = read(ppm_fd, &rc, sizeof(rc)); + + if (result != sizeof(rc)) { + warnx("Error reading RC output"); + (void)close(servo_fd); + (void)close(ppm_fd); + return ERROR; + } + + /* go and check values */ + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_GET(i), pwm_values[i]); + if (result) { + (void)close(servo_fd); + return ERROR; + } + } + + return 0; +} diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 14503276c..f6415b72f 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,6 +48,8 @@ #include #include #include +#include +#include #include @@ -77,6 +78,7 @@ static int accel(int argc, char *argv[]); static int gyro(int argc, char *argv[]); static int mag(int argc, char *argv[]); static int baro(int argc, char *argv[]); +static int mpu6k(int argc, char *argv[]); /**************************************************************************** * Private Data @@ -91,6 +93,7 @@ struct { {"gyro", "/dev/gyro", gyro}, {"mag", "/dev/mag", mag}, {"baro", "/dev/baro", baro}, + {"mpu6k", "/dev/mpu6k", mpu6k}, {NULL, NULL, NULL} }; @@ -133,23 +136,83 @@ accel(int argc, char *argv[]) printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } - // /* wait at least 10ms, sensor should have data after no more than 2ms */ - // usleep(100000); + if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { + warnx("MPU6K acceleration values out of range!"); + return ERROR; + } + + /* Let user know everything is ok */ + printf("\tOK: ACCEL passed all tests successfully\n"); + close(fd); + + return OK; +} + +static int +mpu6k(int argc, char *argv[]) +{ + printf("\tMPU6K: test start\n"); + fflush(stdout); + + int fd; + struct accel_report buf; + struct gyro_report gyro_buf; + int ret; + + fd = open("/dev/accel_mpu6k", O_RDONLY); + + if (fd < 0) { + printf("\tMPU6K: open fail, run first.\n"); + return ERROR; + } + + /* wait at least 100ms, sensor should have data after no more than 20ms */ + usleep(100000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); - // ret = read(fd, buf, sizeof(buf)); + if (ret != sizeof(buf)) { + printf("\tMPU6K: read1 fail (%d)\n", ret); + return ERROR; - // if (ret != sizeof(buf)) { - // printf("\tMPU-6000: read2 fail (%d)\n", ret); - // return ERROR; + } else { + printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); + } - // } else { - // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); - // } + if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { + warnx("MPU6K acceleration values out of range!"); + return ERROR; + } + + /* Let user know everything is ok */ + printf("\tOK: MPU6K ACCEL passed all tests successfully\n"); - /* XXX more tests here */ + close(fd); + fd = open("/dev/gyro_mpu6k", O_RDONLY); + + if (fd < 0) { + printf("\tMPU6K GYRO: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &gyro_buf, sizeof(gyro_buf)); + + if (ret != sizeof(gyro_buf)) { + printf("\tMPU6K GYRO: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z); + } /* Let user know everything is ok */ - printf("\tOK: ACCEL passed all tests successfully\n"); + printf("\tOK: MPU6K GYRO passed all tests successfully\n"); + close(fd); return OK; } @@ -187,6 +250,7 @@ gyro(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); + close(fd); return OK; } @@ -224,6 +288,7 @@ mag(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); + close(fd); return OK; } @@ -261,6 +326,7 @@ baro(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: BARO passed all tests successfully\n"); + close(fd); return OK; } diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index f95760ca8..ef8512bf5 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -1,7 +1,6 @@ /**************************************************************************** - * px4/sensors/test_hrt.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -13,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -32,9 +31,11 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_servo.c + * Tests the servo outputs + * + */ #include @@ -55,39 +56,6 @@ #include "tests.h" -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_servo - ****************************************************************************/ - int test_servo(int argc, char *argv[]) { int fd, result; @@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[]) printf("Servo readback, pairs of values should match defaults\n"); - for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { + unsigned servo_count; + result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { + warnx("PWM_SERVO_GET_COUNT"); + return ERROR; + } + + for (unsigned i = 0; i < servo_count; i++) { result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos); if (result < 0) { @@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[]) } - printf("Servos arming at default values\n"); + /* tell safety that its ok to disable it with the switch */ + result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + /* tell output device that the system is armed (it will output values if safety is off) */ result = ioctl(fd, PWM_SERVO_ARM, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_ARM"); + usleep(5000000); printf("Advancing channel 0 to 1500\n"); result = ioctl(fd, PWM_SERVO_SET(0), 1500); + printf("Advancing channel 1 to 1800\n"); + result = ioctl(fd, PWM_SERVO_SET(1), 1800); out: return 0; } diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index 3be152004..f1d41739b 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -1,8 +1,6 @@ /**************************************************************************** - * px4/sensors/test_gpio.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Lorenz Meier + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -14,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -33,9 +31,11 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_uart_loopback.c + * Tests the uart outputs + * + */ #include @@ -55,40 +55,6 @@ #include #include - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_led - ****************************************************************************/ - int test_uart_loopback(int argc, char *argv[]) { @@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[]) int uart5_nwrite = 0; int uart2_nwrite = 0; - int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); // + /* opening stdout */ + int stdout_fd = 1; - /* assuming NuttShell is on UART1 (/dev/ttyS0) */ - int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); // - int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); // + int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); + int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); if (uart2 < 0) { printf("ERROR opening UART2, aborting..\n"); @@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[]) exit(uart5); } - uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'}; + uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'}; uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; @@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[]) for (i = 0; i < 1000; i++) { // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* uart2 -> uart5 */ r = write(uart2, sample_uart2, sizeof(sample_uart2)); @@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[]) uart2_nwrite += r; // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* uart2 -> uart5 */ r = write(uart5, sample_uart5, sizeof(sample_uart5)); @@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[]) uart5_nwrite += r; // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* try to read back values */ do { @@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[]) } while (r > 0); // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); do { r = read(uart2, sample_uart5, sizeof(sample_uart5)); @@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[]) } while (r > 0); // printf("TEST #%d\n",i); -// write(uart1, sample_uart1, sizeof(sample_uart5)); +// write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5)); } for (i = 0; i < 200000; i++) { @@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[]) } - close(uart1); + close(stdout_fd); close(uart2); close(uart5); diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c483108cf..5cbc5ad88 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,12 @@ #ifndef __APPS_PX4_TESTS_H #define __APPS_PX4_TESTS_H +/** + * @file tests.h + * Tests declaration file. + * + */ + /**************************************************************************** * Included Files ****************************************************************************/ @@ -88,6 +94,7 @@ extern int test_int(int argc, char *argv[]); extern int test_float(int argc, char *argv[]); extern int test_ppm(int argc, char *argv[]); extern int test_servo(int argc, char *argv[]); +extern int test_ppm_loopback(int argc, char *argv[]); extern int test_uart_loopback(int argc, char *argv[]); extern int test_uart_baudchange(int argc, char *argv[]); extern int test_cpuload(int argc, char *argv[]); diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c deleted file mode 100644 index 7f3a654bf..000000000 --- a/src/systemcmds/tests/tests_file.c +++ /dev/null @@ -1,335 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file tests_file.c - * - * File write test. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "tests.h" - -int -test_file(int argc, char *argv[]) -{ - const unsigned iterations = 100; - const unsigned alignments = 65; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); - return 1; - } - - /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; - - for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - - printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); - - for (unsigned a = 0; a < alignments; a++) { - - printf("\n"); - warnx("----- alignment test: %u bytes -----", a); - - uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - - /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { - /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; - } - - uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - hrt_abstime start, end; - //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - - warnx("testing unaligned writes - please wait.."); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - //perf_begin(wperf); - int wret = write(fd, write_buf + a, chunk_sizes[c]); - - if (wret != chunk_sizes[c]) { - warn("WRITE ERROR!"); - - if ((0x3 & (uintptr_t)(write_buf + a))) - errx(1, "memory is unaligned, align shift: %d", a); - - } - - fsync(fd); - //perf_end(wperf); - - } - end = hrt_absolute_time(); - - //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - //perf_print_counter(wperf); - //perf_free(wperf); - - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); - - /* read back data for validation */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, chunk_sizes[c]); - - if (rret != chunk_sizes[c]) { - errx(1, "READ ERROR!"); - } - - /* compare value */ - bool compare_ok = true; - - for (int j = 0; j < chunk_sizes[c]; j++) { - if (read_buf[j] != write_buf[j + a]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); - compare_ok = false; - break; - } - } - - if (!compare_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); - } - - } - - /* - * ALIGNED WRITES AND UNALIGNED READS - */ - - close(fd); - int ret = unlink("/fs/microsd/testfile"); - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - - warnx("testing aligned writes - please wait.."); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - int wret = write(fd, write_buf, chunk_sizes[c]); - - if (wret != chunk_sizes[c]) { - err(1, "WRITE ERROR!"); - } - - } - - fsync(fd); - - warnx("reading data aligned.."); - - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); - - bool align_read_ok = true; - - /* read back data unaligned */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, chunk_sizes[c]); - - if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); - } - - /* compare value */ - bool compare_ok = true; - - for (int j = 0; j < chunk_sizes[c]; j++) { - if (read_buf[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); - align_read_ok = false; - break; - } - } - - if (!align_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); - } - - } - - warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); - - warnx("reading data unaligned.."); - - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); - - bool unalign_read_ok = true; - int unalign_read_err_count = 0; - - memset(read_buf, 0, sizeof(read_buf)); - - /* read back data unaligned */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf + a, chunk_sizes[c]); - - if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); - } - - for (int j = 0; j < chunk_sizes[c]; j++) { - - if ((read_buf + a)[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); - unalign_read_ok = false; - unalign_read_err_count++; - - if (unalign_read_err_count > 10) - break; - } - } - - if (!unalign_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); - } - - } - - ret = unlink("/fs/microsd/testfile"); - close(fd); - - if (ret) - err(1, "UNLINKING FILE FAILED"); - - } - } - - /* list directory */ - DIR *d; - struct dirent *dir; - d = opendir("/fs/microsd"); - if (d) { - - while ((dir = readdir(d)) != NULL) { - //printf("%s\n", dir->d_name); - } - - closedir(d); - - warnx("directory listing ok (FS mounted and readable)"); - - } else { - /* failed opening dir */ - err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); - } - - return 0; -} -#if 0 -int -test_file(int argc, char *argv[]) -{ - const iterations = 1024; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); - return 1; - } - - uint8_t buf[512]; - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, buf, sizeof(buf)); - perf_end(wperf); - } - end = hrt_absolute_time(); - - close(fd); - - unlink("/fs/microsd/testfile"); - - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - perf_print_counter(wperf); - perf_free(wperf); - - warnx("running unlink test"); - - /* ensure that common commands do not run against file count limits */ - for (unsigned i = 0; i < 64; i++) { - - warnx("unlink iteration #%u", i); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file before unlink()"); - int ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file before unlink()"); - close(fd); - - ret = unlink("/fs/microsd/testfile"); - if (ret != OK) - errx(1, "failed unlinking test file"); - - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file after unlink()"); - ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file after unlink()"); - close(fd); - } - - return 0; -} -#endif diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9eb9c632e..cd998dd18 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file tests_main.c * Tests main file, loads individual tests. + * + * @author Lorenz Meier */ #include @@ -57,14 +58,6 @@ #include "tests.h" -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - /**************************************************************************** * Private Function Prototypes ****************************************************************************/ @@ -94,6 +87,7 @@ const struct { {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST}, {"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST}, {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST}, {"adc", test_adc, OPT_NOJIGTEST}, {"jig_voltages", test_jig_voltages, OPT_NOALLTEST}, {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/tests_param.c deleted file mode 100644 index 13f17bc43..000000000 --- a/src/systemcmds/tests/tests_param.c +++ /dev/null @@ -1,78 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file tests_param.c - * - * Tests related to the parameter system. - */ - -#include -#include "systemlib/err.h" - -#include "systemlib/param/param.h" -#include "tests.h" - -PARAM_DEFINE_INT32(test, 0x12345678); - -int -test_param(int argc, char *argv[]) -{ - param_t p; - - p = param_find("test"); - if (p == PARAM_INVALID) - errx(1, "test parameter not found"); - - param_type_t t = param_type(p); - if (t != PARAM_TYPE_INT32) - errx(1, "test parameter type mismatch (got %u)", (unsigned)t); - - int32_t val; - if (param_get(p, &val) != 0) - errx(1, "failed to read test parameter"); - if (val != 0x12345678) - errx(1, "parameter value mismatch"); - - val = 0xa5a5a5a5; - if (param_set(p, &val) != 0) - errx(1, "failed to write test parameter"); - if (param_get(p, &val) != 0) - errx(1, "failed to re-read test parameter"); - if ((uint32_t)val != 0xa5a5a5a5) - errx(1, "parameter value mismatch after write"); - - warnx("parameter test PASS"); - - return 0; -} -- cgit v1.2.3 From 264ef47197432d2cc1372cabf93c3bd7a52df0aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Dec 2013 05:02:00 +0100 Subject: PPM loopback test --- src/systemcmds/tests/test_ppm_loopback.c | 108 +++++++++++++++++++------------ 1 file changed, 66 insertions(+), 42 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index 6d4e45c4c..b9041b013 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -51,6 +51,7 @@ #include #include #include +#include #include #include "tests.h" @@ -61,6 +62,8 @@ int test_ppm_loopback(int argc, char *argv[]) { + int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + int servo_fd, result; servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; @@ -71,12 +74,6 @@ int test_ppm_loopback(int argc, char *argv[]) printf("failed opening /dev/pwm_servo\n"); } - result = read(servo_fd, &data, sizeof(data)); - - if (result != sizeof(data)) { - printf("failed bulk-reading channel values\n"); - } - printf("Servo readback, pairs of values should match defaults\n"); unsigned servo_count; @@ -93,62 +90,89 @@ int test_ppm_loopback(int argc, char *argv[]) printf("failed reading channel %u\n", i); } - printf("%u: %u %u\n", i, pos, data[i]); + //printf("%u: %u %u\n", i, pos, data[i]); } - /* tell safety that its ok to disable it with the switch */ - result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); - if (result != OK) - warnx("FAIL: PWM_SERVO_SET_ARM_OK"); - /* tell output device that the system is armed (it will output values if safety is off) */ - result = ioctl(servo_fd, PWM_SERVO_ARM, 0); - if (result != OK) - warnx("FAIL: PWM_SERVO_ARM"); + // /* tell safety that its ok to disable it with the switch */ + // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); + // if (result != OK) + // warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + // tell output device that the system is armed (it will output values if safety is off) + // result = ioctl(servo_fd, PWM_SERVO_ARM, 0); + // if (result != OK) + // warnx("FAIL: PWM_SERVO_ARM"); int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; - printf("Advancing channel 0 to 1100\n"); - result = ioctl(servo_fd, PWM_SERVO_SET(0), 1100); - printf("Advancing channel 1 to 1900\n"); - result = ioctl(servo_fd, PWM_SERVO_SET(1), 1900); - printf("Advancing channel 2 to 1200\n"); - result = ioctl(servo_fd, PWM_SERVO_SET(2), 1200); + // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + + // if (result) { + // (void)close(servo_fd); + // return ERROR; + // } else { + // warnx("channel %d set to %d", i, pwm_values[i]); + // } + // } + + warnx("servo count: %d", servo_count); + + struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0}; for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); - if (result) { - (void)close(servo_fd); - return ERROR; - } + pwm_out.values[i] = pwm_values[i]; + //warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]); + pwm_out.channel_count++; } + result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out); + /* give driver 10 ms to propagate */ - usleep(10000); + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + usleep(100000); /* open PPM input and expect values close to the output values */ - int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - struct rc_input_values rc; - result = read(ppm_fd, &rc, sizeof(rc)); + if (rc_updated) { - if (result != sizeof(rc)) { - warnx("Error reading RC output"); - (void)close(servo_fd); - (void)close(ppm_fd); - return ERROR; - } + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - /* go and check values */ - for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - result = ioctl(servo_fd, PWM_SERVO_GET(i), pwm_values[i]); - if (result) { - (void)close(servo_fd); - return ERROR; + // int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); + + + + // struct rc_input_values rc; + // result = read(ppm_fd, &rc, sizeof(rc)); + + // if (result != sizeof(rc)) { + // warnx("Error reading RC output"); + // (void)close(servo_fd); + // (void)close(ppm_fd); + // return ERROR; + // } + + /* go and check values */ + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) { + warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]); + (void)close(servo_fd); + return ERROR; + } } + } else { + warnx("failed reading RC input data"); + return ERROR; } + warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!"); + return 0; } -- cgit v1.2.3