From 93f3398dfedd787419f54294adc6c92c30e282b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 10:06:43 +1100 Subject: [PATCH 01/48] lsm303d: added 'lsm303d regdump' command useful for diagnosing issues --- src/drivers/lsm303d/lsm303d.cpp | 53 ++++++++++++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 60601e22c8..c21a255223 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -201,6 +201,11 @@ public: */ void print_info(); + /** + * dump register values + */ + void print_registers(); + protected: virtual int probe(); @@ -1380,6 +1385,30 @@ LSM303D::print_info() _mag_reports->print_info("mag reports"); } +void +LSM303D::print_registers() +{ + const struct { + uint8_t reg; + const char *name; + } regmap[] = { + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + }; + for (uint8_t i=0; iprint_registers(); + + exit(0); +} + } // namespace @@ -1634,5 +1679,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "info")) lsm303d::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "regdump")) + lsm303d::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From af049f7cf8b261aaa85a659537d75c1476da40f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:17:53 +1100 Subject: [PATCH 02/48] lsm303d: define some more register addresses --- src/drivers/lsm303d/lsm303d.cpp | 51 ++++++++++++++++++++++++--------- 1 file changed, 38 insertions(+), 13 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c21a255223..de227974ec 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -82,19 +82,24 @@ static const int ERROR = -1; /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 - -#define ADDR_OUT_L_T 0x05 -#define ADDR_OUT_H_T 0x06 -#define ADDR_STATUS_M 0x07 -#define ADDR_OUT_X_L_M 0x08 -#define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x0A -#define ADDR_OUT_Y_H_M 0x0B -#define ADDR_OUT_Z_L_M 0x0C -#define ADDR_OUT_Z_H_M 0x0D - -#define ADDR_OUT_TEMP_A 0x26 +#define WHO_I_AM 0x49 + +#define ADDR_OUT_TEMP_L 0x05 +#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_INT_CTRL_M 0x12 +#define ADDR_INT_SRC_M 0x13 +#define ADDR_REFERENCE_X 0x1c +#define ADDR_REFERENCE_Y 0x1d +#define ADDR_REFERENCE_Z 0x1e + #define ADDR_STATUS_A 0x27 #define ADDR_OUT_X_L_A 0x28 #define ADDR_OUT_X_H_A 0x29 @@ -112,6 +117,26 @@ static const int ERROR = -1; #define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 +#define ADDR_FIFO_CTRL 0x2e +#define ADDR_FIFO_SRC 0x2f + +#define ADDR_IG_CFG1 0x30 +#define ADDR_IG_SRC1 0x31 +#define ADDR_IG_THS1 0x32 +#define ADDR_IG_DUR1 0x33 +#define ADDR_IG_CFG2 0x34 +#define ADDR_IG_SRC2 0x35 +#define ADDR_IG_THS2 0x36 +#define ADDR_IG_DUR2 0x37 +#define ADDR_CLICK_CFG 0x38 +#define ADDR_CLICK_SRC 0x39 +#define ADDR_CLICK_THS 0x3a +#define ADDR_TIME_LIMIT 0x3b +#define ADDR_TIME_LATENCY 0x3c +#define ADDR_TIME_WINDOW 0x3d +#define ADDR_ACT_THS 0x3e +#define ADDR_ACT_DUR 0x3f + #define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) From 415417196bcafdafa6aafa8109487ed0ed18cab6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 18:12:50 +1100 Subject: [PATCH 03/48] lsm303d: print more registers in "lsm303d regdump" --- src/drivers/lsm303d/lsm303d.cpp | 55 ++++++++++++++++++++++++++------- 1 file changed, 44 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index de227974ec..91f1fe0059 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1417,21 +1417,54 @@ LSM303D::print_registers() uint8_t reg; const char *name; } regmap[] = { - { ADDR_WHO_AM_I, "WHO_AM_I" }, - { ADDR_STATUS_A, "STATUS_A" }, - { ADDR_STATUS_M, "STATUS_M" }, - { ADDR_CTRL_REG0, "CTRL_REG0" }, - { ADDR_CTRL_REG1, "CTRL_REG1" }, - { ADDR_CTRL_REG2, "CTRL_REG2" }, - { ADDR_CTRL_REG3, "CTRL_REG3" }, - { ADDR_CTRL_REG4, "CTRL_REG4" }, - { ADDR_CTRL_REG5, "CTRL_REG5" }, - { ADDR_CTRL_REG6, "CTRL_REG6" }, - { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_OUT_TEMP_L, "TEMP_L" }, + { ADDR_OUT_TEMP_H, "TEMP_H" }, + { ADDR_INT_CTRL_M, "INT_CTRL_M" }, + { ADDR_INT_SRC_M, "INT_SRC_M" }, + { ADDR_REFERENCE_X, "REFERENCE_X" }, + { ADDR_REFERENCE_Y, "REFERENCE_Y" }, + { ADDR_REFERENCE_Z, "REFERENCE_Z" }, + { ADDR_OUT_X_L_A, "ACCEL_XL" }, + { ADDR_OUT_X_H_A, "ACCEL_XH" }, + { ADDR_OUT_Y_L_A, "ACCEL_YL" }, + { ADDR_OUT_Y_H_A, "ACCEL_YH" }, + { ADDR_OUT_Z_L_A, "ACCEL_ZL" }, + { ADDR_OUT_Z_H_A, "ACCEL_ZH" }, + { ADDR_FIFO_CTRL, "FIFO_CTRL" }, + { ADDR_FIFO_SRC, "FIFO_SRC" }, + { ADDR_IG_CFG1, "IG_CFG1" }, + { ADDR_IG_SRC1, "IG_SRC1" }, + { ADDR_IG_THS1, "IG_THS1" }, + { ADDR_IG_DUR1, "IG_DUR1" }, + { ADDR_IG_CFG2, "IG_CFG2" }, + { ADDR_IG_SRC2, "IG_SRC2" }, + { ADDR_IG_THS2, "IG_THS2" }, + { ADDR_IG_DUR2, "IG_DUR2" }, + { ADDR_CLICK_CFG, "CLICK_CFG" }, + { ADDR_CLICK_SRC, "CLICK_SRC" }, + { ADDR_CLICK_THS, "CLICK_THS" }, + { ADDR_TIME_LIMIT, "TIME_LIMIT" }, + { ADDR_TIME_LATENCY,"TIME_LATENCY" }, + { ADDR_TIME_WINDOW, "TIME_WINDOW" }, + { ADDR_ACT_THS, "ACT_THS" }, + { ADDR_ACT_DUR, "ACT_DUR" } }; for (uint8_t i=0; i Date: Wed, 30 Oct 2013 09:45:58 +1100 Subject: [PATCH 04/48] SPI: added set_frequency() API this allows the bus speed to be changed on the fly by device drivers. This is needed for the MPU6000 --- src/drivers/device/spi.cpp | 6 ++++++ src/drivers/device/spi.h | 11 +++++++++++ 2 files changed, 17 insertions(+) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fa6b78d640..fed6c312c1 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) return OK; } +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9103dca2ea..299575dc5c 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Set the SPI bus frequency + * This is used to change frequency on the fly. Some sensors + * (such as the MPU6000) need a lower frequency for setup + * registers and can handle higher frequency for sensor + * value registers + * + * @param frequency Frequency to set (Hz) + */ + void set_frequency(uint32_t frequency); + /** * Locking modes supported by the driver. */ From 97af3d22040e67520a835102684a1b2a9575aaaa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:46:52 +1100 Subject: [PATCH 05/48] mpu6000: change bus speed based on registers being accessed this ensures we follow the datasheet requirement of 1MHz for general registers and up to 20MHz for sensor and int status registers --- src/drivers/mpu6000/mpu6000.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 70359110ea..6bfa583fb0 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -161,6 +161,14 @@ #define MPU6000_ONE_G 9.80665f +/* + the MPU6000 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define MPU6000_LOW_BUS_SPEED 1000*1000 +#define MPU6000_HIGH_BUS_SPEED 10*1000*1000 + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -351,7 +359,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), + SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -991,6 +999,9 @@ MPU6000::read_reg(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; @@ -1003,6 +1014,9 @@ MPU6000::read_reg16(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; @@ -1016,6 +1030,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, nullptr, sizeof(cmd)); } @@ -1139,6 +1156,10 @@ MPU6000::measure() * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + // sensor transfer at high clock speed + set_frequency(MPU6000_HIGH_BUS_SPEED); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; From 50d5241985792d829833f287f2fc1936d8caef84 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 15:27:48 +1100 Subject: [PATCH 06/48] px4io: moved blue heartbeat LED to main loop this allows us to tell if the main loop is running by looking for a blinking blue LED --- src/modules/px4iofirmware/px4io.c | 13 +++++++++++++ src/modules/px4iofirmware/safety.c | 16 +--------------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index ff9eecd74f..cd9bd197ba 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -117,6 +117,13 @@ show_debug_messages(void) } } +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + int user_start(int argc, char *argv[]) { @@ -201,6 +208,7 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ @@ -216,6 +224,11 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); + if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 95335f038a..cdb54a80ad 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -77,7 +77,6 @@ static unsigned blink_counter = 0; static bool safety_button_pressed; static void safety_check_button(void *arg); -static void heartbeat_blink(void *arg); static void failsafe_blink(void *arg); void @@ -86,9 +85,6 @@ safety_init(void) /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); - /* arrange for the heartbeat handler to be called at 4Hz */ - hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); - /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -163,16 +159,6 @@ safety_check_button(void *arg) } } -static void -heartbeat_blink(void *arg) -{ - static bool heartbeat = false; - - /* XXX add flags here that need to be frobbed by various loops */ - - LED_BLUE(heartbeat = !heartbeat); -} - static void failsafe_blink(void *arg) { @@ -192,4 +178,4 @@ failsafe_blink(void *arg) } LED_AMBER(failsafe); -} \ No newline at end of file +} From cdaafff6e45b2d68d20248fd00ec157b47ff6f78 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 10:19:35 +1100 Subject: [PATCH 07/48] lsm303d: added detailed logging of accels on extremes this will log accel values and registers to /fs/microsd/lsm303d.log if any extreme values are seen --- src/drivers/lsm303d/lsm303d.cpp | 165 +++++++++++++++++++++++++++++++- 1 file changed, 163 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 91f1fe0059..e7869527cf 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,7 @@ #include #include #include +#include #include #include @@ -231,6 +233,16 @@ public: */ void print_registers(); + /** + * toggle logging + */ + void toggle_logging(); + + /** + * check for extreme accel values + */ + void check_extremes(const accel_report *arb); + protected: virtual int probe(); @@ -273,6 +285,7 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _extreme_values; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -283,6 +296,15 @@ private: uint8_t _reg7_expected; uint8_t _reg1_expected; + // accel logging + int _accel_log_fd; + bool _accel_logging_enabled; + uint64_t _last_extreme_us; + uint64_t _last_log_us; + uint64_t _last_log_sync_us; + uint64_t _last_log_reg_us; + uint64_t _last_log_alarm_us; + /** * Start automatic measurement. */ @@ -473,11 +495,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), + _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _reg1_expected(0), - _reg7_expected(0) + _reg7_expected(0), + _accel_log_fd(-1), + _accel_logging_enabled(false), + _last_log_us(0), + _last_log_sync_us(0), + _last_log_reg_us(0), + _last_log_alarm_us(0) { // enable debug() calls _debug_enabled = true; @@ -514,6 +543,9 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); + perf_free(_reg1_resets); + perf_free(_reg7_resets); + perf_free(_extreme_values); } int @@ -602,6 +634,101 @@ LSM303D::probe() return -EIO; } +#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" + +/** + check for extreme accelerometer values and log to a file on the SD card + */ +void +LSM303D::check_extremes(const accel_report *arb) +{ + const float extreme_threshold = 30; + bool is_extreme = (fabsf(arb->x) > extreme_threshold && + fabsf(arb->y) > extreme_threshold && + fabsf(arb->z) > extreme_threshold); + if (is_extreme) { + perf_count(_extreme_values); + // force accel logging on if we see extreme values + _accel_logging_enabled = true; + } + + if (! _accel_logging_enabled) { + // logging has been disabled by user, close + if (_accel_log_fd != -1) { + ::close(_accel_log_fd); + _accel_log_fd = -1; + } + return; + } + if (_accel_log_fd == -1) { + // keep last 10 logs + ::unlink(ACCEL_LOGFILE ".9"); + for (uint8_t i=8; i>0; i--) { + uint8_t len = strlen(ACCEL_LOGFILE)+3; + char log1[len], log2[len]; + snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); + snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); + ::rename(log1, log2); + } + ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); + + // open the new logfile + _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); + if (_accel_log_fd == -1) { + return; + } + } + + uint64_t now = hrt_absolute_time(); + // log accels at 1Hz + if (now - _last_log_us > 1000*1000) { + _last_log_us = now; + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + (unsigned long long)arb->timestamp, + arb->x, arb->y, arb->z, + (int)arb->x_raw, + (int)arb->y_raw, + (int)arb->z_raw); + } + + // log registers at 10Hz when we have extreme values, or 0.5 Hz without + if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + (now - _last_log_reg_us > 10*1000*1000)) { + _last_log_reg_us = now; + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR }; + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + for (uint8_t i=0; i 10*1000*1000) { + _last_log_sync_us = now; + ::fsync(_accel_log_fd); + } + + // play alarm every 10s if we have had an extreme value + if (perf_event_count(_extreme_values) != 0 && + (now - _last_log_alarm_us > 10*1000*1000)) { + _last_log_alarm_us = now; + int tfd = ::open(TONEALARM_DEVICE_PATH, 0); + if (tfd != -1) { + ::ioctl(tfd, TONE_SET_ALARM, 4); + ::close(tfd); + } + } +} + ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { @@ -620,6 +747,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { + check_extremes(arb); ret += sizeof(*arb); arb++; } @@ -1467,6 +1595,18 @@ LSM303D::print_registers() printf("_reg7_expected=0x%02x\n", _reg7_expected); } +void +LSM303D::toggle_logging() +{ + if (! _accel_logging_enabled) { + _accel_logging_enabled = true; + printf("Started logging to %s\n", ACCEL_LOGFILE); + } else { + _accel_logging_enabled = false; + printf("Stopped logging\n"); + } +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", MAG_DEVICE_PATH), _parent(parent) @@ -1520,6 +1660,7 @@ void test(); void reset(); void info(); void regdump(); +void logging(); /** * Start the driver. @@ -1706,6 +1847,20 @@ regdump() exit(0); } +/** + * toggle logging + */ +void +logging() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->toggle_logging(); + + exit(0); +} + } // namespace @@ -1743,5 +1898,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "logging")) + lsm303d::logging(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); } From 671447ce2c94ad523ea9dc636c5066db98831d87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 11:12:08 +1100 Subject: [PATCH 08/48] lsm303d: fixed TEMP_H register define --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e7869527cf..bbf70c1541 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -87,7 +87,7 @@ static const int ERROR = -1; #define WHO_I_AM 0x49 #define ADDR_OUT_TEMP_L 0x05 -#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_OUT_TEMP_H 0x06 #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 From 5ef91d694b94c297bfecae297b2c933561e0ac16 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 13:39:59 +1100 Subject: [PATCH 09/48] lsm303d: log mag regs too --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index bbf70c1541..c20c820aa9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -703,7 +703,9 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR }; + ADDR_ACT_THS, ADDR_ACT_DUR, + ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Fri, 15 Nov 2013 14:12:24 +1100 Subject: [PATCH 10/48] lsm303d: always log first ARB and REG values --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c20c820aa9..6d7b3df500 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -681,7 +681,8 @@ LSM303D::check_extremes(const accel_report *arb) uint64_t now = hrt_absolute_time(); // log accels at 1Hz - if (now - _last_log_us > 1000*1000) { + if (_last_log_us == 0 || + now - _last_log_us > 1000*1000) { _last_log_us = now; ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", (unsigned long long)arb->timestamp, @@ -692,7 +693,8 @@ LSM303D::check_extremes(const accel_report *arb) } // log registers at 10Hz when we have extreme values, or 0.5 Hz without - if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + if (_last_log_reg_us == 0 || + (is_extreme && (now - _last_log_reg_us > 500*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, From ea33a19c8f5b109fd9ba35603b0af56dddef3708 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Nov 2013 18:36:48 +1100 Subject: [PATCH 11/48] lsm303d: show regs at both high and low bus speed on error --- src/drivers/lsm303d/lsm303d.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6d7b3df500..2d150cdba5 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -708,11 +708,20 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_ACT_THS, ADDR_ACT_DUR, ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; - ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "FREG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Sat, 16 Nov 2013 18:37:26 +1100 Subject: [PATCH 12/48] lsm303d: zero-fill register reads --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2d150cdba5..bfa1293641 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1123,6 +1123,7 @@ LSM303D::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); From b927974a976a86271826a04f0020be01007368ad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Nov 2013 20:56:15 +1100 Subject: [PATCH 13/48] FMUv2: added support for MPU6000 on v2.4 board --- src/drivers/boards/px4fmu-v2/board_config.h | 2 ++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ec8dde4993..534394274c 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -85,11 +85,13 @@ __BEGIN_DECLS #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 /* 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 09838d02fe..2527e4c143 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, * required for some peripheral @@ -81,6 +82,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); #endif #ifdef CONFIG_STM32_SPI2 @@ -99,6 +101,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_ACCEL_MAG: @@ -106,6 +109,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_BARO: @@ -113,6 +117,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: From 3ce14497a118fc6a10c475704dfa5dbdd9669476 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:28:42 +1100 Subject: [PATCH 14/48] FMUv2: added define for MPU DRDY pin --- src/drivers/boards/px4fmu-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 534394274c..9f66d195d7 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -79,6 +79,7 @@ __BEGIN_DECLS #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) #define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) #define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) From 44b2543d2d968a61b2b7cbef6d4409dcdf5c9174 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:02 +1100 Subject: [PATCH 15/48] FMUv2: set MPU6000 CS as initially de-selected --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index ae2a645f70..31ad60bd34 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\n"); From 9a169d8ef453aea1729b76940b10733591c4c6d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:33 +1100 Subject: [PATCH 16/48] l3gd20: added I2C disable based on method from ST engineering support --- src/drivers/l3gd20/l3gd20.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 8f56748234..31e38fbd9a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -218,6 +218,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -574,6 +579,7 @@ L3GD20::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); @@ -699,9 +705,19 @@ L3GD20::stop() hrt_cancel(&_call); } +void +L3GD20::disable_i2c(void) +{ + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); +} + void L3GD20::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ @@ -753,6 +769,7 @@ L3GD20::measure() perf_begin(_sample_perf); /* fetch data from the sensor */ + memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); From 7c9d92a5d64ee4b2282139a49e189df434ec00f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:30:04 +1100 Subject: [PATCH 17/48] lsm303d: added I2C disable based on method from ST engineering support --- src/drivers/lsm303d/lsm303d.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index bfa1293641..da981989c4 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -322,6 +322,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -595,9 +600,25 @@ out: return ret; } +void +LSM303D::disable_i2c(void) +{ + uint8_t a = read_reg(0x02); + write_reg(0x02, (0x10 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xF7 & a)); + a = read_reg(0x15); + write_reg(0x15, (0x80 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xE7 & a)); +} + void LSM303D::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* enable accel*/ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; write_reg(ADDR_CTRL_REG1, _reg1_expected); From a2b31118cb4dc01174c4c3436c29d5850d116441 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:30:48 +1100 Subject: [PATCH 18/48] lsm303d: get cleaner logic traces by gathering all regs more regularly --- src/drivers/lsm303d/lsm303d.cpp | 42 +++++++++++++++------------------ 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index da981989c4..0c0aa0ff52 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -713,36 +713,32 @@ LSM303D::check_extremes(const accel_report *arb) (int)arb->z_raw); } + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR, + ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; + uint8_t regval[sizeof(reglist)]; + for (uint8_t i=0; i 500*1000)) || + (is_extreme && (now - _last_log_reg_us > 250*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; - const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, - ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, - ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, - ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, - ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, - ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, - ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR, - ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; - ::dprintf(_accel_log_fd, "FREG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Thu, 28 Nov 2013 20:31:28 +1100 Subject: [PATCH 19/48] lsm303d: cleanup logic traces by pre-zeroing all transfers --- 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 0c0aa0ff52..576bc184e1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1437,6 +1437,7 @@ LSM303D::measure() perf_begin(_accel_sample_perf); /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); @@ -1514,6 +1515,7 @@ LSM303D::mag_measure() perf_begin(_mag_sample_perf); /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); From aeba9e5c1e59b016b6ce707852490714db0544e2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:31:51 +1100 Subject: [PATCH 20/48] FMUv2: change CS pins to 2MHz this gives cleaner traces --- src/drivers/boards/px4fmu-v2/board_config.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 9f66d195d7..586661b58f 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -82,11 +82,11 @@ __BEGIN_DECLS #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) -#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 From 51a1ad48c5734f259bc8c90b2b5f5626b83cbdbb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:33:32 +1100 Subject: [PATCH 21/48] FMUv2: don't config ADC pins that are now used for MPU6k CS and other uses --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 31ad60bd34..269ec238eb 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ From c46ab017e12f5ccbe0f22ec77f90d8a8e28a8c63 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 12:06:05 +1100 Subject: [PATCH 22/48] lsm303d: make log distinctive with i2c disable included --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 576bc184e1..5c9a87eaa8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -734,7 +734,7 @@ LSM303D::check_extremes(const accel_report *arb) (is_extreme && (now - _last_log_reg_us > 250*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; - ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Wed, 4 Dec 2013 15:50:41 +1100 Subject: [PATCH 23/48] lsm303d: changed tones for accel fail to 3 tones distinct tones for init fail, post-boot fail and recovery --- src/drivers/lsm303d/lsm303d.cpp | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 5c9a87eaa8..44cb469128 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -664,6 +664,7 @@ void LSM303D::check_extremes(const accel_report *arb) { const float extreme_threshold = 30; + static bool boot_ok = false; bool is_extreme = (fabsf(arb->x) > extreme_threshold && fabsf(arb->y) > extreme_threshold && fabsf(arb->z) > extreme_threshold); @@ -671,7 +672,9 @@ LSM303D::check_extremes(const accel_report *arb) perf_count(_extreme_values); // force accel logging on if we see extreme values _accel_logging_enabled = true; - } + } else { + boot_ok = true; + } if (! _accel_logging_enabled) { // logging has been disabled by user, close @@ -705,15 +708,16 @@ LSM303D::check_extremes(const accel_report *arb) if (_last_log_us == 0 || now - _last_log_us > 1000*1000) { _last_log_us = now; - ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", (unsigned long long)arb->timestamp, arb->x, arb->y, arb->z, (int)arb->x_raw, (int)arb->y_raw, - (int)arb->z_raw); + (int)arb->z_raw, + (unsigned)boot_ok); } - const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, @@ -723,7 +727,7 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, ADDR_ACT_THS, ADDR_ACT_DUR, ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I}; uint8_t regval[sizeof(reglist)]; for (uint8_t i=0; i Date: Fri, 1 Nov 2013 08:11:58 +1100 Subject: [PATCH 24/48] lsm303d: init filter to 773 Hz --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 44cb469128..a38524c220 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -631,7 +631,7 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); From 0a83772c0d8b4b600245590d571f679a6894e75f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 16:12:25 +1100 Subject: [PATCH 25/48] l3gd20: use highest possible on-chip filter bandwidth this allows the software filter to do its job properly --- src/drivers/l3gd20/l3gd20.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 31e38fbd9a..103b26ac58 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -92,9 +92,12 @@ static const int ERROR = -1; #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -659,16 +662,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_25HZ; + bits |= RATE_190HZ_LP_70HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_20HZ; + bits |= RATE_380HZ_LP_100HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_30HZ; - + bits |= RATE_760HZ_LP_100HZ; } else { return -EINVAL; } @@ -732,7 +734,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(L3GD20_DEFAULT_RATE); + set_samplerate(0); // 760Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); From 24a243843ef771273c81536c0bd18c5d70c010f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 22:54:02 +1100 Subject: [PATCH 26/48] lsm303d/l3gd20: change filters to 50Hz analog on-chip filters after discussion with Leonard these analog on-chip filters should be at 50Hz --- src/drivers/l3gd20/l3gd20.cpp | 11 ++++++++--- src/drivers/lsm303d/lsm303d.cpp | 7 ++++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 103b26ac58..17b53bfa96 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -93,10 +93,15 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 @@ -662,15 +667,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_70HZ; + bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_100HZ; + bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_100HZ; + bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a38524c220..701947b981 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -631,7 +631,12 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz + + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); From 513d014f03831111a1b9f9af293e253f7d5218a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Dec 2013 09:06:53 +1100 Subject: [PATCH 27/48] l3gd20: added retries to disable_i2c() --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 17b53bfa96..176ef648bb 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -715,8 +715,16 @@ L3GD20::stop() void L3GD20::disable_i2c(void) { - uint8_t a = read_reg(0x05); - write_reg(0x05, (0x20 | a)); + uint8_t retries = 10; + while (retries--) { + // add retries + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); + if (read_reg(0x05) == (a | 0x20)) { + return; + } + } + debug("FAILED TO DISABLE I2C"); } void From 4956feffdfc8459c5f27e471b50978db29f23c07 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:00 +1100 Subject: [PATCH 28/48] drv_hrt: added hrt_call_init() and hrt_call_delay() APIs hrt_call_init() can be used to initialise (zero) a hrt_call structure to ensure safe usage. The hrt_call_every() interface calls this automatically. hrt_call_delay() can be used to delay a current callout by the given number of microseconds --- src/drivers/drv_hrt.h | 14 ++++++++++++++ src/drivers/stm32/drv_hrt.c | 19 ++++++++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8a99eeca79..d130d68b34 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry); */ __EXPORT extern void hrt_cancel(struct hrt_call *entry); +/* + * initialise a hrt_call structure + */ +__EXPORT extern void hrt_call_init(struct hrt_call *entry); + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); + /* * Initialise the HRT. */ diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index e79d7e10a3..dc28f446b4 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,6 +720,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { + hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -839,7 +840,12 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { - call->deadline = deadline + call->period; + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } hrt_call_enter(call); } } @@ -906,5 +912,16 @@ hrt_latency_update(void) latency_counters[index]++; } +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} #endif /* HRT_TIMER */ From 0e97c288bbe5acde6358e10237d1bfb5e522ee19 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:37 +1100 Subject: [PATCH 29/48] px4fmu2: enable SPI sensor DRDY pins --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 2527e4c143..c66c490a7f 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -83,6 +83,11 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + stm32_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI2 From 30ff61fa90179af00609738da6bd5365872d4f61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:52:31 +1100 Subject: [PATCH 30/48] lsm303d: use DRDY pins to automatically reschedule measurements this prevents double reads of sensor data, and missing samples from the accel --- src/drivers/lsm303d/lsm303d.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 701947b981..6010a2ad73 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -286,6 +286,7 @@ private: perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; perf_counter_t _extreme_values; + perf_counter_t _accel_reschedules; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -501,6 +502,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), + _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -627,6 +629,8 @@ LSM303D::reset() _reg7_expected = REG7_CONT_MODE_M; write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); @@ -1430,6 +1434,14 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + // if the accel 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_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + return; + } if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { perf_count(_reg1_resets); reset(); From 7e309414758d2c526da7ef3bcab7bf75779f6950 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:53:25 +1100 Subject: [PATCH 31/48] ms5611: change bus speed to 5MHz this gives 5MHz SPI bus speed (by asking for 6MHz due to timer granularity). Tests with a logic analyser show that the ms5611 is actually more reliable at 5MHz than lower speeds --- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e547c913ba..33e5be940d 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000), _prom(prom_buf) { } From bc6ddb971fa23b88679ea8201a8205be3c08e90c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:54:58 +1100 Subject: [PATCH 32/48] ms5611: removed unused variable --- src/drivers/ms5611/ms5611_spi.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 33e5be940d..8dd89120d2 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,7 +134,6 @@ int MS5611_SPI::init() { int ret; - irqstate_t flags; ret = SPI::init(); if (ret != OK) { From 0456ee2364600ba6b5a9f109c7464a71579e7d58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:55:10 +1100 Subject: [PATCH 33/48] ms5611: give cleaner SPI traces this makes logic traces cleaner by zeroing extra bytes written --- src/drivers/ms5611/ms5611_spi.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 8dd89120d2..e9dff5a8bc 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -166,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[4]; + uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 }; /* read the most recent measurement */ - buf[0] = 0 | DIR_WRITE; int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { @@ -240,18 +239,21 @@ MS5611_SPI::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + if (ret != OK) { + debug("crc failed"); + } + return ret; } uint16_t MS5611_SPI::_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; _transfer(cmd, cmd, sizeof(cmd)); From 3d27dd7246c660bb0a00caae3cd0b0e66bfa6467 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:34:25 +0100 Subject: [PATCH 34/48] Made all usual suspects default to their custom names and only register the default name if its not already taken by someone else --- src/drivers/hmc5883/hmc5883.cpp | 7 ++++--- src/drivers/l3gd20/l3gd20.cpp | 7 +++++++ src/drivers/lsm303d/lsm303d.cpp | 33 ++++++++++++++++++++++++--------- src/drivers/mpu6000/mpu6000.cpp | 33 +++++++++++++++++++++++++-------- 4 files changed, 60 insertions(+), 20 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5f0ce4ff86..22ad301ff3 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,6 +77,7 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 +#define HMC5883L_DEVICE_PATH "/dev/hmc5883" /* 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 */ @@ -315,7 +316,7 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : - I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1256,7 +1257,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -1288,7 +1289,7 @@ test() ssize_t sz; int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 176ef648bb..a27bc52803 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -365,6 +365,13 @@ L3GD20::init() if (_reports == nullptr) goto out; + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + /* advertise sensor topic */ struct gyro_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6010a2ad73..a3409cbac1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,7 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) - +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" +#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -585,6 +586,20 @@ LSM303D::init() reset(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); + + if (mag_ret == OK) { + log("default mag device"); + } + /* advertise mag topic */ struct mag_report zero_mag_report; memset(&zero_mag_report, 0, sizeof(zero_mag_report)); @@ -1670,7 +1685,7 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), + CDev("LSM303D_mag", "/dev/lsm303d_mag"), _parent(parent) { } @@ -1736,7 +1751,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1747,7 +1762,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1755,7 +1770,7 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); /* don't fail if open cannot be opened */ if (0 <= fd_mag) { @@ -1790,10 +1805,10 @@ test() int ret; /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); @@ -1819,10 +1834,10 @@ test() struct mag_report m_report; /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6bfa583fb0..ce00107521 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -75,6 +75,9 @@ #define DIR_READ 0x80 #define DIR_WRITE 0x00 +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" + // MPU 6000 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 @@ -359,7 +362,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -468,6 +471,20 @@ MPU6000::init() /* fetch an initial set of measurements for advertisement */ measure(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + if (gyro_ret != OK) { _gyro_topic = -1; } else { @@ -1290,7 +1307,7 @@ MPU6000::print_info() } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", GYRO_DEVICE_PATH), + CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent) { } @@ -1352,7 +1369,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1384,17 +1401,17 @@ test() ssize_t sz; /* get the driver */ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + MPU_DEVICE_PATH_ACCEL); /* get the driver */ - int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1452,7 +1469,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); From 1fb406ba094a091c6976f752c3b6db71fc895605 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:44:29 +0100 Subject: [PATCH 35/48] Add also default descriptor for alternate sensors --- src/drivers/lsm303d/lsm303d.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a3409cbac1..94966a9637 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -586,11 +586,21 @@ LSM303D::init() reset(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ + /* register the first instance as plain name, the 2nd as two and counting */ ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default accel device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* try to claim the generic accel node as well - it's OK if we fail at this */ @@ -598,6 +608,16 @@ LSM303D::init() if (mag_ret == OK) { log("default mag device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* advertise mag topic */ From f24479c27a3627d315eee0f329da8aca8741eebe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:39:07 +1100 Subject: [PATCH 36/48] lsm303d: dump I2C control registers in regdump --- 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 94966a9637..c535c4e871 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1643,6 +1643,8 @@ LSM303D::print_registers() const char *name; } regmap[] = { { ADDR_WHO_AM_I, "WHO_AM_I" }, + { 0x02, "I2C_CONTROL1" }, + { 0x15, "I2C_CONTROL2" }, { ADDR_STATUS_A, "STATUS_A" }, { ADDR_STATUS_M, "STATUS_M" }, { ADDR_CTRL_REG0, "CTRL_REG0" }, From 6145e69fc62c7448baf0531d5c492e26b9225b01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:43:54 +1100 Subject: [PATCH 37/48] device: added register_class_devname() API this allows drivers to register generic device names for a device class, with automatic class instance handling --- src/drivers/device/cdev.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/drivers/device/device.h | 22 ++++++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 47ebcd40a7..7954ce5ab5 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -108,6 +108,42 @@ CDev::~CDev() unregister_driver(_devname); } +int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + int class_instance = 0; + int ret = -ENOSPC; + while (class_instance < 4) { + if (class_instance == 0) { + ret = register_driver(class_devname, &fops, 0666, (void *)this); + if (ret == OK) break; + } else { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + ret = register_driver(name, &fops, 0666, (void *)this); + if (ret == OK) break; + } + class_instance++; + } + if (class_instance == 4) + return ret; + return class_instance; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + if (class_instance > 0) { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + return unregister_driver(name); + } + return unregister_driver(class_devname); +} + int CDev::init() { diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index a9ed5d77c2..0235f62844 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -396,6 +396,25 @@ protected: */ virtual int close_last(struct file *filp); + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + private: static const unsigned _max_pollwaiters = 8; @@ -488,4 +507,7 @@ private: } // namespace device +// class instance for primary driver of each class +#define CLASS_DEVICE_PRIMARY 0 + #endif /* _DEVICE_DEVICE_H */ From c5097a6561928e9d5e88926e5be28df845256d91 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:44:58 +1100 Subject: [PATCH 38/48] hmc5883: use register_class_devname() --- src/drivers/hmc5883/hmc5883.cpp | 35 ++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22ad301ff3..d3b99ae66e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -155,6 +155,7 @@ private: float _range_scale; float _range_ga; bool _collect_phase; + int _class_instance; orb_advert_t _mag_topic; @@ -322,6 +323,7 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _mag_topic(-1), + _class_instance(-1), _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")), @@ -352,6 +354,9 @@ HMC5883::~HMC5883() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -375,13 +380,17 @@ HMC5883::init() /* reset the device configuration */ reset(); - /* get a publish handle on the mag topic */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + _class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the mag topic if we are + * the primary mag */ + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } ret = OK; /* sensor is ok, but not calibrated */ @@ -876,8 +885,10 @@ HMC5883::collect() } #endif - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } /* post a report to the ring */ if (_reports->force(&new_report)) { @@ -1292,7 +1303,7 @@ test() int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1389,10 +1400,10 @@ int calibrate() { int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1414,7 +1425,7 @@ int calibrate() void reset() { - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); From 1d5f0a1433f838fc553461c808129a89d917058a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:10 +1100 Subject: [PATCH 39/48] l3gd20: use register_class_devname() --- src/drivers/l3gd20/l3gd20.cpp | 37 +++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a27bc52803..78a086b110 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,8 @@ #include #include +#define L3GD20_DEVICE_PATH "/dev/l3gd20" + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -199,6 +201,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + int _class_instance; unsigned _current_rate; unsigned _orientation; @@ -317,6 +320,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), @@ -346,6 +350,9 @@ L3GD20::~L3GD20() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _class_instance); + /* delete the perf counter */ perf_free(_sample_perf); } @@ -365,17 +372,13 @@ L3GD20::init() if (_reports == nullptr) goto out; - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + _class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic */ + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + } reset(); @@ -743,7 +746,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -922,7 +925,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; @@ -931,7 +934,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(GYRO_DEVICE_PATH, O_RDONLY); + fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -963,10 +966,10 @@ test() ssize_t sz; /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", L3GD20_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -999,7 +1002,7 @@ test() void reset() { - int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); From 02e7f7fa85ec4d443e32cd320d80a4152712a22c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:28 +1100 Subject: [PATCH 40/48] lsm303d: use register_class_devname() --- src/drivers/lsm303d/lsm303d.cpp | 121 ++++++++++++++++---------------- 1 file changed, 62 insertions(+), 59 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c535c4e871..beac3c7510 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,8 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -277,7 +277,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_advert_t _mag_topic; + int _class_instance; unsigned _accel_read; unsigned _mag_read; @@ -467,6 +467,8 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class LSM303D; @@ -474,6 +476,9 @@ protected: private: LSM303D *_parent; + orb_advert_t _mag_topic; + int _mag_class_instance; + void measure(); void measure_trampoline(void *arg); @@ -495,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _mag_topic(-1), + _class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -546,6 +551,9 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; + if (_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + delete _mag; /* delete the perf counter */ @@ -575,10 +583,6 @@ LSM303D::init() goto out; /* advertise accel topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -586,53 +590,22 @@ LSM303D::init() reset(); - /* register the first instance as plain name, the 2nd as two and counting */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); - - if (mag_ret == OK) { - log("default mag device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); + /* do CDev init for the mag device node */ + ret = _mag->init(); + if (ret != OK) { + warnx("MAG init failed"); + goto out; } - /* advertise mag topic */ - struct mag_report zero_mag_report; - memset(&zero_mag_report, 0, sizeof(zero_mag_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); - - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); - - if (mag_ret != OK) { - _mag_topic = -1; + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary accel device, so advertise to + // the ORB + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); } - ret = OK; out: return ret; } @@ -1544,8 +1517,10 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (_accel_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } _accel_read++; @@ -1616,8 +1591,10 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + if (_mag->_mag_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } _mag_read++; @@ -1707,13 +1684,39 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", "/dev/lsm303d_mag"), - _parent(parent) + CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(-1), + _mag_class_instance(-1) { } LSM303D_mag::~LSM303D_mag() { + if (_mag_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance); +} + +int +LSM303D_mag::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) + goto out; + + _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary mag device, so advertise to + // the ORB + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + } + +out: + return ret; } void @@ -1773,7 +1776,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1892,7 +1895,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1903,7 +1906,7 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); From 1fc122562c16dfd419e832b95292678a7db8ec22 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:39 +1100 Subject: [PATCH 41/48] mpu6000: use register_class_devname() --- src/drivers/mpu6000/mpu6000.cpp | 114 ++++++++++++++++++++------------ 1 file changed, 71 insertions(+), 43 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ce00107521..6145a51175 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -211,16 +211,17 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _accel_class_instance; RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - unsigned _reads; unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; perf_counter_t _sample_perf; math::LowPassFilter2p _accel_filter_x; @@ -349,12 +350,17 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class MPU6000; void parent_poll_notify(); + private: MPU6000 *_parent; + orb_advert_t _gyro_topic; + int _gyro_class_instance; }; @@ -370,12 +376,13 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _reads(0), _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -420,8 +427,13 @@ MPU6000::~MPU6000() if (_gyro_reports != nullptr) delete _gyro_reports; + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); + /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); } int @@ -466,38 +478,23 @@ MPU6000::init() _gyro_scale.z_scale = 1.0f; /* do CDev init for the gyro device node, keep it optional */ - gyro_ret = _gyro->init(); + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } /* fetch an initial set of measurements for advertisement */ measure(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - if (gyro_ret != OK) { - _gyro_topic = -1; - } else { - gyro_report gr; - _gyro_reports->get(&gr); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } - - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + } out: return ret; @@ -677,6 +674,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) if (_accel_reports->empty()) return -EAGAIN; + perf_count(_accel_reads); + /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; @@ -694,12 +693,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) int MPU6000::self_test() { - if (_reads == 0) { + if (perf_event_count(_sample_perf) == 0) { measure(); } /* return 0 on success, 1 else */ - return (_reads > 0) ? 0 : 1; + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; } int @@ -771,6 +770,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) if (_gyro_reports->empty()) return -EAGAIN; + perf_count(_gyro_reads); + /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; @@ -1180,9 +1181,6 @@ MPU6000::measure() if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; - /* count measurement */ - _reads++; - /* * Convert from big to little endian */ @@ -1287,10 +1285,11 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + if (_accel_topic != -1) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + if (_gyro->_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1301,19 +1300,48 @@ void MPU6000::print_info() { perf_print_counter(_sample_perf); - printf("reads: %u\n", _reads); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), - _parent(parent) + _parent(parent), + _gyro_class_instance(-1) { } MPU6000_gyro::~MPU6000_gyro() { + if (_gyro_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance); +} + +int +MPU6000_gyro::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + gyro_report gr; + memset(&gr, 0, sizeof(gr)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + +out: + return ret; } void From 09ece4306e929b9ffbd7d21a50c5d3d21265bd87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:31 +1100 Subject: [PATCH 42/48] l3gd20: close fds before exit --- src/drivers/l3gd20/l3gd20.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 78a086b110..910131c6a0 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -942,6 +942,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -990,6 +992,8 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + close(fd_gyro); + /* XXX add poll-rate tests here too */ reset(); @@ -1013,6 +1017,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + exit(0); } From acd0a70dca1066e09fc98ed6576682b6de330e7c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:43 +1100 Subject: [PATCH 43/48] lsm303d: close fds before exit --- src/drivers/lsm303d/lsm303d.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index beac3c7510..11e5b95a7e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1804,6 +1804,8 @@ start() } } + close(fd); + close(fd_mag); exit(0); fail: @@ -1885,6 +1887,9 @@ test() /* XXX add poll-rate tests here too */ + close(fd_accel); + close(fd_mag); + reset(); errx(0, "PASS"); } @@ -1906,6 +1911,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { @@ -1916,6 +1923,8 @@ reset() err(1, "mag pollrate reset failed"); } + close(fd); + exit(0); } From f0d84d4826a6563693ae0abd6ac1f72a3eafdc68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:54 +1100 Subject: [PATCH 44/48] mpu6000: close fds before exit --- src/drivers/mpu6000/mpu6000.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6145a51175..ff921274b8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1405,6 +1405,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -1508,6 +1510,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); + close(fd); + exit(0); } From 96881d88106c421a4ea44118b754f90d47b94e4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Dec 2013 10:03:50 +1100 Subject: [PATCH 45/48] ms5611: check for all zero in the prom when SPI CLK fails we get all zero data --- src/drivers/ms5611/ms5611_spi.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e9dff5a8bc..bc4074c55e 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -236,9 +236,12 @@ MS5611_SPI::_read_prom() usleep(3000); /* read and convert PROM words */ + bool all_zero = true; for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + if (_prom.c[i] != 0) + all_zero = false; //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } @@ -247,6 +250,10 @@ MS5611_SPI::_read_prom() if (ret != OK) { debug("crc failed"); } + if (all_zero) { + debug("prom all zero"); + ret = -EIO; + } return ret; } From 91870953d951bc0e15640363bcc1d826cb5ed1b1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 09:13:38 +1100 Subject: [PATCH 46/48] mpu6000: treat all zero data from mpu6k as bad --- src/drivers/mpu6000/mpu6000.cpp | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ff921274b8..c95d11c83b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -223,6 +223,7 @@ private: perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -384,6 +385,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -434,6 +436,7 @@ MPU6000::~MPU6000() perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); + perf_free(_bad_transfers); } int @@ -1013,9 +1016,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) uint8_t MPU6000::read_reg(unsigned reg) { - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1028,9 +1029,7 @@ MPU6000::read_reg(unsigned reg) uint16_t MPU6000::read_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1195,6 +1194,20 @@ MPU6000::measure() report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + return; + } + + /* * Swap axes and negate y */ From d43e3394b07b5999dff5d04a3dd77f96d76c1134 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 14:03:16 +1100 Subject: [PATCH 47/48] l3gd20: added rescheduling and error checking --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 910131c6a0..1077eb43b1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -209,6 +209,8 @@ private: unsigned _read; perf_counter_t _sample_perf; + perf_counter_t _reschedules; + perf_counter_t _errors; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -325,6 +327,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), + _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _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) @@ -355,6 +359,8 @@ L3GD20::~L3GD20() /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_reschedules); + perf_free(_errors); } int @@ -746,7 +752,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -776,6 +782,17 @@ L3GD20::measure_trampoline(void *arg) void L3GD20::measure() { +#ifdef GPIO_EXTI_GYRO_DRDY + // 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) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + return; + } +#endif + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -798,6 +815,16 @@ L3GD20::measure() raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +#ifdef GPIO_EXTI_GYRO_DRDY + if (raw_report.status & 0xF != 0xF) { + /* + we waited for DRDY, but did not see DRDY on all axes + when we captured. That means a transfer error of some sort + */ + perf_count(_errors); + return; + } +#endif /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) From cf78440ee6cb8c1469f53ca096d7388524facd59 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 15:09:36 +1100 Subject: [PATCH 48/48] drv_hrt: added note on why an uninitialised hrt_call is safe --- src/drivers/stm32/drv_hrt.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index dc28f446b4..1bd251bc2a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,7 +720,6 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { - hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -734,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue);