dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merged master into ros

This commit is contained in:
Lorenz Meier 2015-02-02 21:21:51 +01:00
parent 143508529f e6a7dc7a3f
commit d441d38677
17 changed files with 276 additions and 416 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
makefiles/config_px4fmu-v1_default.mk
View File

@ -24,18 +24,18 @@ MODULES += drivers/l3gd20
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
#MODULES += drivers/ll40ls
MODULES += drivers/ll40ls
MODULES += drivers/trone
#MODULES += drivers/mb12xx
MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
#MODULES += drivers/blinkm
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/airspeed
#MODULES += drivers/ets_airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
#MODULES += drivers/frsky_telemetry
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
#

12
makefiles/config_px4fmu-v2_default.mk
View File

@ -27,15 +27,15 @@ MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
# MODULES += drivers/sf0x
MODULES += drivers/sf0x
MODULES += drivers/ll40ls
# MODULES += drivers/trone
MODULES += drivers/trone
MODULES += drivers/gps
MODULES += drivers/hil
# MODULES += drivers/hott
# MODULES += drivers/hott/hott_telemetry
# MODULES += drivers/hott/hott_sensors
# MODULES += drivers/blinkm
MODULES += drivers/hott
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed

4
makefiles/config_px4fmu-v2_test.mk
View File

@ -26,6 +26,9 @@ MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/pca8574
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
@ -33,6 +36,7 @@ MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/ver
MODULES += systemcmds/top
MODULES += modules/sensors
#
# System commands

1
src/drivers/boards/px4fmu-v1/board_config.h
View File

@ -114,7 +114,6 @@ __BEGIN_DECLS
* Note that these are unshifted addresses.
*/
#define PX4_I2C_OBDEV_HMC5883 0x1e
#define PX4_I2C_OBDEV_MS5611 0x76
#define PX4_I2C_OBDEV_EEPROM NOTDEFINED
#define PX4_I2C_OBDEV_LED 0x55

260
src/drivers/ms5611/ms5611.cpp
View File

