diff options
Diffstat (limited to 'apps/px4/tests/test_sensors.c')
| -rw-r--r-- | apps/px4/tests/test_sensors.c | 501 |
1 files changed, 501 insertions, 0 deletions
diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c new file mode 100644 index 000000000..bdb68f88b --- /dev/null +++ b/apps/px4/tests/test_sensors.c @@ -0,0 +1,501 @@ +/**************************************************************************** + * px4/sensors/test_sensors.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +#include <arch/board/drv_lis331.h> +#include <arch/board/drv_bma180.h> +#include <arch/board/drv_l3gd20.h> +#include <arch/board/drv_hmc5883l.h> +#include <arch/board/drv_mpu6000.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +//static int lis331(int argc, char *argv[]); +static int l3gd20(int argc, char *argv[]); +static int bma180(int argc, char *argv[]); +static int hmc5883l(int argc, char *argv[]); +static int ms5611(int argc, char *argv[]); +static int mpu6000(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + const char *path; + int (* test)(int argc, char *argv[]); +} sensors[] = { + {"l3gd20", "/dev/l3gd20", l3gd20}, + {"bma180", "/dev/bma180", bma180}, + {"hmc5883l", "/dev/hmc5883l", hmc5883l}, + {"ms5611", "/dev/ms5611", ms5611}, + {"mpu6000", "/dev/mpu6000", mpu6000}, +// {"lis331", "/dev/lis331", lis331}, + {NULL, NULL, NULL} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +//static int +//lis331(int argc, char *argv[]) +//{ +// int fd; +// int16_t buf[3]; +// int ret; +// +// fd = open("/dev/lis331", O_RDONLY); +// if (fd < 0) { +// printf("\tlis331: not present on PX4FMU v1.5 and later\n"); +// return ERROR; +// } +// +// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || +// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { +// +// printf("LIS331: ioctl fail\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)); +// if (ret != sizeof(buf)) { +// printf("LIS331: read1 fail (%d)\n", ret); +// return ERROR; +// } +// +// /* read data - expect no samples (should not be ready again yet) */ +// ret = read(fd, buf, sizeof(buf)); +// if (ret != 0) { +// printf("LIS331: read2 fail (%d)\n", ret); +// return ERROR; +// } +// +// /* XXX more tests here */ +// +// return 0; +//} + +static int +l3gd20(int argc, char *argv[]) +{ + printf("\tL3GD20: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/l3gd20", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("L3GD20: open fail\n"); + return ERROR; + } + +// if (ioctl(fd, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_50HZ) || +// ioctl(fd, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { +// +// printf("L3GD20: ioctl fail\n"); +// return ERROR; +// } else { +// printf("\tconfigured..\n"); +// } +// +// /* wait at least 100ms, sensor should have data after no more than 2ms */ +// usleep(100000); + + + + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tL3GD20: read1 fail (%d should have been %d)\n", ret, sizeof(buf)); + //return ERROR; + + } else { + printf("\tL3GD20 values #1: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* wait at least 2 ms, sensor should have data after no more than 1.5ms */ + usleep(2000); + + /* read data - expect no samples (should not be ready again yet) */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tL3GD20: read2 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tL3GD20 values #2: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* empty sensor buffer */ + ret = 0; + + while (ret != sizeof(buf)) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + } + + /* test if FIFO is operational */ + usleep(14800); // Expecting 10 measurements + + ret = 0; + int count = 0; + bool dataready = true; + + while (dataready) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + dataready = false; + + } else { + count++; + } + } + + printf("\tL3GD20: Drained FIFO with %d values (expected 8-12)\n", count); + + /* read data - expect no samples (should not be ready again yet) */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != 0) { + printf("\tL3GD20: Note: read3 got data - there should not have been data ready\n", ret); +// return ERROR; + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: L3GD20 passed all tests successfully\n"); + return OK; +} + +static int +bma180(int argc, char *argv[]) +{ + printf("\tBMA180: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/bma180", O_RDONLY); + + if (fd < 0) { + printf("\tBMA180: open fail\n"); + return ERROR; + } + +// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || +// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { +// +// printf("BMA180: ioctl fail\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)); + + if (ret != sizeof(buf)) { + printf("\tBMA180: read1 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* wait at least 10ms, sensor should have data after no more than 2ms */ + usleep(100000); + + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tBMA180: read2 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* empty sensor buffer */ + ret = 0; + + while (ret != sizeof(buf)) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + } + + ret = read(fd, buf, sizeof(buf)); + + if (ret != 0) { + printf("\tBMA180: Note: read3 got data - there should not have been data ready\n", ret); + } + + /* Let user know everything is ok */ + printf("\tOK: BMA180 passed all tests successfully\n"); + close(fd); + + return OK; +} + +static int +mpu6000(int argc, char *argv[]) +{ + printf("\tMPU-6000: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[5] = { -1, 0, -1, 0, -1, 0}; + int ret; + + fd = open("/dev/mpu6000", O_RDONLY); + + if (fd < 0) { + printf("\tMPU-6000: open fail\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)); + + // if (ret != sizeof(buf)) { + // printf("\tMPU-6000: read1 fail (%d)\n", ret); + // return ERROR; + + // } 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]); + // } + + // /* wait at least 10ms, sensor should have data after no more than 2ms */ + // usleep(100000); + + // ret = read(fd, buf, sizeof(buf)); + + // if (ret != sizeof(buf)) { + // printf("\tMPU-6000: read2 fail (%d)\n", ret); + // return ERROR; + + // } 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]); + // } + + /* XXX more tests here */ + + /* Let user know everything is ok */ + printf("\tOK: MPU-6000 passed all tests successfully\n"); + + return OK; +} + +static int +ms5611(int argc, char *argv[]) +{ + printf("\tMS5611: test start\n"); + fflush(stdout); + + int fd; + float buf[3] = {0.0f, 0.0f, 0.0f}; + int ret; + + fd = open("/dev/ms5611", O_RDONLY); + + if (fd < 0) { + printf("\tMS5611: open fail\n"); + return ERROR; + } + + for (int i = 0; i < 5; i++) { + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + if ((uint8_t)ret == -EAGAIN || (uint8_t)ret == -EINPROGRESS || i < 3) { + /* waiting for device to become ready, this is not an error */ + } else { + printf("\tMS5611: read fail (%d)\n", ret); + close(fd); + return ERROR; + } + + } else { + + /* hack for float printing */ + int32_t pressure_int = buf[0]; + int32_t altitude_int = buf[1]; + int32_t temperature_int = buf[2]; + + printf("\tMS5611: pressure:%d.%03d mbar - altitude: %d.%02d meters - temp:%d.%02d deg celcius\n", pressure_int, (int)(buf[0] * 1000 - pressure_int * 1000), altitude_int, (int)(buf[1] * 100 - altitude_int * 100), temperature_int, (int)(buf[2] * 100 - temperature_int * 100)); + } + + /* wait at least 10ms, sensor should have data after no more than 6.5ms */ + usleep(10000); + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: MS5611 passed all tests successfully\n"); + + return OK; +} + +static int +hmc5883l(int argc, char *argv[]) +{ + printf("\tHMC5883L: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/hmc5883l", O_RDONLY); + + if (fd < 0) { + printf("\tHMC5883L: open fail\n"); + return ERROR; + } + + int i; + + for (i = 0; i < 5; i++) { + /* wait at least 7ms, sensor should have data after no more than 6.5ms */ + usleep(7000); + + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tHMC5883L: read1 fail (%d) values: x:%d\ty:%d\tz:%d\n", ret, buf[0], buf[1], buf[2]); + close(fd); + return ERROR; + + } else { + printf("\tHMC5883L: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: HMC5883L passed all tests successfully\n"); + + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_sensors + ****************************************************************************/ + +int test_sensors(int argc, char *argv[]) +{ + unsigned i; + + printf("Running sensors tests:\n\n"); + fflush(stdout); + + int ret = OK; + + for (i = 0; sensors[i].name; i++) { + printf(" sensor: %s\n", sensors[i].name); + + /* Flush and leave enough time for the flush to become effective */ + fflush(stdout); + usleep(50000); + /* Test the sensor - if the tests crash at this point, the right sensor name has been printed */ + + ret += sensors[i].test(argc, argv); + } + + return ret; +} |