From 37b4cdfce21ff6a7374599a6706ad387bd359515 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Mar 2014 10:56:33 +1100 Subject: [PATCH 01/54] board_serial: use a uint8_t buffer we should not be using 'char' for binary APIs, as the C standard does not specify if it is signed or unsigned, so results may not be consistent --- src/drivers/px4fmu/fmu.cpp | 2 +- src/modules/systemlib/board_serial.c | 8 ++++---- src/modules/systemlib/board_serial.h | 2 +- src/modules/systemlib/otp.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a4635728e..39cc329673 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1784,7 +1784,7 @@ fmu_main(int argc, char *argv[]) } if (!strcmp(verb, "id")) { - char id[12]; + uint8_t id[12]; (void)get_board_serial(id); errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c index ad8c2bf83e..182fd15c63 100644 --- a/src/modules/systemlib/board_serial.c +++ b/src/modules/systemlib/board_serial.c @@ -44,11 +44,11 @@ #include "board_config.h" #include "board_serial.h" -int get_board_serial(char *serialid) +int get_board_serial(uint8_t *serialid) { - const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START; union udid id; - val_read((unsigned *)&id, udid_ptr, sizeof(id)); + val_read((uint32_t *)&id, udid_ptr, sizeof(id)); /* Copy the serial from the chips non-write memory and swap endianess */ @@ -57,4 +57,4 @@ int get_board_serial(char *serialid) serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; return 0; -} \ No newline at end of file +} diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h index b14bb4376b..873d9657ba 100644 --- a/src/modules/systemlib/board_serial.h +++ b/src/modules/systemlib/board_serial.h @@ -44,6 +44,6 @@ __BEGIN_DECLS -__EXPORT int get_board_serial(char *serialid); +__EXPORT int get_board_serial(uint8_t *serialid); __END_DECLS diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index f10e129d8c..273b064f0e 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -125,7 +125,7 @@ struct otp_lock { #pragma pack(push, 1) union udid { uint32_t serial[3]; - char data[12]; + uint8_t data[12]; }; #pragma pack(pop) From ef79d032760e83850a9dbcbe5ae34c8b72f5fb4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 May 2014 16:01:10 +1000 Subject: [PATCH 02/54] mpu6000: allow disabling of on-sensor low pass filter used for vibration testing --- src/drivers/mpu6000/mpu6000.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0edec3d0ee..fb4acc3601 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -666,7 +666,9 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) /* choose next highest filter frequency available */ - if (frequency_hz <= 5) { + if (frequency_hz == 0) { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } else if (frequency_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; @@ -922,10 +924,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - - // XXX decide on relationship of both filters - // i.e. disable the on-chip filter - //_set_dlpf_filter((uint16_t)arg); + if (arg == 0) { + // allow disabling of on-chip filter using + // zero as desired filter frequency + _set_dlpf_filter(0); + } _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1009,8 +1012,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - // XXX check relation to the internal lowpass - //_set_dlpf_filter((uint16_t)arg); + if (arg == 0) { + // allow disabling of on-chip filter using 0 + // as desired frequency + _set_dlpf_filter(0); + } return OK; case GYROIOCSSCALE: From 07855120268cef79e9d23d7cd091c526fa4622af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 May 2014 16:49:41 +1000 Subject: [PATCH 03/54] px_uploader: added ModemManager warning --- Tools/px_uploader.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 985e6ffd9d..a113f56284 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -63,6 +63,7 @@ import zlib import base64 import time import array +import os from sys import platform as _platform @@ -449,6 +450,12 @@ parser.add_argument('--baud', action="store", type=int, default=115200, help="Ba parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() +# warn people about ModemManager which interferes badly with Pixhawk +if os.path.exists("/usr/sbin/ModemManager"): + print("=======================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with Pixhawk") + print("=======================================================================") + # Load the firmware file fw = firmware(args.firmware) print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) From d6999384ceb905f82896b99dda6341c7b615ba7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 11:42:46 +0200 Subject: [PATCH 04/54] Improve modem manager warning to avoid having smart people tell us we have Linux compatibility issues while we actually do not. --- Tools/px_uploader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index a113f56284..cd7884f6d2 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -452,9 +452,9 @@ args = parser.parse_args() # warn people about ModemManager which interferes badly with Pixhawk if os.path.exists("/usr/sbin/ModemManager"): - print("=======================================================================") - print("WARNING: You should uninstall ModemManager as it conflicts with Pixhawk") - print("=======================================================================") + print("==========================================================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)") + print("==========================================================================================================") # Load the firmware file fw = firmware(args.firmware) From db304480d996c134fbc2525e5bc96a08178275a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 May 2014 11:14:38 +1000 Subject: [PATCH 05/54] lsm303d: disable check_extremes code this could trigger with a bungee launch, and could cause higher latency due to SD card writes --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bf76dcc33..56baf32746 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -75,6 +75,10 @@ #endif static const int ERROR = -1; +// enable this to debug the buggy lsm303d sensor in very early +// prototype pixhawk boards +#define CHECK_EXTREMES 0 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -830,7 +834,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { +#if CHECK_EXTREMES check_extremes(arb); +#endif ret += sizeof(*arb); arb++; } From 644d4bb3dc6186d7908ed0aa75d973cfc0826253 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 10:06:52 +1000 Subject: [PATCH 06/54] FMUv2: added defines for FMUv3 sensors this enables EXT0 to EXT3 on external SPI bus, and gives correct names for FMUv3 board --- src/drivers/boards/px4fmu-v2/board_config.h | 10 ++++++++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 26 ++++++++++++++++++++- 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 36eb7bec4e..2ed51d7b11 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -108,6 +108,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) #define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_EXT2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define PX4_SPI_BUS_SENSORS 1 #define PX4_SPI_BUS_EXT 4 @@ -121,6 +123,14 @@ __BEGIN_DECLS /* External bus */ #define PX4_SPIDEV_EXT0 1 #define PX4_SPIDEV_EXT1 2 +#define PX4_SPIDEV_EXT2 3 +#define PX4_SPIDEV_EXT3 4 + +/* FMUv3 SPI on external bus */ +#define PX4_SPIDEV_EXT_MPU PX4_SPIDEV_EXT0 +#define PX4_SPIDEV_EXT_BARO PX4_SPIDEV_EXT1 +#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_SPIDEV_EXT2 +#define PX4_SPIDEV_EXT_GYRO PX4_SPIDEV_EXT3 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 01dbd6e774..8c37d31a73 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -98,8 +98,12 @@ __EXPORT void weak_function stm32_spiinitialize(void) #ifdef CONFIG_STM32_SPI4 stm32_configgpio(GPIO_SPI_CS_EXT0); stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_configgpio(GPIO_SPI_CS_EXT2); + stm32_configgpio(GPIO_SPI_CS_EXT3); stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); #endif } @@ -174,12 +178,32 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT2: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT3: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected); break; default: From fea4845ed97ca5219ceb8af0b0fb6d68603eea17 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:13:05 +1000 Subject: [PATCH 07/54] SPI: make _bus protected this allows runtime use of internal/external bus to determine if DRDY should be used on the L3GD20 --- src/drivers/device/spi.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 299575dc5c..03986da1e7 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -124,11 +124,13 @@ protected: LockMode locking_mode; /**< selected locking mode */ private: - int _bus; enum spi_dev_e _device; enum spi_mode_e _mode; uint32_t _frequency; struct spi_dev_s *_dev; + +protected: + int _bus; }; } // namespace device From e7360f40169a0d467448bca8f85d32a84025ca4e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:13:58 +1000 Subject: [PATCH 08/54] l3gd20: added -X switch for external bus --- src/drivers/l3gd20/l3gd20.cpp | 50 +++++++++++++++++++++++++++++------ 1 file changed, 42 insertions(+), 8 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 37e72388be..06b36d6882 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -821,7 +822,7 @@ L3GD20::measure() // if the gyro doesn't have any data ready then re-schedule // for 100 microseconds later. This ensures we don't double // read a value and then miss the next value - if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { perf_count(_reschedules); hrt_call_delay(&_call, 100); return; @@ -983,7 +984,7 @@ void info(); * Start the driver. */ void -start() +start(bool external_bus) { int fd; @@ -991,7 +992,15 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT + g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO); +#else + errx(0, "External SPI not available"); +#endif + } else { + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + } if (g_dev == nullptr) goto fail; @@ -1106,32 +1115,57 @@ info() } // namespace +void +l3gd20_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int l3gd20_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + l3gd20_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - l3gd20::start(); + if (!strcmp(verb, "start")) + l3gd20::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) l3gd20::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) l3gd20::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) l3gd20::info(); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); From e0dbc82d84c2ad0e36b88f6b6d3cbfab866b4c44 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:14:17 +1000 Subject: [PATCH 09/54] lsm303d: added -X option for external bus --- src/drivers/lsm303d/lsm303d.cpp | 51 ++++++++++++++++++++++++++------- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 56baf32746..0428901a3b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -85,6 +86,7 @@ static const int ERROR = -1; #define ADDR_INCREMENT (1<<6) #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_ACCEL_EXT "/dev/lsm303d_accel_ext" #define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ @@ -1791,16 +1793,18 @@ void logging(); * Start the driver. */ void -start() +start(bool external_bus) { int fd, fd_mag; - if (g_dev != nullptr) errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - + if (external_bus) { + g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG); + } else { + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + } if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); goto fail; @@ -1998,44 +2002,69 @@ logging() } // namespace +void +lsm303d_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int lsm303d_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + lsm303d_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - lsm303d::start(); + if (!strcmp(verb, "start")) + lsm303d::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) lsm303d::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) lsm303d::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) lsm303d::info(); /* * dump device registers */ - if (!strcmp(argv[1], "regdump")) + if (!strcmp(verb, "regdump")) lsm303d::regdump(); /* * dump device registers */ - if (!strcmp(argv[1], "logging")) + if (!strcmp(verb, "logging")) lsm303d::logging(); errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); From 541dc1825cfca3724a7fbe08abfdf88b881b0d3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:14:33 +1000 Subject: [PATCH 10/54] mpu6000: added -X option for external bus --- src/drivers/mpu6000/mpu6000.cpp | 48 ++++++++++++++++++++++++++++----- 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fb4acc3601..a1c169ffc1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -1424,7 +1425,7 @@ void info(); * Start the driver. */ void -start() +start(bool external_bus) { int fd; @@ -1433,7 +1434,15 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT + g_dev = new MPU6000(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_MPU); +#else + errx(0, "External SPI not available"); +#endif + } else { + g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_MPU); + } if (g_dev == nullptr) goto fail; @@ -1578,32 +1587,57 @@ info() } // namespace +void +mpu6000_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int mpu6000_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + mpu6000_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - mpu6000::start(); + if (!strcmp(verb, "start")) + mpu6000::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) mpu6000::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) mpu6000::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) mpu6000::info(); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); From ab90fe783287a068ee3654e488ea9144077586ab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:15:43 +1000 Subject: [PATCH 11/54] ms5611: added -X option for external SPI bus --- src/drivers/ms5611/ms5611.cpp | 46 ++++++++++++++++++++++++------- src/drivers/ms5611/ms5611.h | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 10 +++++-- 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 1ce93aeeaf..7a8d2eecf3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -775,7 +776,7 @@ namespace ms5611 MS5611 *g_dev; -void start(); +void start(bool external_bus); void test(); void reset(); void info(); @@ -832,7 +833,7 @@ crc4(uint16_t *n_prom) * Start the driver. */ void -start() +start(bool external_bus) { int fd; prom_u prom_buf; @@ -845,7 +846,7 @@ start() /* create the driver, try SPI first, fall back to I2C if unsuccessful */ if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(prom_buf); + interface = MS5611_spi_interface(prom_buf, external_bus); if (interface == nullptr && (MS5611_i2c_interface != nullptr)) interface = MS5611_i2c_interface(prom_buf); @@ -1058,41 +1059,66 @@ calibrate(unsigned altitude) } // namespace +void +ms5611_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int ms5611_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + ms5611_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - ms5611::start(); + if (!strcmp(verb, "start")) + ms5611::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) ms5611::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) ms5611::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) ms5611::info(); /* * Perform MSL pressure calibration given an altitude in metres */ - if (!strcmp(argv[1], "calibrate")) { + if (!strcmp(verb, "calibrate")) { if (argc < 2) errx(1, "missing altitude"); - long altitude = strtol(argv[2], nullptr, 10); + long altitude = strtol(argv[optind+1], nullptr, 10); ms5611::calibrate(altitude); } diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 76fb84de8e..f0b3fd61d7 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -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) weak_function; +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; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 8759d16a16..00d016aed3 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -62,7 +62,7 @@ #ifdef PX4_SPIDEV_BARO -device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf); +device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus); class MS5611_SPI : public device::SPI { @@ -115,9 +115,13 @@ private: }; device::Device * -MS5611_spi_interface(ms5611::prom_u &prom_buf) +MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) { - return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + if (external_bus) { + return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); + } else { + return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + } } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : From 19dbbf17e8b2fd37e43aa1a5cb6ae8c39012ab0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 13:51:04 +1000 Subject: [PATCH 12/54] mpu6000: allow for two mpu6000 instances, one internal, one external split g_dev into g_dev_int and g_dev_ext --- src/drivers/mpu6000/mpu6000.cpp | 82 ++++++++++++++++++--------------- 1 file changed, 46 insertions(+), 36 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index a1c169ffc1..7f9e9fde43 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -78,6 +78,8 @@ #define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" #define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" +#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext" +#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext" // MPU 6000 registers #define MPUREG_WHOAMI 0x75 @@ -178,7 +180,7 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, spi_dev_e device); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device); virtual ~MPU6000(); virtual int init(); @@ -346,7 +348,7 @@ private: class MPU6000_gyro : public device::CDev { public: - MPU6000_gyro(MPU6000 *parent); + MPU6000_gyro(MPU6000 *parent, const char *path); ~MPU6000_gyro(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); @@ -369,9 +371,9 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), - _gyro(new MPU6000_gyro(this)), +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device) : + SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call_interval(0), _accel_reports(nullptr), @@ -1357,8 +1359,8 @@ MPU6000::print_info() _gyro_reports->print_info("gyro queue"); } -MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), +MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : + CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), _gyro_class_instance(-1) @@ -1414,12 +1416,13 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) namespace mpu6000 { -MPU6000 *g_dev; +MPU6000 *g_dev_int; // on internal bus +MPU6000 *g_dev_ext; // on external bus -void start(); -void test(); -void reset(); -void info(); +void start(bool); +void test(bool); +void reset(bool); +void info(bool); /** * Start the driver. @@ -1428,30 +1431,33 @@ void start(bool external_bus) { int fd; + MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; - if (g_dev != nullptr) + if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ errx(0, "already started"); /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - g_dev = new MPU6000(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU); #else errx(0, "External SPI not available"); #endif } else { - g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU); } - if (g_dev == nullptr) + if (*g_dev_ptr == nullptr) goto fail; - if (OK != g_dev->init()) + if (OK != (*g_dev_ptr)->init()) goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + fd = open(path_accel, O_RDONLY); if (fd < 0) goto fail; @@ -1464,9 +1470,9 @@ start(bool external_bus) exit(0); fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (*g_dev_ptr != nullptr) { + delete (*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -1478,24 +1484,26 @@ fail: * and automatic modes. */ void -test() +test(bool external_bus) { + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; /* get the driver */ - int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + int fd = open(path_accel, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - MPU_DEVICE_PATH_ACCEL); + path_accel); /* get the driver */ - int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); + int fd_gyro = open(path_gyro, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); + err(1, "%s open failed", path_gyro); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1543,7 +1551,7 @@ test() /* XXX add poll-rate tests here too */ - reset(); + reset(external_bus); errx(0, "PASS"); } @@ -1551,9 +1559,10 @@ test() * Reset the driver. */ void -reset() +reset(bool external_bus) { - int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + int fd = open(path_accel, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1573,13 +1582,14 @@ reset() * Print a little info about the driver. */ void -info() +info(bool external_bus) { - if (g_dev == nullptr) + MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + if (*g_dev_ptr == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); - g_dev->print_info(); + printf("state @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_info(); exit(0); } @@ -1626,19 +1636,19 @@ mpu6000_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - mpu6000::test(); + mpu6000::test(external_bus); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - mpu6000::reset(); + mpu6000::reset(external_bus); /* * Print driver information. */ if (!strcmp(verb, "info")) - mpu6000::info(); + mpu6000::info(external_bus); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } From e4e152a85b5a29cada6559197580a9dce93e45e3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 11:30:05 +1000 Subject: [PATCH 13/54] FMUv2: added define for PX4_I2C_BUS_ONBOARD needed for hmc5883 on main bus (for FMUv3) --- src/drivers/boards/px4fmu-v2/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 2ed51d7b11..0190a5b5b0 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -134,7 +134,8 @@ __BEGIN_DECLS /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 -#define PX4_I2C_BUS_LED 2 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_ONBOARD /* Devices on the onboard bus. * From f56724f7dff1f6d1167f4fd61015ffe24a64c8c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:02:05 +1000 Subject: [PATCH 14/54] lib/conversion: added rotate_3f() will be used for user specified rotations in sensor drivers --- src/lib/conversion/rotation.cpp | 142 ++++++++++++++++++++++++++++++++ src/lib/conversion/rotation.h | 8 ++ 2 files changed, 150 insertions(+) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 614877b183..e177157333 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -49,3 +49,145 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) rot_matrix->from_euler(roll, pitch, yaw); } + +#define HALF_SQRT_2 0.70710678118654757f + +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + float tmp; + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_YAW_180: + x = -x; y = -y; + return; + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2*(y - x); + y = -HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; + return; + } + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2*(y - x); + y = HALF_SQRT_2*(y + x); + x = tmp; z = -z; + return; + } + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2*(x - y); + y = -HALF_SQRT_2*(x + y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 0c56494c5e..5187b448f2 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,4 +118,12 @@ const rot_lookup_t rot_lookup[] = { __EXPORT void get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); + +/** + * rotate a 3 element float vector in-place + */ +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z); + + #endif /* ROTATION_H_ */ From c681d6621d8e4b29c3be8d8b94bf765b42f10e49 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:02:28 +1000 Subject: [PATCH 15/54] mpu6000: added -R rotation option --- src/drivers/mpu6000/mpu6000.cpp | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 7f9e9fde43..f6e00feeef 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -180,7 +181,7 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); virtual ~MPU6000(); virtual int init(); @@ -235,6 +236,8 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + enum Rotation _rotation; + /** * Start automatic measurement. */ @@ -371,7 +374,7 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), @@ -394,7 +397,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ) + _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _rotation(rotation) { // disable debug() calls _debug_enabled = false; @@ -1304,6 +1308,9 @@ MPU6000::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + // apply user specified rotation + rotate_3f(_rotation, arb.x, arb.y, arb.z); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1322,6 +1329,9 @@ MPU6000::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + // apply user specified rotation + rotate_3f(_rotation, grb.x, grb.y, grb.z); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1419,7 +1429,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool); +void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); @@ -1428,7 +1438,7 @@ void info(bool); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd; MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; @@ -1442,12 +1452,12 @@ start(bool external_bus) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif } else { - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } if (*g_dev_ptr == nullptr) @@ -1610,13 +1620,17 @@ mpu6000_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: mpu6000_usage(); exit(0); @@ -1630,7 +1644,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - mpu6000::start(external_bus); + mpu6000::start(external_bus, rotation); /* * Test the driver/device. From 1dfc7bad7b04af74d154b9c4bd3886f346f38d72 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:06:52 +1000 Subject: [PATCH 16/54] Merged lsm303d update, keeping default frequency --- src/drivers/lsm303d/lsm303d.cpp | 33 +++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 0428901a3b..f25ac8f874 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -69,6 +69,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -222,7 +223,7 @@ class LSM303D_mag; class LSM303D : public device::SPI { public: - LSM303D(int bus, const char* path, spi_dev_e device); + LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation); virtual ~LSM303D(); virtual int init(); @@ -311,7 +312,8 @@ private: uint64_t _last_log_us; uint64_t _last_log_sync_us; uint64_t _last_log_reg_us; - uint64_t _last_log_alarm_us; + uint64_t _last_log_alarm_us; + enum Rotation _rotation; /** * Start automatic measurement. @@ -491,7 +493,7 @@ private: }; -LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _call_accel_interval(0), @@ -525,7 +527,8 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _last_log_us(0), _last_log_sync_us(0), _last_log_reg_us(0), - _last_log_alarm_us(0) + _last_log_alarm_us(0), + _rotation(rotation) { // enable debug() calls _debug_enabled = true; @@ -1541,6 +1544,9 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); + // apply user specified rotation + rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); + accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1617,6 +1623,9 @@ LSM303D::mag_measure() mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; + // apply user specified rotation + rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ @@ -1782,7 +1791,7 @@ namespace lsm303d LSM303D *g_dev; -void start(); +void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); @@ -1793,7 +1802,7 @@ void logging(); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd, fd_mag; if (g_dev != nullptr) @@ -1801,9 +1810,9 @@ start(bool external_bus) /* create the driver */ if (external_bus) { - g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); } else { - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -2015,13 +2024,17 @@ lsm303d_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: lsm303d_usage(); exit(0); @@ -2035,7 +2048,7 @@ lsm303d_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - lsm303d::start(external_bus); + lsm303d::start(external_bus, rotation); /* * Test the driver/device. From a049f0841d0a68edf2f1e5d10ba2ab24d15aa472 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:10:38 +1000 Subject: [PATCH 17/54] Merged L3GD20 orientation flag while keeping original bus speed --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 06b36d6882..f8cd509a2e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -69,6 +69,7 @@ #include #include +#include #define L3GD20_DEVICE_PATH "/dev/l3gd20" @@ -185,7 +186,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI { public: - L3GD20(int bus, const char* path, spi_dev_e device); + L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation); virtual ~L3GD20(); virtual int init(); @@ -230,6 +231,8 @@ private: /* true if an L3G4200D is detected */ bool _is_l3g4200d; + enum Rotation _rotation; + /** * Start automatic measurement. */ @@ -329,7 +332,7 @@ private: int self_test(); }; -L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : +L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call_interval(0), _reports(nullptr), @@ -346,7 +349,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), - _is_l3g4200d(false) + _is_l3g4200d(false), + _rotation(rotation) { // enable debug() calls _debug_enabled = true; @@ -915,6 +919,9 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); + // apply user specified rotation + rotate_3f(_rotation, report.x, report.y, report.z); + report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; @@ -975,7 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; -void start(); +void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); @@ -984,7 +991,7 @@ void info(); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd; @@ -994,12 +1001,12 @@ start(bool external_bus) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation); #else errx(0, "External SPI not available"); #endif } else { - g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation); } if (g_dev == nullptr) @@ -1128,13 +1135,17 @@ l3gd20_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: l3gd20_usage(); exit(0); @@ -1148,7 +1159,7 @@ l3gd20_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - l3gd20::start(external_bus); + l3gd20::start(external_bus, rotation); /* * Test the driver/device. From d2487e771884d62f893b14629caae8437bd6998e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 11:20:14 +1000 Subject: [PATCH 18/54] hmc5883: added support for -R rotation option --- src/drivers/hmc5883/hmc5883.cpp | 45 +++++++++++++++++++++++++++------ 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b7b368a5e8..75acf32e31 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -71,6 +71,8 @@ #include #include +#include +#include /* * HMC5883 internal constants and data structures. @@ -130,7 +132,7 @@ static const int ERROR = -1; class HMC5883 : public device::I2C { public: - HMC5883(int bus); + HMC5883(int bus, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -169,6 +171,7 @@ private: bool _calibrated; /**< the calibration is valid */ int _bus; /**< the bus the device is connected to */ + enum Rotation _rotation; struct mag_report _last_report; /**< used for info() */ @@ -319,7 +322,7 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus) : +HMC5883::HMC5883(int bus, enum Rotation rotation) : I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), @@ -334,7 +337,8 @@ HMC5883::HMC5883(int bus) : _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _sensor_ok(false), _calibrated(false), - _bus(bus) + _bus(bus), + _rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -862,6 +866,9 @@ HMC5883::collect() /* z remains z */ new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + // apply user specified rotation + rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { if (_mag_topic != -1) { @@ -1246,7 +1253,7 @@ const int ERROR = -1; HMC5883 *g_dev; -void start(); +void start(enum Rotation rotation); void test(); void reset(); void info(); @@ -1256,7 +1263,7 @@ int calibrate(); * Start the driver. */ void -start() +start(enum Rotation rotation) { int fd; @@ -1265,7 +1272,7 @@ start() errx(0, "already started"); /* create the driver, attempt expansion bus first */ - g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); + g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION, rotation); if (g_dev != nullptr && OK != g_dev->init()) { delete g_dev; g_dev = nullptr; @@ -1275,7 +1282,7 @@ start() #ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ if (g_dev == nullptr) { - g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD); + g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD, rotation); if (g_dev != nullptr && OK != g_dev->init()) { goto fail; } @@ -1474,14 +1481,36 @@ info() } // namespace +void +hmc5883_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); + warnx("options:"); + warnx(" -R rotation"); +} + int hmc5883_main(int argc, char *argv[]) { + int ch; + enum Rotation rotation = ROTATION_NONE; + + while ((ch = getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + default: + hmc5883_usage(); + exit(0); + } + } + /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) - hmc5883::start(); + hmc5883::start(rotation); /* * Test the driver/device. From d952e81ab7c5ef050784cf3766c5b7bf18909777 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 11:48:44 +1000 Subject: [PATCH 19/54] added support for two instances of hmc5883 driver --- src/drivers/hmc5883/hmc5883.cpp | 115 ++++++++++++++++++++------------ 1 file changed, 72 insertions(+), 43 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 75acf32e31..b9270d2a0b 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -79,7 +79,8 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 -#define HMC5883L_DEVICE_PATH "/dev/hmc5883" +#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" +#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -132,7 +133,7 @@ static const int ERROR = -1; class HMC5883 : public device::I2C { public: - HMC5883(int bus, enum Rotation rotation); + HMC5883(int bus, const char *path, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -322,8 +323,8 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus, enum Rotation rotation) : - I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), +HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : + I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1251,13 +1252,14 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev; +HMC5883 *g_dev_int; +HMC5883 *g_dev_ext; -void start(enum Rotation rotation); -void test(); -void reset(); +void start(int bus, enum Rotation rotation); +void test(int bus); +void reset(int bus); void info(); -int calibrate(); +int calibrate(int bus); /** * Start the driver. @@ -1267,47 +1269,69 @@ start(enum Rotation rotation) { int fd; - if (g_dev != nullptr) - /* if already started, the still command succeeded */ - errx(0, "already started"); - /* create the driver, attempt expansion bus first */ - g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION, rotation); - if (g_dev != nullptr && OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) + errx(0, "already started external"); + g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } } #ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (g_dev == nullptr) { - g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD, rotation); - if (g_dev != nullptr && OK != g_dev->init()) { + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) + errx(0, "already started internal"); + g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { goto fail; } } #endif - if (g_dev == nullptr) + if (g_dev_int == nullptr && g_dev_ext == nullptr) goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + if (g_dev_int != nullptr) { + fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY); + if (fd < 0) + goto fail; - if (fd < 0) - goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + close(fd); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; + if (g_dev_ext != nullptr) { + fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY); + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + close(fd); + } exit(0); fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + 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"); @@ -1319,16 +1343,17 @@ fail: * and automatic modes. */ void -test() +test(int bus) { struct mag_report report; ssize_t sz; int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1421,14 +1446,15 @@ test() * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. * Using the self test method described above, the user can scale sensor */ -int calibrate() +int calibrate(int bus) { int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1448,9 +1474,11 @@ int calibrate() * Reset the driver. */ void -reset() +reset(int bus) { - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + + int fd = open(path, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1468,8 +1496,9 @@ reset() * Print a little info about the driver. */ void -info() +info(int bus) { + HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) errx(1, "driver not running"); @@ -1516,25 +1545,25 @@ hmc5883_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(argv[1], "test")) - hmc5883::test(); + hmc5883::test(bus); /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) - hmc5883::reset(); + hmc5883::reset(bus); /* * Print driver information. */ if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) - hmc5883::info(); + hmc5883::info(bus); /* * Autocalibrate the scaling */ if (!strcmp(argv[1], "calibrate")) { - if (hmc5883::calibrate() == 0) { + if (hmc5883::calibrate(bus) == 0) { errx(0, "calibration successful"); } else { From 5e62ae7a9e2a7d3ea05d293900f7171884fbb448 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 12:07:23 +1000 Subject: [PATCH 20/54] hmc5883: added -C option to calibrate on startup Conflicts: src/drivers/hmc5883/hmc5883.cpp --- src/drivers/hmc5883/hmc5883.cpp | 42 +++++++++++++++++++++++++++------ 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b9270d2a0b..65c8c64d05 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1516,6 +1516,11 @@ hmc5883_usage() warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); warnx("options:"); warnx(" -R rotation"); + warnx(" -C calibrate on start"); + warnx(" -X only external bus"); +#ifdef PX4_I2C_BUS_ONBOARD + warnx(" -I only internal bus"); +#endif } int @@ -1523,46 +1528,69 @@ hmc5883_main(int argc, char *argv[]) { int ch; enum Rotation rotation = ROTATION_NONE; + bool calibrate = false; - while ((ch = getopt(argc, argv, "R:")) != EOF) { + while ((ch = getopt(argc, argv, "XIR:C")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); break; +#ifdef PX4_I2C_BUS_ONBOARD + case 'I': + bus = PX4_I2C_BUS_ONBOARD; + break; +#endif + case 'X': + bus = PX4_I2C_BUS_EXPANSION; + break; + case 'C': + calibrate = true; + break; default: hmc5883_usage(); exit(0); } } + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - hmc5883::start(rotation); + if (!strcmp(verb, "start")) { + hmc5883::start(bus, rotation); + if (calibrate) { + if (hmc5883::calibrate(bus) == 0) { + errx(0, "calibration successful"); + + } else { + errx(1, "calibration failed"); + } + } + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) hmc5883::test(bus); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) hmc5883::reset(bus); /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(verb, "info") || !strcmp(verb, "status")) hmc5883::info(bus); /* * Autocalibrate the scaling */ - if (!strcmp(argv[1], "calibrate")) { + if (!strcmp(verb, "calibrate")) { if (hmc5883::calibrate(bus) == 0) { errx(0, "calibration successful"); From dfee93f3b128a7f23d74363b7700c80ababbe690 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jul 2014 22:37:40 +1000 Subject: [PATCH 21/54] hmc5883: fixed driver startup when trying both buses --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 65c8c64d05..25cbc06799 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1325,11 +1325,11 @@ start(enum Rotation rotation) exit(0); fail: - if (g_dev_int != nullptr) { + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } - if (g_dev_ext != nullptr) { + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; } From 1c6ea067902708a2b1b3faf55938e6e8768abe18 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Jul 2014 08:52:16 +1000 Subject: [PATCH 22/54] hmc5883: fixed build warnings --- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 25cbc06799..72d6bdd959 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1255,10 +1255,11 @@ const int ERROR = -1; HMC5883 *g_dev_int; HMC5883 *g_dev_ext; +void hmc5883_usage(); void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); -void info(); +void info(int bus); int calibrate(int bus); /** From 42716345859ded979189ea7f2548d512975c4dec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Jul 2014 08:52:27 +1000 Subject: [PATCH 23/54] l3gd20: fixed a build warning --- src/drivers/l3gd20/l3gd20.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f8cd509a2e..cad3ff9378 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -982,6 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; +void l3gd20_usage(); void start(bool external_bus, enum Rotation rotation); void test(); void reset(); From 1b555f2d2e0684485fa47db7acfcf5555a7c6b16 Mon Sep 17 00:00:00 2001 From: akdslr Date: Tue, 15 Apr 2014 15:28:30 -0400 Subject: [PATCH 24/54] LL40LS driver: Added new driver to the config make files --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 4d727aa4de..cc0958c29a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,6 +25,7 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx +MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8e83df391a..adfbc2b7db 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -28,6 +28,7 @@ MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/sf0x +MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry From 36a9123822dca314f4efc4b4cb140159dbe9e634 Mon Sep 17 00:00:00 2001 From: akdslr Date: Tue, 15 Apr 2014 20:50:31 -0400 Subject: [PATCH 25/54] LL40LS driver: adding new range finder driver --- src/drivers/ll40ls/ll40ls.cpp | 880 ++++++++++++++++++++++++++++++++++ src/drivers/ll40ls/module.mk | 40 ++ 2 files changed, 920 insertions(+) create mode 100644 src/drivers/ll40ls/ll40ls.cpp create mode 100644 src/drivers/ll40ls/module.mk diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp new file mode 100644 index 0000000000..cb0eac91b2 --- /dev/null +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -0,0 +1,880 @@ +/**************************************************************************** + * + * Copyright (c) 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 ll40ls.cpp + * @author Allyson Kreft + * + * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* Configuration Constants */ +#define LL40LS_BUS PX4_I2C_BUS_EXPANSION +#define LL40LS_BASEADDR 0x42 /* 7-bit address */ +#define LL40LS_DEVICE_PATH "/dev/ll40ls" + +/* LL40LS Registers addresses */ + +#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ +#define LL40LS_MSRREG_ACQUIRE 0x61 /* Value to initiate a measurement */ +#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ + +/* Device limits */ +#define LL40LS_MIN_DISTANCE (0.00f) +#define LL40LS_MAX_DISTANCE (14.00f) + +#define LL40LS_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class LL40LS : public device::I2C +{ +public: + LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR); + virtual ~LL40LS(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); + +LL40LS::LL40LS(int bus, int address) : + I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000), + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _class_instance(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) +{ + // up the retries since the device misses the first measure attempts + I2C::_retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +LL40LS::~LL40LS() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +LL40LS::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +LL40LS::probe() +{ + return measure(); +} + +void +LL40LS::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +LL40LS::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +LL40LS::get_minimum_distance() +{ + return _min_distance; +} + +float +LL40LS::get_maximum_distance() +{ + return _max_distance; +} + +int +LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +LL40LS::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(LL40LS_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +LL40LS::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; + ret = transfer(cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +LL40LS::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + // read the high and low byte distance registers + uint8_t distance_reg = LL40LS_DISTHIGH_REG; + ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { + report.valid = 1; + } + else { + report.valid = 0; + } + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +LL40LS::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +LL40LS::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +LL40LS::cycle_trampoline(void *arg) +{ + LL40LS *dev = (LL40LS *)arg; + + dev->cycle(); +} + +void +LL40LS::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LL40LS::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LL40LS::cycle_trampoline, + this, + USEC2TICK(LL40LS_CONVERSION_INTERVAL)); +} + +void +LL40LS::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace ll40ls +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +LL40LS *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new LL40LS(LL40LS_BUS); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +ll40ls_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + ll40ls::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + ll40ls::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + ll40ls::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + ll40ls::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + ll40ls::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk new file mode 100644 index 0000000000..ab7d432690 --- /dev/null +++ b/src/drivers/ll40ls/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +# +# Makefile to build the PulsedLight Lidar-Lite driver. +# + +MODULE_COMMAND = ll40ls + +SRCS = ll40ls.cpp From 7f91f0cc3ef806a7959e06ab16d4094bcfe871bd Mon Sep 17 00:00:00 2001 From: akdslr Date: Wed, 30 Apr 2014 14:09:30 -0400 Subject: [PATCH 26/54] LL40LS driver: Updated the value to write to the register to trigger a measurement --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index cb0eac91b2..2872078375 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -79,7 +79,7 @@ /* LL40LS Registers addresses */ #define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_ACQUIRE 0x61 /* Value to initiate a measurement */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ #define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ /* Device limits */ From 67b7a858881229b95483e986a6e4d90ab34ac257 Mon Sep 17 00:00:00 2001 From: akdslr Date: Thu, 5 Jun 2014 09:53:56 -0400 Subject: [PATCH 27/54] LL40LS driver: Changes from the May 4th plane test flight --- src/drivers/ll40ls/ll40ls.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 2872078375..bc012e2e92 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -86,7 +86,7 @@ #define LL40LS_MIN_DISTANCE (0.00f) #define LL40LS_MAX_DISTANCE (14.00f) -#define LL40LS_CONVERSION_INTERVAL 60000 /* 60ms */ +#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -517,8 +517,8 @@ LL40LS::collect() return ret; } - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.01f; /* cm to m */ struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -532,8 +532,10 @@ LL40LS::collect() report.valid = 0; } - /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); From ba54983cdf33b55b47f3b46c9b2461ab7217461a Mon Sep 17 00:00:00 2001 From: akdslr Date: Fri, 6 Jun 2014 08:52:06 -0400 Subject: [PATCH 28/54] MB12XX Driver: Added a class instance and device specific path --- src/drivers/mb12xx/mb12xx.cpp | 45 +++++++++++++++++++++++++---------- 1 file changed, 32 insertions(+), 13 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 5adb8cf698..b63e54dacb 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -74,6 +74,7 @@ /* Configuration Constants */ #define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ +#define MB12XX_DEVICE_PATH "/dev/mb12xx" /* MB12xx Registers addresses */ @@ -124,6 +125,7 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; + int _class_instance; orb_advert_t _range_finder_topic; @@ -187,7 +189,7 @@ private: extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); MB12XX::MB12XX(int bus, int address) : - I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), _reports(nullptr), @@ -195,6 +197,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _range_finder_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) @@ -215,6 +218,15 @@ MB12XX::~MB12XX() if (_reports != nullptr) { delete _reports; } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -234,13 +246,18 @@ MB12XX::init() goto out; } - /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } } ret = OK; @@ -505,8 +522,10 @@ MB12XX::collect() report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -665,7 +684,7 @@ start() } /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { goto fail; @@ -715,10 +734,10 @@ test() ssize_t sz; int ret; - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", MB12XX_DEVICE_PATH); } /* do a simple demand read */ @@ -776,7 +795,7 @@ test() void reset() { - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { err(1, "failed "); From a6b52f1c9f720a8dd31099e7adeedee694f60729 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:49:24 +0200 Subject: [PATCH 29/54] HMC5883 post merge cleanup --- src/drivers/device/spi.cpp | 4 ++-- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fed6c312c1..0b232e158a 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -69,11 +69,11 @@ SPI::SPI(const char *name, // protected locking_mode(LOCK_PREEMPTION), // private - _bus(bus), _device(device), _mode(mode), _frequency(frequency), - _dev(nullptr) + _dev(nullptr), + _bus(bus) { } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 72d6bdd959..c2e4700d83 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1266,7 +1266,7 @@ int calibrate(int bus); * Start the driver. */ void -start(enum Rotation rotation) +start(int bus, enum Rotation rotation) { int fd; @@ -1528,6 +1528,7 @@ int hmc5883_main(int argc, char *argv[]) { int ch; + int bus = -1; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; From 369c7d277f2fea351ca4243debccc0a115f3f7e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:50:00 +0200 Subject: [PATCH 30/54] Board config cleanup for external bus support --- makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/boards/px4fmu-v1/board_config.h | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 66d9efbf8c..395a8f2ace 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -42,6 +42,7 @@ MODULES += modules/uORB LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter +MODULES += lib/conversion # # Libraries diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index c944007a56..a70a6240cb 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -86,7 +86,6 @@ __BEGIN_DECLS #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPI_BUS_EXT 2 /* * Use these in place of the spi_dev_e enumeration to From a42ec7df1b417a34e072a68c6e34240a97d5ba80 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:50:24 +0200 Subject: [PATCH 31/54] MS5611: Deal with missing external bus --- src/drivers/ms5611/ms5611_spi.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 00d016aed3..5234ce8d6b 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -118,7 +118,11 @@ device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) { 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 + return nullptr; + #endif } else { return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } From 92426c5cfc8e9746fe42e4c9c087a5e25a4be658 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:51:33 +0200 Subject: [PATCH 32/54] LSM303D: deal with missing external bus --- src/drivers/lsm303d/lsm303d.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index f25ac8f874..296899ccdd 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1810,10 +1810,15 @@ start(bool external_bus, enum Rotation rotation) /* create the driver */ if (external_bus) { + #ifdef PX4_SPI_BUS_EXT g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); + #else + errx(0, "External SPI not available"); + #endif } else { g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } + if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); goto fail; From 7d15e999f12fcfa7650c673e8be1daffbbfc46d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:10 +0200 Subject: [PATCH 33/54] LSM303: Fix usage function call to fit existing structure. --- src/drivers/lsm303d/lsm303d.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 296899ccdd..7ba0fcc88b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1797,6 +1797,7 @@ void reset(); void info(); void regdump(); void logging(); +void usage(); /** * Start the driver. @@ -2013,17 +2014,17 @@ logging() exit(0); } - -} // namespace - void -lsm303d_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int lsm303d_main(int argc, char *argv[]) { @@ -2041,7 +2042,7 @@ lsm303d_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - lsm303d_usage(); + lsm303d::usage(); exit(0); } } From 875be65242dc58503c44ff522372b3cc2df83619 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:28 +0200 Subject: [PATCH 34/54] MPU6000: Fix usage function call to fit existing structure. --- src/drivers/mpu6000/mpu6000.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index f6e00feeef..5668b0865f 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1433,6 +1433,7 @@ void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); +void usage(); /** * Start the driver. @@ -1604,17 +1605,17 @@ info(bool external_bus) exit(0); } - -} // namespace - void -mpu6000_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int mpu6000_main(int argc, char *argv[]) { @@ -1632,7 +1633,7 @@ mpu6000_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - mpu6000_usage(); + mpu6000::usage(); exit(0); } } From 812d326912feb41bb37848d61b1a6e6e1d18ab90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:42 +0200 Subject: [PATCH 35/54] L3GD20: Fix usage function call to fit existing structure. --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index cad3ff9378..64d1a7e552 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -982,7 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; -void l3gd20_usage(); +void usage(); void start(bool external_bus, enum Rotation rotation); void test(); void reset(); @@ -1120,17 +1120,17 @@ info() exit(0); } - -} // namespace - void -l3gd20_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int l3gd20_main(int argc, char *argv[]) { @@ -1148,7 +1148,7 @@ l3gd20_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - l3gd20_usage(); + l3gd20::usage(); exit(0); } } From f2cbc7fe2778b4148bdecd59148ec96b84a248df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:53:10 +0200 Subject: [PATCH 36/54] MB12xx: Fix initialiser order in class --- src/drivers/mb12xx/mb12xx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index b63e54dacb..beb6c8e35e 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -196,8 +196,8 @@ MB12XX::MB12XX(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), - _range_finder_topic(-1), _class_instance(-1), + _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) From 83f343f196c07d0a633ee8fc7d2324e8cb3d129c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:53:25 +0200 Subject: [PATCH 37/54] LL40LS: Fix initialiser order in class --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index bc012e2e92..a69e6ee556 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -196,8 +196,8 @@ LL40LS::LL40LS(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), - _range_finder_topic(-1), _class_instance(-1), + _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) From ea4de4adc388e9353114e6e2c89eb27cec5b249e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:54:01 +0200 Subject: [PATCH 38/54] param command: fix warnings --- src/systemcmds/param/param.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 28e1b108b0..e110335e7c 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,7 +63,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); -static void do_compare(const char* name, const char* vals[], unsigned comparisons); +static void do_compare(const char* name, char* vals[], unsigned comparisons); static void do_reset(void); static void do_reset_nostart(void); @@ -351,7 +351,7 @@ do_set(const char* name, const char* val, bool fail_on_not_found) } static void -do_compare(const char* name, const char* vals[], unsigned comparisons) +do_compare(const char* name, char* vals[], unsigned comparisons) { int32_t i; float f; From e380aa3f62c9db89f048f3683e3611e455de4ad2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:54:33 +0200 Subject: [PATCH 39/54] AT24C: Fix warning due to missing function prototype. --- src/systemcmds/mtd/24xxxx_mtd.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 72200f4188..9913637970 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -161,6 +161,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); void at24c_test(void); +int at24c_nuke(void); /************************************************************************************ * Private Data From ac8f179f2dad99d664a6f9de4df954bea7fe5858 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:24:18 +1000 Subject: [PATCH 40/54] Tools: skip check_submodules.sh when NUTTX_SRC is set this avoids using submodules when a specific NuttX tree is specified --- Tools/check_submodules.sh | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47e..4b8789b288 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,5 +1,12 @@ #!/bin/sh +[ -n "$NUTTX_SRC" ] && { + # NUTTX_SRC is set, meaning user is overriding the NuttX tree to use. Don't + # use submodules to pull an alternatiie tree + echo "Skipping submodules as NUTTX_SRC is set to $NUTTX_SRC" + exit 0 +} + if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") From 52713bc0ba891854934f62ad856f1600e8ba3065 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:00:44 +0200 Subject: [PATCH 41/54] Revert "Tools: skip check_submodules.sh when NUTTX_SRC is set" This reverts commit ac8f179f2dad99d664a6f9de4df954bea7fe5858. --- Tools/check_submodules.sh | 7 ------- 1 file changed, 7 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4b8789b288..fb180ef47e 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,12 +1,5 @@ #!/bin/sh -[ -n "$NUTTX_SRC" ] && { - # NUTTX_SRC is set, meaning user is overriding the NuttX tree to use. Don't - # use submodules to pull an alternatiie tree - echo "Skipping submodules as NUTTX_SRC is set to $NUTTX_SRC" - exit 0 -} - if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") From bc06d839eab504e039c27cd472bbd1162cb7bbf4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:24:18 +1000 Subject: [PATCH 42/54] Tools: skip check_submodules.sh when GIT_SUBMODULES_ARE_EVIL is set this avoids using submodules when a specific NuttX tree is specified --- Tools/check_submodules.sh | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47e..55bcc05ee0 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,5 +1,11 @@ #!/bin/sh +[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && { + # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules + echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC" + exit 0 +} + if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") From 76f82bf237de367a3a3b214f2ba06faddb077c57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:04:01 +0200 Subject: [PATCH 43/54] Updated submodule update instructions --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 55bcc05ee0..7c3a3bfc2e 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -12,7 +12,7 @@ if [ -d NuttX/nuttx ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" + echo "NuttX sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi @@ -28,7 +28,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked mavlink submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi From ee9233451244604f1522dda5e58d1deb7ec6473d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:23:07 +1000 Subject: [PATCH 44/54] build: fixed running build from external directory --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 8bf96ca230..7b7c00704f 100644 --- a/Makefile +++ b/Makefile @@ -210,11 +210,11 @@ menuconfig: endif $(NUTTX_SRC): - $(Q) (./Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) .PHONY: checksubmodules checksubmodules: - $(Q) (./Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) .PHONY: updatesubmodules updatesubmodules: From 93d444d1aade59b5e88f41b71c842a00ab950c64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:12 +1000 Subject: [PATCH 45/54] device: added a _device_id to all drivers this device ID identifies a specific device via the tuple of (bus, bus type, address, devtype). This allows device specific configuration data to be stored along with a device ID, so the code can know when the user has changed device configuration (such as removing an external compass), and either invalidate the device configuration or force the user to re-calibrate --- src/drivers/device/device.cpp | 12 ++++++++++++ src/drivers/device/device.h | 25 +++++++++++++++++++++++++ src/drivers/device/i2c.cpp | 8 +++++++- src/drivers/device/spi.cpp | 6 ++++++ src/drivers/drv_device.h | 7 +++++++ 5 files changed, 57 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index 6834551493..f1f777dce0 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -42,6 +42,7 @@ #include #include #include +#include namespace device { @@ -93,6 +94,13 @@ Device::Device(const char *name, _irq_attached(false) { sem_init(&_lock, 0, 1); + + /* setup a default device ID. When bus_type is UNKNOWN the + other fields are invalid */ + _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; + _device_id.devid_s.bus = 0; + _device_id.devid_s.address = 0; + _device_id.devid_s.devtype = 0; } Device::~Device() @@ -238,6 +246,10 @@ Device::write(unsigned offset, void *data, unsigned count) int Device::ioctl(unsigned operation, unsigned &arg) { + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } return -ENODEV; } diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index d99f229220..c98386eb07 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -124,9 +124,34 @@ public: */ virtual int ioctl(unsigned operation, unsigned &arg); + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2 + }; + + /* + broken out + */ + struct DeviceStructure { + enum DeviceBusType bus_type; + uint8_t bus; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; + protected: const char *_name; /**< driver name */ bool _debug_enabled; /**< if true, debug messages are printed */ + union DeviceId _device_id; /**< device identifier information */ /** * Constructor diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp index a416801eb7..286778c014 100644 --- a/src/drivers/device/i2c.cpp +++ b/src/drivers/device/i2c.cpp @@ -62,6 +62,12 @@ I2C::I2C(const char *name, _frequency(frequency), _dev(nullptr) { + // fill in _device_id fields for a I2C device + _device_id.devid_s.bus_type = DeviceBusType_I2C; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -201,4 +207,4 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) return ret; } -} // namespace device \ No newline at end of file +} // namespace device diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 1ab1dc6997..200acac05a 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -75,6 +75,12 @@ SPI::SPI(const char *name, _dev(nullptr), _bus(bus) { + // fill in _device_id fields for a SPI device + _device_id.devid_s.bus_type = DeviceBusType_SPI; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = (uint8_t)device; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; } SPI::~SPI() diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index b310beb746..19d55cef38 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -59,4 +59,11 @@ /** check publication block status */ #define DEVIOCGPUBBLOCK _DEVICEIOC(1) +/** + * Return device ID, to enable matching of configuration parameters + * (such as compass offsets) to specific sensors + */ +#define DEVIOCGDEVICEID _DEVICEIOC(2) + + #endif /* _DRV_DEVICE_H */ From a2739707bb18a1aed0dcc0f809badce606aaed51 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:26 +1000 Subject: [PATCH 46/54] drv_mag: added devtypes for magnetometers --- src/drivers/drv_mag.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 06107bd3d1..a259ac9c06 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -81,6 +81,13 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag); + +/* + * mag device types, for _device_id + */ +#define DRV_MAG_DEVTYPE_HMC5883 1 +#define DRV_MAG_DEVTYPE_LSM303D 2 + /* * ioctl() definitions */ From 30a6a3d0b6cc53fef45264d140ce3026d986af83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:38 +1000 Subject: [PATCH 47/54] hmc5883: setup device type --- src/drivers/hmc5883/hmc5883.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index c2e4700d83..cd2b5c48e5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -341,6 +341,8 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _bus(bus), _rotation(rotation) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + // enable debug() calls _debug_enabled = false; From c6b0dc1ee85348d8dbe398457e9631e095ec9c61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:47 +1000 Subject: [PATCH 48/54] lsm303d: setup device type --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 7ba0fcc88b..45e7750552 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -530,6 +530,8 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _last_log_alarm_us(0), _rotation(rotation) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + // enable debug() calls _debug_enabled = true; From 6cffa948fe6edc1e58b7d55acf119e8793461510 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:37:45 +1000 Subject: [PATCH 49/54] device: pass CDev::ioctl() to superclass this allows DEVIOCGDEVICEID to work. --- src/drivers/device/cdev.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 6e2ebb1f7b..39fb89501d 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -268,6 +268,13 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) break; } + /* try the superclass. The different ioctl() function form + * means we need to copy arg */ + unsigned arg2 = arg; + int ret = Device::ioctl(cmd, arg2); + if (ret != -ENODEV) + return ret; + return -ENOTTY; } From 20cd5026d817a4f17b96906d1c93fc3cbaa498dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Jul 2014 09:49:43 +1000 Subject: [PATCH 50/54] device: use bitfields to keep DeviceStructure small this keeps it small enough to fit in a float, which makes it possible to see the full value in a MAVLink tlog Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/device/device.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index c98386eb07..7df234cab0 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -134,11 +134,14 @@ public: }; /* - broken out + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type; - uint8_t bus; // which instance of the bus type + enum DeviceBusType bus_type:3; + uint8_t bus:5; // which instance of the bus type uint8_t address; // address on the bus (eg. I2C address) uint8_t devtype; // device class specific device type }; From 8a3a87331da2077cd1da4c3da8fe2743f188a4a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Jul 2014 22:21:53 +1000 Subject: [PATCH 51/54] mpu6000: fixed internal/external mixup in pointers Thanks to Emile for spotting this! Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/mpu6000/mpu6000.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 5668b0865f..1b3a96a0d4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1442,7 +1442,7 @@ void start(bool external_bus, enum Rotation rotation) { int fd; - MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; @@ -1595,7 +1595,7 @@ reset(bool external_bus) void info(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; if (*g_dev_ptr == nullptr) errx(1, "driver not running"); From 005dd206d17920761f8e27b21d54770da59faa13 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Jul 2014 09:51:22 +1000 Subject: [PATCH 52/54] hmc5883: periodically check the config and range registers this copes with I2C comms errors causing the range or config registers to become corrupted, leading to bad reading. This is easily reproducible with a 1.3m I2C cable in the same run of cable as a GPS UART cable. The error happens every half hour or so. Conflicts: mavlink/include/mavlink/v1.0 src/drivers/hmc5883/hmc5883.cpp --- src/drivers/hmc5883/hmc5883.cpp | 141 ++++++++++++++++++++++++++------ 1 file changed, 114 insertions(+), 27 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cd2b5c48e5..4aef43102e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -166,6 +166,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + perf_counter_t _range_errors; + perf_counter_t _conf_errors; /* status reporting */ bool _sensor_ok; /**< sensor was found and reports ok */ @@ -176,6 +178,9 @@ private: struct mag_report _last_report; /**< used for info() */ + uint8_t _range_bits; + uint8_t _conf_reg; + /** * Test whether the device supported by the driver is present at a * specific address. @@ -233,6 +238,23 @@ private: */ int set_range(unsigned range); + /** + * check the sensor range. + * + * checks that the range of the sensor is correctly set, to + * cope with communication errors causing the range to change + */ + void check_range(void); + + /** + * check the sensor configuration. + * + * checks that the config of the sensor is correctly set, to + * cope with communication errors causing the configuration to + * change + */ + void check_conf(void); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -336,10 +358,15 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), + _range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")), + _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), _bus(bus), - _rotation(rotation) + _rotation(rotation), + _last_report{0}, + _range_bits(0), + _conf_reg(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -373,6 +400,8 @@ HMC5883::~HMC5883() perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); + perf_free(_range_errors); + perf_free(_conf_errors); } int @@ -403,45 +432,43 @@ out: int HMC5883::set_range(unsigned range) { - uint8_t range_bits; - if (range < 1) { - range_bits = 0x00; + _range_bits = 0x00; _range_scale = 1.0f / 1370.0f; _range_ga = 0.88f; } else if (range <= 1) { - range_bits = 0x01; + _range_bits = 0x01; _range_scale = 1.0f / 1090.0f; _range_ga = 1.3f; } else if (range <= 2) { - range_bits = 0x02; + _range_bits = 0x02; _range_scale = 1.0f / 820.0f; _range_ga = 1.9f; } else if (range <= 3) { - range_bits = 0x03; + _range_bits = 0x03; _range_scale = 1.0f / 660.0f; _range_ga = 2.5f; } else if (range <= 4) { - range_bits = 0x04; + _range_bits = 0x04; _range_scale = 1.0f / 440.0f; _range_ga = 4.0f; } else if (range <= 4.7f) { - range_bits = 0x05; + _range_bits = 0x05; _range_scale = 1.0f / 390.0f; _range_ga = 4.7f; } else if (range <= 5.6f) { - range_bits = 0x06; + _range_bits = 0x06; _range_scale = 1.0f / 330.0f; _range_ga = 5.6f; } else { - range_bits = 0x07; + _range_bits = 0x07; _range_scale = 1.0f / 230.0f; _range_ga = 8.1f; } @@ -451,7 +478,7 @@ int HMC5883::set_range(unsigned range) /* * Send the command to set the range */ - ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); if (OK != ret) perf_count(_comms_errors); @@ -462,7 +489,53 @@ int HMC5883::set_range(unsigned range) if (OK != ret) perf_count(_comms_errors); - return !(range_bits_in == (range_bits << 5)); + return !(range_bits_in == (_range_bits << 5)); +} + +/** + check that the range register has the right value. This is done + periodically to cope with I2C bus noise causing the range of the + compass changing. + */ +void HMC5883::check_range(void) +{ + int ret; + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (range_bits_in != (_range_bits<<5)) { + perf_count(_range_errors); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); + if (OK != ret) + perf_count(_comms_errors); + } +} + +/** + check that the configuration register has the right value. This is + done periodically to cope with I2C bus noise causing the + configuration of the compass to change. + */ +void HMC5883::check_conf(void) +{ + int ret; + + uint8_t conf_reg_in; + ret = read_reg(ADDR_CONF_A, conf_reg_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (conf_reg_in != _conf_reg) { + perf_count(_conf_errors); + ret = write_reg(ADDR_CONF_A, _conf_reg); + if (OK != ret) + perf_count(_comms_errors); + } } int @@ -796,7 +869,7 @@ HMC5883::collect() } report; int ret = -EIO; uint8_t cmd; - + uint8_t check_counter; perf_begin(_sample_perf); struct mag_report new_report; @@ -895,6 +968,21 @@ HMC5883::collect() /* notify anyone waiting for data */ poll_notify(POLLIN); + /* + periodically check the range register and configuration + registers. With a bad I2C cable it is possible for the + registers to become corrupt, leading to bad readings. It + doesn't happen often, but given the poor cables some + vehicles have it is worth checking for. + */ + check_counter = perf_event_count(_sample_perf) % 256; + if (check_counter == 0) { + check_range(); + } + if (check_counter == 128) { + check_conf(); + } + ret = OK; out: @@ -1168,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable) { int ret; /* arm the excitement strap */ - uint8_t conf_reg; - ret = read_reg(ADDR_CONF_A, conf_reg); + ret = read_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); if (((int)enable) < 0) { - conf_reg |= 0x01; + _conf_reg |= 0x01; } else if (enable > 0) { - conf_reg |= 0x02; + _conf_reg |= 0x02; } else { - conf_reg &= ~0x03; + _conf_reg &= ~0x03; } - // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); - ret = write_reg(ADDR_CONF_A, conf_reg); + ret = write_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); @@ -1196,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable) //print_info(); - return !(conf_reg == conf_reg_ret); + return !(_conf_reg == conf_reg_ret); } int @@ -1257,12 +1344,12 @@ const int ERROR = -1; HMC5883 *g_dev_int; HMC5883 *g_dev_ext; -void hmc5883_usage(); void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); void info(int bus); int calibrate(int bus); +void usage(); /** * Start the driver. @@ -1511,10 +1598,8 @@ info(int bus) exit(0); } -} // namespace - void -hmc5883_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); warnx("options:"); @@ -1526,6 +1611,8 @@ hmc5883_usage() #endif } +} // namespace + int hmc5883_main(int argc, char *argv[]) { @@ -1551,7 +1638,7 @@ hmc5883_main(int argc, char *argv[]) calibrate = true; break; default: - hmc5883_usage(); + hmc5883::usage(); exit(0); } } From 67e3a904b6526f9268530d9c8e9529585c0be235 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:05:13 +0200 Subject: [PATCH 53/54] fix ms5611 code style for usage call --- src/drivers/ms5611/ms5611.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 7a8d2eecf3..fe669b5f52 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -781,6 +781,7 @@ void test(); void reset(); void info(); void calibrate(unsigned altitude); +void usage(); /** * MS5611 crc4 cribbed from the datasheet @@ -1057,16 +1058,16 @@ calibrate(unsigned altitude) exit(0); } -} // namespace - void -ms5611_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); warnx("options:"); warnx(" -X (external bus)"); } +} // namespace + int ms5611_main(int argc, char *argv[]) { @@ -1080,7 +1081,7 @@ ms5611_main(int argc, char *argv[]) external_bus = true; break; default: - ms5611_usage(); + ms5611::usage(); exit(0); } } From e6b5e3ae613e89189ac69cf0b174b10002d51068 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:22:36 +0200 Subject: [PATCH 54/54] Add note about need to scan external buses first --- ROMFS/px4fmu_common/init.d/rc.sensors | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 1e14930fe3..be54ea98b0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,6 +24,7 @@ fi if ver hwcmp PX4FMU_V2 then + # IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST if lsm303d start then echo "[init] Using LSM303D"