@ -69,6 +69,14 @@
#include "ms5611.h"
enum MS5611_BUS {
MS5611_BUS_ALL = 0,
MS5611_BUS_I2C_INTERNAL,
MS5611_BUS_I2C_EXTERNAL,
MS5611_BUS_SPI_INTERNAL,
MS5611_BUS_SPI_EXTERNAL
};
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@ -784,15 +792,38 @@ MS5611::print_info()
namespace ms5611
{
/* initialize explicitely for clarity */
MS5611 *g_dev_ext = nullptr;
MS5611 *g_dev_int = nullptr;
/*
list of supported bus configurations
*/
struct ms5611_bus_option {
enum MS5611_BUS busid;
const char *devpath;
MS5611_constructor interface_constructor;
uint8_t busnum;
MS5611 *dev;
} bus_options[] = {
#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT)
{ MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL },
#endif
#ifdef PX4_SPIDEV_BARO
{ MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL },
#endif
#ifdef PX4_I2C_BUS_ONBOARD
{ MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL },
#endif
#ifdef PX4_I2C_BUS_EXPANSION
{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
void start(bool external_bus);
void test(bool external_bus);
void reset(bool external_bus);
bool start_bus(struct ms5611_bus_option &bus);
struct ms5611_bus_option &find_bus(enum MS5611_BUS busid);
void start(enum MS5611_BUS busid);
void test(enum MS5611_BUS busid);
void reset(enum MS5611_BUS busid);
void info();
void calibrate(unsigned altitude, bool external_bus);
void calibrate(unsigned altitude, enum MS5611_BUS busid);
void usage();
/**
@ -845,89 +876,87 @@ crc4(uint16_t *n_prom)
/**
* Start the driver.
*/
void
start(bool external_bus)
bool
start_bus(struct ms5611_bus_option &bus)
{
int fd;
if (bus.dev != nullptr)
errx(1,"bus option already started");
prom_u prom_buf;
if (external_bus && (g_dev_ext != nullptr)) {
/* if already started, the still command succeeded */
errx(0, "ext already started");
} else if (!external_bus && (g_dev_int != nullptr)) {
/* if already started, the still command succeeded */
errx(0, "int already started");
}
device::Device *interface = nullptr;
/* create the driver, try SPI first, fall back to I2C if unsuccessful */
if (MS5611_spi_interface != nullptr)
interface = MS5611_spi_interface(prom_buf, external_bus);
if (interface == nullptr && (MS5611_i2c_interface != nullptr))
interface = MS5611_i2c_interface(prom_buf);
if (interface == nullptr)
errx(1, "failed to allocate an interface");
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
if (interface->init() != OK) {
delete interface;
errx(1, "interface init failed");
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
if (external_bus) {
g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT);
if (g_dev_ext == nullptr) {
delete interface;
errx(1, "failed to allocate driver");
}
if (g_dev_ext->init() != OK)
goto fail;
fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
} else {
g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT);
if (g_dev_int == nullptr) {
delete interface;
errx(1, "failed to allocate driver");
}
if (g_dev_int->init() != OK)
goto fail;
fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
bus.dev = new MS5611(interface, prom_buf, bus.devpath);
if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev;
bus.dev = NULL;
return false;
}
int fd = open(bus.devpath, O_RDONLY);
/* set the poll rate to default, starts automatic data collection */
if (fd < 0) {
warnx("can't open baro device");
goto fail;
if (fd == -1) {
errx(1, "can't open baro device");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
warnx("failed setting default poll rate");
goto fail;
close(fd);
errx(1, "failed setting default poll rate");
}
close(fd);
return true;
}
/**
* Start the driver.
*
* This function call only returns once the driver
* is either successfully up and running or failed to start.
*/
void
start(enum MS5611_BUS busid)
{
uint8_t i;
bool started = false;
for (i=0; i<NUM_BUS_OPTIONS; i++) {
if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i]);
}
if (!started)
errx(1, "driver start failed");
// one or more drivers started OK
exit(0);
}
fail:
if (g_dev_int != nullptr) {
delete g_dev_int;
g_dev_int = nullptr;
}
if (g_dev_ext != nullptr) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
errx(1, "driver start failed");
/**
* find a bus structure for a busid
*/
struct ms5611_bus_option &find_bus(enum MS5611_BUS busid)
{
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
if ((busid == MS5611_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
errx(1,"bus %u not started", (unsigned)busid);
}
/**
@ -936,20 +965,16 @@ fail:
* and automatic modes.
*/
void
test(bool external_bus)
test(enum MS5611_BUS busid)
{
struct ms5611_bus_option &bus = find_bus(busid);
struct baro_report report;
ssize_t sz;
int ret;
int fd;
if (external_bus) {
fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
} else {
fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
}
fd = open(bus.devpath, O_RDONLY);
if (fd < 0)
err(1, "open failed (try 'ms5611 start' if the driver is not running)");
@ -998,6 +1023,7 @@ test(bool external_bus)
warnx("time: %lld", report.timestamp);
}
close(fd);
errx(0, "PASS");
}
@ -1005,16 +1031,12 @@ test(bool external_bus)
* Reset the driver.
*/
void
reset(bool external_bus)
reset(enum MS5611_BUS busid)
{
struct ms5611_bus_option &bus = find_bus(busid);
int fd;
if (external_bus) {
fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
} else {
fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
}
fd = open(bus.devpath, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -1033,19 +1055,13 @@ reset(bool external_bus)
void
info()
{
if (g_dev_ext == nullptr && g_dev_int == nullptr)
errx(1, "driver not running");
if (g_dev_ext) {
warnx("ext:");
g_dev_ext->print_info();
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
struct ms5611_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
warnx("%s", bus.devpath);
bus.dev->print_info();
}
}
if (g_dev_int) {
warnx("int:");
g_dev_int->print_info();
}
exit(0);
}
@ -1053,19 +1069,16 @@ info()
* Calculate actual MSL pressure given current altitude
*/
void
calibrate(unsigned altitude, bool external_bus)
calibrate(unsigned altitude, enum MS5611_BUS busid)
{
struct ms5611_bus_option &bus = find_bus(busid);
struct baro_report report;
float pressure;
float p1;
int fd;
if (external_bus) {
fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
} else {
fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
}
fd = open(bus.devpath, O_RDONLY);
if (fd < 0)
err(1, "open failed (try 'ms5611 start' if the driver is not running)");
@ -1120,6 +1133,7 @@ calibrate(unsigned altitude, bool external_bus)
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
err(1, "BAROIOCSMSLPRESSURE");
close(fd);
exit(0);
}
@ -1128,7 +1142,10 @@ usage()
{
warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -X (external I2C bus)");
warnx(" -I (intternal I2C bus)");
warnx(" -S (external SPI bus)");
warnx(" -s (internal SPI bus)");
}
} // namespace
@ -1136,14 +1153,23 @@ usage()
int
ms5611_main(int argc, char *argv[])
{
bool external_bus = false;
enum MS5611_BUS busid = MS5611_BUS_ALL;
int ch;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "X")) != EOF) {
while ((ch = getopt(argc, argv, "XISs")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
busid = MS5611_BUS_I2C_EXTERNAL;
break;
case 'I':
busid = MS5611_BUS_I2C_INTERNAL;
break;
case 'S':
busid = MS5611_BUS_SPI_EXTERNAL;
break;
case 's':
busid = MS5611_BUS_SPI_INTERNAL;
break;
default:
ms5611::usage();
@ -1153,29 +1179,23 @@ ms5611_main(int argc, char *argv[])
const char *verb = argv[optind];
if (argc > optind+1) {
if (!strcmp(argv[optind+1], "-X")) {
external_bus = true;
}
}
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start"))
ms5611::start(external_bus);
ms5611::start(busid);
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test"))
ms5611::test(external_bus);
ms5611::test(busid);
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset"))
ms5611::reset(external_bus);
ms5611::reset(busid);
/*
* Print driver information.
@ -1192,7 +1212,7 @@ ms5611_main(int argc, char *argv[])
long altitude = strtol(argv[optind+1], nullptr, 10);
ms5611::calibrate(altitude, external_bus);
ms5611::calibrate(altitude, busid);
}
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");

6
src/drivers/ms5611/ms5611.h
View File

@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom);
} /* namespace */
/* interface factories */
extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function;
extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function;
extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function;
typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);

18
src/drivers/ms5611/ms5611_i2c.cpp
View File

@ -56,14 +56,6 @@
#include "board_config.h"
#ifdef PX4_I2C_OBDEV_MS5611
#ifndef PX4_I2C_BUS_ONBOARD
#define MS5611_BUS 1
#else
#define MS5611_BUS PX4_I2C_BUS_ONBOARD
#endif
#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */
#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */
@ -74,7 +66,7 @@ device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf);
class MS5611_I2C : public device::I2C
{
public:
MS5611_I2C(int bus, ms5611::prom_u &prom_buf);
MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf);
virtual ~MS5611_I2C();
virtual int init();
@ -113,12 +105,12 @@ private:
};
device::Device *
MS5611_i2c_interface(ms5611::prom_u &prom_buf)
MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
{
return new MS5611_I2C(MS5611_BUS, prom_buf);
return new MS5611_I2C(busnum, prom_buf);
}
MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) :
MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) :
I2C("MS5611_I2C", nullptr, bus, 0, 400000),
_prom(prom)
{
@ -274,5 +266,3 @@ MS5611_I2C::_read_prom()
/* calculate CRC and return success/failure accordingly */
return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
}
#endif /* PX4_I2C_OBDEV_MS5611 */

25
src/drivers/ms5611/ms5611_spi.cpp
View File

@ -60,14 +60,14 @@
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#ifdef PX4_SPIDEV_BARO
#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO)
device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus);
class MS5611_SPI : public device::SPI
{
public:
MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf);
MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf);
virtual ~MS5611_SPI();
virtual int init();
@ -115,20 +115,21 @@ private:
};
device::Device *
MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus)
MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
{
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
#else
#ifdef PX4_SPI_BUS_EXT
if (busnum == PX4_SPI_BUS_EXT) {
#ifdef PX4_SPIDEV_EXT_BARO
return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
#else
return nullptr;
#endif
} else {
return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
#endif
}
#endif
return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
}
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
_prom(prom_buf)
{
@ -281,4 +282,4 @@ MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
return transfer(send, recv, len);
}
#endif /* PX4_SPIDEV_BARO */
#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */

18
src/drivers/px4io/px4io.cpp
View File

@ -3271,7 +3271,23 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "safety_off")) {
int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
if (ret != OK) {
printf("failed to disable safety\n");
exit(1);
}
exit(0);
}
if (!strcmp(argv[1], "safety_on")) {
int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
if (ret != OK) {
printf("failed to enable safety\n");
exit(1);
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
@ -3400,6 +3416,6 @@ px4io_main(int argc, char *argv[])
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
"'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
"'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n"
"'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
}

3
src/modules/systemlib/err.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -66,6 +66,7 @@
#define _SYSTEMLIB_ERR_H
#include <stdarg.h>
#include "visibility.h"
__BEGIN_DECLS

85
src/modules/uORB/uORB.cpp
View File

@ -56,6 +56,7 @@
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
@ -682,9 +683,11 @@ namespace
{
ORBDevMaster *g_dev;
bool pubsubtest_passed = false;
struct orb_test {
int val;
hrt_abstime time;
};
ORB_DEFINE(orb_test, struct orb_test);
@ -718,6 +721,46 @@ test_note(const char *fmt, ...)
return OK;
}
int pubsublatency_main(void)
{
/* poll on test topic and output latency */
float latency_integral = 0.0f;
/* wakeup source(s) */
struct pollfd fds[1];
int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
fds[0].fd = test_multi_sub;
fds[0].events = POLLIN;
struct orb_test t;
const unsigned maxruns = 10;
for (unsigned i = 0; i < maxruns; i++) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t);
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
hrt_abstime elt = hrt_elapsed_time(&t.time);
latency_integral += elt;
}
orb_unsubscribe(test_multi_sub);
warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns));
pubsubtest_passed = true;
return OK;
}
int
test()
{
@ -779,8 +822,7 @@ test()
int instance0;
int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
test_note("advertised");
usleep(300000);
test_note("advertised");
int instance1;
int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
@ -795,8 +837,7 @@ test()
if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
return test_fail("mult. pub0 fail");
test_note("published");
usleep(300000);
test_note("published");
t.val = 203;
if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
@ -805,19 +846,19 @@ test()
/* subscribe to both topics and ensure valid data is received */
int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t))
if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u))
return test_fail("sub #0 copy failed: %d", errno);
if (t.val != 103)
return test_fail("sub #0 val. mismatch: %d", t.val);
if (u.val != 103)
return test_fail("sub #0 val. mismatch: %d", u.val);
int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t))
if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u))
return test_fail("sub #1 copy failed: %d", errno);
if (t.val != 203)
return test_fail("sub #1 val. mismatch: %d", t.val);
if (u.val != 203)
return test_fail("sub #1 val. mismatch: %d", u.val);
/* test priorities */
int prio;
@ -833,6 +874,30 @@ test()
if (prio != ORB_PRIO_MIN)
return test_fail("prio: %d", prio);
/* test pub / sub latency */
int pubsub_task = task_spawn_cmd("uorb_latency",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(main_t)&pubsublatency_main,
nullptr);
/* give the test task some data */
while (!pubsubtest_passed) {
t.val = 303;
t.time = hrt_absolute_time();
if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
return test_fail("mult. pub0 timing fail");
/* simulate >800 Hz system operation */
usleep(1000);
}
if (pubsub_task < 0) {
return test_fail("failed launching task");
}
return test_note("PASS");
}

3
src/systemcmds/tests/module.mk
View File

@ -32,8 +32,7 @@ SRCS = test_adc.c \
test_ppm_loopback.c \
test_rc.c \
test_conv.cpp \
test_mount.c \
test_mtd.c
test_mount.c
EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion

236
src/systemcmds/tests/test_mtd.c
View File

@ -1,236 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 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_mtd.c
*
* Param storage / file test.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <sys/stat.h>
#include <poll.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include "tests.h"
#define PARAM_FILE_NAME "/fs/mtd_params"
static int check_user_abort(int fd);
static void print_fail(void);
static void print_success(void);
int check_user_abort(int fd) {
/* check if user wants to abort */
char c;
struct pollfd fds;
int ret;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
{
warnx("Test aborted.");
fsync(fd);
close(fd);
return OK;
/* not reached */
}
}
}
return 1;
}
void print_fail()
{
printf("<[T]: MTD: FAIL>\n");
}
void print_success()
{
printf("<[T]: MTD: OK>\n");
}
int
test_mtd(int argc, char *argv[])
{
unsigned iterations= 0;
/* check if microSD card is mounted */
struct stat buffer;
if (stat(PARAM_FILE_NAME, &buffer)) {
warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
print_fail();
return 1;
}
// XXX get real storage space here
unsigned file_size = 4096;
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {256, 512, 4096};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (unsigned 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]] __attribute__((aligned(64)));
hrt_abstime start, end;
int fd = open(PARAM_FILE_NAME, O_RDONLY);
int rret = read(fd, read_buf, chunk_sizes[c]);
close(fd);
if (rret <= 0) {
err(1, "read error");
}
fd = open(PARAM_FILE_NAME, O_WRONLY);
printf("printing 2 percent of the first chunk:\n");
for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) {
printf("%02X", read_buf[i]);
}
printf("\n");
iterations = file_size / chunk_sizes[c];
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
if (wret != (int)chunk_sizes[c]) {
warn("WRITE ERROR!");
print_fail();
return 1;
}
fsync(fd);
if (!check_user_abort(fd))
return OK;
}
end = hrt_absolute_time();
warnx("write took %llu us", (end - start));
close(fd);
fd = open(PARAM_FILE_NAME, O_RDONLY);
/* read back data for validation */
for (unsigned i = 0; i < iterations; i++) {
int rret2 = read(fd, read_buf, chunk_sizes[c]);
if (rret2 != (int)chunk_sizes[c]) {
warnx("READ ERROR!");
print_fail();
return 1;
}
/* compare value */
bool compare_ok = true;
for (unsigned j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d", j);
print_fail();
compare_ok = false;
break;
}
}
if (!compare_ok) {
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
print_fail();
return 1;
}
if (!check_user_abort(fd))
return OK;
}
close(fd);
}
/* fill the file with 0xFF to make it look new again */
char ffbuf[64];
memset(ffbuf, 0xFF, sizeof(ffbuf));
int fd = open(PARAM_FILE_NAME, O_WRONLY);
for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) {
int ret = write(fd, ffbuf, sizeof(ffbuf));
if (ret != sizeof(ffbuf)) {
warnx("ERROR! Could not fill file with 0xFF");
close(fd);
print_fail();
return 1;
}
}
(void)close(fd);
print_success();
return 0;
}

3
src/systemcmds/tests/tests.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -112,7 +112,6 @@ extern int test_mixer(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]);
extern int test_conv(int argc, char *argv[]);
extern int test_mount(int argc, char *argv[]);
extern int test_mtd(int argc, char *argv[]);
extern int test_mathlib(int argc, char *argv[]);
__END_DECLS

3
src/systemcmds/tests/tests_main.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -109,7 +109,6 @@ const struct {
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mtd", test_mtd, 0},
#ifndef TESTS_MATHLIB_DISABLE
{"mathlib", test_mathlib, 0},
#endif

3
unittests/CMakeLists.txt
View File

@ -23,11 +23,14 @@ include_directories(${CMAKE_SOURCE_DIR})
include_directories(${PX_SRC})
include_directories(${PX_SRC}/modules)
include_directories(${PX_SRC}/lib)
include_directories(${PX_SRC}/drivers)
add_definitions(-D__EXPORT=)
add_definitions(-D__PX4_TESTS)
add_definitions(-Dnoreturn_function=)
add_definitions(-Dmain_t=int)
add_definitions(-DERROR=-1)
add_definitions(-DOK=0)
# check
add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)

2
unittests/sbus2_test.cpp
View File

@ -11,7 +11,7 @@
#include "gtest/gtest.h"
TEST(SBUS2Test, SBUS2) {
char *filepath = "testdata/sbus2_r7008SB.txt";
const char *filepath = "testdata/sbus2_r7008SB.txt";
FILE *fp;
fp = fopen(filepath,"rt");