diff options
-rw-r--r-- | makefiles/toolchain_gnu-arm-eabi.mk | 12 | ||||
-rw-r--r-- | src/drivers/drv_px4flow.h | 2 | ||||
-rw-r--r-- | src/drivers/drv_sensor.h | 15 | ||||
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 196 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 347 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 524 | ||||
-rw-r--r-- | src/drivers/px4flow/px4flow.cpp | 22 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 9 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_tests/module.mk | 4 | ||||
-rw-r--r-- | src/modules/sensors/sensor_params.c | 19 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 32 |
11 files changed, 864 insertions, 318 deletions
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7119c723b..d7e48f03b 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -109,10 +109,10 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections # enable precise stack overflow tracking # note - requires corresponding support in NuttX @@ -228,7 +228,7 @@ DEP_INCLUDES = $(subst .o,.d,$(OBJS)) define COMPILE @$(ECHO) "CC: $1" @$(MKDIR) -p $(dir $2) - $(Q) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 endef # Compile C++ source $1 to $2 @@ -237,7 +237,7 @@ endef define COMPILEXX @$(ECHO) "CXX: $1" @$(MKDIR) -p $(dir $2) - $(Q) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 endef # Assemble $1 into $2 diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index ab640837b..5aed3f02b 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file Rangefinder driver interface. + * @file PX4Flow driver interface. */ #ifndef _DRV_PX4FLOW_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 8e8b2c1b9..5e4598de8 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,8 +82,19 @@ #define SENSORIOCGQUEUEDEPTH _SENSORIOC(3) /** - * Reset the sensor to its default configuration. + * Reset the sensor to its default configuration */ #define SENSORIOCRESET _SENSORIOC(4) -#endif /* _DRV_SENSOR_H */
\ No newline at end of file +/** + * Set the sensor orientation + */ +#define SENSORIOCSROTATION _SENSORIOC(5) + +/** + * Get the sensor orientation + */ +#define SENSORIOCGROTATION _SENSORIOC(6) + +#endif /* _DRV_SENSOR_H */ + diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 82fa5cc6e..3a0c05c9e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -142,6 +142,7 @@ static const int ERROR = -1; #define ADDR_INT1_TSH_ZH 0x36 #define ADDR_INT1_TSH_ZL 0x37 #define ADDR_INT1_DURATION 0x38 +#define ADDR_LOW_ODR 0x39 /* Internal configuration values */ @@ -200,6 +201,9 @@ public: */ void print_info(); + // print register dump + void print_registers(); + protected: virtual int probe(); @@ -225,6 +229,9 @@ private: perf_counter_t _sample_perf; perf_counter_t _reschedules; perf_counter_t _errors; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -235,6 +242,14 @@ private: enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define L3GD20_NUM_CHECKED_REGISTERS 8 + static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -267,6 +282,11 @@ private: static void measure_trampoline(void *arg); /** + * check key registers for correct values + */ + void check_registers(void); + + /** * Fetch measurements from the sensor and update the report ring. */ void measure(); @@ -299,6 +319,14 @@ private: void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** + * Write a register in the L3GD20, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** * Set the L3GD20 measurement range. * * @param max_dps The measurement range is set to permit reading at least @@ -338,6 +366,19 @@ private: L3GD20 operator=(const L3GD20&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_FIFO_CTRL_REG, + ADDR_LOW_ODR }; + L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, @@ -355,11 +396,14 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), + _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")), + _register_wait(0), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _is_l3g4200d(false), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { // enable debug() calls _debug_enabled = true; @@ -389,6 +433,7 @@ L3GD20::~L3GD20() perf_free(_sample_perf); perf_free(_reschedules); perf_free(_errors); + perf_free(_bad_registers); } int @@ -448,29 +493,27 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); bool success = false; + uint8_t v = 0; - /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { - + /* verify that the device is attached and functioning, accept + * L3GD20, L3GD20H and L3G4200D */ + if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; - } - - - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; success = true; - } - - /* Detect the L3G4200D used on AeroCore */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { + /* Detect the L3G4200D used on AeroCore */ _is_l3g4200d = true; _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; } - if (success) + if (success) { + _checked_values[0] = v; return OK; + } return -EIO; } @@ -673,6 +716,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value) } void +L3GD20::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + + +void L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { uint8_t val; @@ -680,7 +735,7 @@ L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) val = read_reg(reg); val &= ~clearbits; val |= setbits; - write_reg(reg, val); + write_checked_reg(reg, val); } int @@ -714,7 +769,7 @@ L3GD20::set_range(unsigned max_dps) _gyro_range_rad_s = new_range / 180.0f * M_PI_F; _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; - write_reg(ADDR_CTRL_REG4, bits); + write_checked_reg(ADDR_CTRL_REG4, bits); return OK; } @@ -750,7 +805,7 @@ L3GD20::set_samplerate(unsigned frequency) return -EINVAL; } - write_reg(ADDR_CTRL_REG1, bits); + write_checked_reg(ADDR_CTRL_REG1, bits); return OK; } @@ -791,6 +846,11 @@ L3GD20::disable_i2c(void) uint8_t a = read_reg(0x05); write_reg(0x05, (0x20 | a)); if (read_reg(0x05) == (a | 0x20)) { + // this sets the I2C_DIS bit on the + // L3GD20H. The l3gd20 datasheet doesn't + // mention this register, but it does seem to + // accept it. + write_checked_reg(ADDR_LOW_ODR, 0x08); return; } } @@ -804,18 +864,18 @@ L3GD20::reset() 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 */ - write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ - write_reg(ADDR_CTRL_REG4, REG4_BDU); - write_reg(ADDR_CTRL_REG5, 0); - - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + write_checked_reg(ADDR_CTRL_REG1, + REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ + write_checked_reg(ADDR_CTRL_REG4, REG4_BDU); + write_checked_reg(ADDR_CTRL_REG5, 0); + write_checked_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ /* disable FIFO. This makes things simpler and ensures we * aren't getting stale data. It means we must run the hrt * callback fast enough to not miss data. */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); + write_checked_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); set_samplerate(0); // 760Hz or 800Hz set_range(L3GD20_DEFAULT_RANGE_DPS); @@ -840,6 +900,41 @@ L3GD20::measure_trampoline(void *arg) #endif void +L3GD20::check_registers(void) +{ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the WHO_AM_I, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 1 + if (_register_wait == 0) { + ::printf("L3GD20: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } + _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS; +} + +void L3GD20::measure() { #if L3GD20_USE_DRDY @@ -870,6 +965,8 @@ L3GD20::measure() /* start the performance counter */ perf_begin(_sample_perf); + check_registers(); + /* fetch data from the sensor */ memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; @@ -973,7 +1070,32 @@ L3GD20::print_info() perf_print_counter(_sample_perf); perf_print_counter(_reschedules); perf_print_counter(_errors); + perf_print_counter(_bad_registers); _reports->print_info("report queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } +} + +void +L3GD20::print_registers() +{ + printf("L3GD20 registers\n"); + for (uint8_t reg=0; reg<=0x40; reg++) { + uint8_t v = read_reg(reg); + printf("%02x:%02x ",(unsigned)reg, (unsigned)v); + if ((reg+1) % 16 == 0) { + printf("\n"); + } + } + printf("\n"); } int @@ -1011,6 +1133,7 @@ void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); +void regdump(); /** * Start the driver. @@ -1149,10 +1272,25 @@ info() exit(0); } +/** + * Dump the register information + */ +void +regdump(void) +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", g_dev); + g_dev->print_registers(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1209,5 +1347,11 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "info")) l3gd20::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) + l3gd20::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 01c89192a..dbd5c1f4c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -77,10 +77,6 @@ #endif static const int ERROR = -1; -// enable this to debug the buggy lsm303d sensor in very early -// prototype pixhawk boards -#define CHECK_EXTREMES 0 - /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -241,16 +237,6 @@ 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(); @@ -292,30 +278,25 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _reg1_resets; - perf_counter_t _reg7_resets; - perf_counter_t _extreme_values; perf_counter_t _accel_reschedules; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; - // expceted values of reg1 and reg7 to catch in-flight - // brownouts of the sensor - uint8_t _reg1_expected; - uint8_t _reg7_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; enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define LSM303D_NUM_CHECKED_REGISTERS 6 + static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -357,6 +338,11 @@ private: static void mag_measure_trampoline(void *arg); /** + * check key registers for correct values + */ + void check_registers(void); + + /** * Fetch accel measurements from the sensor and update the report ring. */ void measure(); @@ -408,6 +394,14 @@ private: void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** + * Write a register in the LSM303D, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** * Set the LSM303D accel measurement range. * * @param max_g The measurement range of the accel is in g (9.81m/s^2) @@ -468,6 +462,17 @@ private: LSM303D operator=(const LSM303D&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG5, + ADDR_CTRL_REG6, + ADDR_CTRL_REG7 }; + /** * Helper class implementing the mag driver node. */ @@ -528,23 +533,14 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _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_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), + _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), + _register_wait(0), _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), - _accel_log_fd(-1), - _accel_logging_enabled(false), - _last_extreme_us(0), - _last_log_us(0), - _last_log_sync_us(0), - _last_log_reg_us(0), - _last_log_alarm_us(0), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -586,9 +582,7 @@ 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); + perf_free(_bad_registers); perf_free(_accel_reschedules); } @@ -703,13 +697,12 @@ LSM303D::reset() 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); + write_checked_reg(ADDR_CTRL_REG1, + REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ - _reg7_expected = REG7_CONT_MODE_M; - write_reg(ADDR_CTRL_REG7, _reg7_expected); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_checked_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 @@ -739,126 +732,12 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - if (success) + if (success) { + _checked_values[0] = WHO_I_AM; return OK; - - 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; - static bool boot_ok = false; - 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; - } else { - boot_ok = 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 (_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 boot_ok=%u\r\n", - (unsigned long long)arb->timestamp, - (double)arb->x, (double)arb->y, (double)arb->z, - (int)arb->x_raw, - (int)arb->y_raw, - (int)arb->z_raw, - (unsigned)boot_ok); - } - - 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, - 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, 0x02, 0x15, ADDR_WHO_AM_I}; - uint8_t regval[sizeof(reglist)]; - for (uint8_t i=0; i<sizeof(reglist); i++) { - regval[i] = read_reg(reglist[i]); } - // log registers at 10Hz when we have extreme values, or 0.5 Hz without - if (_last_log_reg_us == 0 || - (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, "XREG %llu", (unsigned long long)hrt_absolute_time()); - for (uint8_t i=0; i<sizeof(reglist); i++) { - ::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]); - } - ::dprintf(_accel_log_fd, "\n"); - } - - // fsync at 0.1Hz - if (now - _last_log_sync_us > 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) { - uint8_t tone = 3; - if (!is_extreme) { - tone = 3; - } else if (boot_ok) { - tone = 4; - } else { - tone = 5; - } - ::ioctl(tfd, TONE_SET_ALARM, tone); - ::close(tfd); - } - } + return -EIO; } ssize_t @@ -879,9 +758,6 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { -#if CHECK_EXTREMES - check_extremes(arb); -#endif ret += sizeof(*arb); arb++; } @@ -1263,6 +1139,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value) } void +LSM303D::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + +void LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { uint8_t val; @@ -1270,7 +1157,7 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) val = read_reg(reg); val &= ~clearbits; val |= setbits; - write_reg(reg, val); + write_checked_reg(reg, val); } int @@ -1439,7 +1326,6 @@ LSM303D::accel_set_samplerate(unsigned frequency) } modify_reg(ADDR_CTRL_REG1, clearbits, setbits); - _reg1_expected = (_reg1_expected & ~clearbits) | setbits; return OK; } @@ -1515,6 +1401,41 @@ LSM303D::mag_measure_trampoline(void *arg) } void +LSM303D::check_registers(void) +{ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the WHO_AM_I, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 1 + if (_register_wait == 0) { + ::printf("LSM303D: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } + _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; +} + +void LSM303D::measure() { // if the accel doesn't have any data ready then re-schedule @@ -1522,16 +1443,13 @@ LSM303D::measure() // read a value and then miss the next value. // Note that DRDY is not available when the lsm303d is // connected on the external bus +#ifdef GPIO_EXTI_ACCEL_DRDY if (_bus == PX4_SPI_BUS_SENSORS && 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(); - return; - } +#endif /* status register and data as read back from the device */ @@ -1550,6 +1468,16 @@ LSM303D::measure() /* start the performance counter */ perf_begin(_accel_sample_perf); + check_registers(); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. + _register_wait--; + perf_end(_accel_sample_perf); + return; + } + /* 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; @@ -1572,7 +1500,12 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); - accel_report.error_count = 0; // not reported + + // report the error count as the sum of the number of bad bad + // register reads. This allows the higher level code to decide + // if it should use this sensor based on whether it has had + // failures + accel_report.error_count = perf_event_count(_bad_registers); accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; @@ -1611,12 +1544,6 @@ LSM303D::measure() void LSM303D::mag_measure() { - if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { - perf_count(_reg7_resets); - reset(); - return; - } - /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -1691,8 +1618,19 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_bad_registers); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } } void @@ -1750,20 +1688,6 @@ LSM303D::print_registers() for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) { printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name); } - printf("_reg1_expected=0x%02x\n", _reg1_expected); - 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) : @@ -1839,7 +1763,6 @@ void test(); void reset(); void info(); void regdump(); -void logging(); void usage(); /** @@ -2046,24 +1969,10 @@ regdump() exit(0); } -/** - * toggle logging - */ -void -logging() -{ - if (g_dev == nullptr) - errx(1, "driver not running\n"); - - g_dev->toggle_logging(); - - exit(0); -} - void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2126,11 +2035,5 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) lsm303d::regdump(); - /* - * dump device registers - */ - if (!strcmp(verb, "logging")) - lsm303d::logging(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'"); } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c9c27892f..e7e88bb82 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -113,6 +113,10 @@ #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 #define MPUREG_PRODUCT_ID 0x0C +#define MPUREG_TRIM1 0x0D +#define MPUREG_TRIM2 0x0E +#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BIT_SLEEP 0x40 @@ -121,6 +125,9 @@ #define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 #define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 @@ -196,6 +203,16 @@ public: void print_registers(); + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + + // deliberately cause a sensor error + void test_error(); + protected: virtual int probe(); @@ -231,7 +248,13 @@ private: perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; perf_counter_t _good_transfers; + perf_counter_t _reset_retries; + + uint8_t _register_wait; + uint64_t _reset_wait; + uint64_t _printf_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -242,6 +265,18 @@ private: enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define MPU6000_NUM_CHECKED_REGISTERS 9 + static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + + // use this to avoid processing measurements when in factory + // self test + volatile bool _in_factory_test; + /** * Start automatic measurement. */ @@ -257,7 +292,7 @@ private: * * Resets the chip and measurements ranges, but not scale and offset. */ - void reset(); + int reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -281,7 +316,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -304,6 +339,14 @@ private: void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** + * Write a register in the MPU6000, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** * Set the MPU6000 measurement range. * * @param max_g The maximum G value the range must support. @@ -347,11 +390,50 @@ private: */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* + check that key registers still have the right value + */ + void check_registers(void); + /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); + +#pragma pack(push, 1) + /** + * Report conversation within the MPU6000, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) }; +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_PWR_MGMT_1, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG }; + + + /** * Helper class implementing the gyro driver node. */ @@ -407,14 +489,21 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _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")), + _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), + _register_wait(0), + _reset_wait(0), + _printf_wait(0), _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), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _rotation(rotation) + _rotation(rotation), + _checked_next(0), + _in_factory_test(false) { // disable debug() calls _debug_enabled = false; @@ -460,6 +549,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_bad_registers); perf_free(_good_transfers); } @@ -486,7 +576,8 @@ MPU6000::init() if (_gyro_reports == nullptr) goto out; - reset(); + if (reset() != OK) + goto out; /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -571,27 +662,39 @@ out: return ret; } -void MPU6000::reset() +int MPU6000::reset() { // if the mpu6000 is initialised after the l3gd20 and lsm303d // then if we don't do an irqsave/irqrestore here the mpu6000 // frequenctly comes up in a bad state where all transfers // come as zero - irqstate_t state; - state = irqsave(); - - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - up_udelay(10000); - - // Wake up device and select GyroZ clock. Note that the - // MPU6000 starts up in sleep mode, and it can take some time - // for it to come out of sleep - write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - up_udelay(1000); + uint8_t tries = 5; + while (--tries != 0) { + irqstate_t state; + state = irqsave(); + + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + up_udelay(10000); + + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep + write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + up_udelay(1000); + + // Disable I2C bus (recommended on datasheet) + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); - // Disable I2C bus (recommended on datasheet) - write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - irqrestore(state); + if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { + break; + } + perf_count(_reset_retries); + usleep(2000); + } + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { + return -EIO; + } usleep(1000); @@ -605,7 +708,7 @@ void MPU6000::reset() _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); // correct gyro scale factors @@ -624,7 +727,7 @@ void MPU6000::reset() case MPU6000_REV_C5: // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); break; case MPU6000ES_REV_D6: @@ -639,7 +742,7 @@ void MPU6000::reset() // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) - write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); break; } @@ -651,15 +754,16 @@ void MPU6000::reset() usleep(1000); // INT CFG => Interrupt on Data Ready - write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); - write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); // Oscillator set // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); + return OK; } int @@ -684,6 +788,7 @@ MPU6000::probe() case MPU6000_REV_D9: case MPU6000_REV_D10: debug("ID 0x%02x", _product); + _checked_values[0] = _product; return OK; } @@ -700,7 +805,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; - write_reg(MPUREG_SMPLRT_DIV, div-1); + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); _sample_rate = 1000 / div; } @@ -734,7 +839,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } - write_reg(MPUREG_CONFIG, filter); + write_checked_reg(MPUREG_CONFIG, filter); } ssize_t @@ -833,6 +938,173 @@ MPU6000::gyro_self_test() return 0; } + +/* + perform a self-test comparison to factory trim values. This takes + about 200ms and will return OK if the current values are within 14% + of the expected values (as per datasheet) + */ +int +MPU6000::factory_self_test() +{ + _in_factory_test = true; + uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); + uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); + const uint16_t repeats = 100; + int ret = OK; + + // gyro self test has to be done at 250DPS + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); + + struct MPUReport mpu_report; + float accel_baseline[3]; + float gyro_baseline[3]; + float accel[3]; + float gyro[3]; + float accel_ftrim[3]; + float gyro_ftrim[3]; + + // get baseline values without self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + memset(accel_baseline, 0, sizeof(accel_baseline)); + memset(gyro_baseline, 0, sizeof(gyro_baseline)); + memset(accel, 0, sizeof(accel)); + memset(gyro, 0, sizeof(gyro)); + + for (uint8_t i=0; i<repeats; i++) { + up_udelay(1000); + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + + accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x); + accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y); + accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z); + gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x); + gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y); + gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z); + } + +#if 1 + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | + BITS_GYRO_ST_Z); + + // accel 8g, self-test enabled all axes + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0); +#endif + + up_udelay(20000); + + // get values with self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + + for (uint8_t i=0; i<repeats; i++) { + up_udelay(1000); + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + accel[0] += int16_t_from_bytes(mpu_report.accel_x); + accel[1] += int16_t_from_bytes(mpu_report.accel_y); + accel[2] += int16_t_from_bytes(mpu_report.accel_z); + gyro[0] += int16_t_from_bytes(mpu_report.gyro_x); + gyro[1] += int16_t_from_bytes(mpu_report.gyro_y); + gyro[2] += int16_t_from_bytes(mpu_report.gyro_z); + } + + for (uint8_t i=0; i<3; i++) { + accel_baseline[i] /= repeats; + gyro_baseline[i] /= repeats; + accel[i] /= repeats; + gyro[i] /= repeats; + } + + // extract factory trim values + uint8_t trims[4]; + trims[0] = read_reg(MPUREG_TRIM1); + trims[1] = read_reg(MPUREG_TRIM2); + trims[2] = read_reg(MPUREG_TRIM3); + trims[3] = read_reg(MPUREG_TRIM4); + uint8_t atrim[3]; + uint8_t gtrim[3]; + + atrim[0] = ((trims[0]>>3)&0x1C) | ((trims[3]>>4)&0x03); + atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); + atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + gtrim[0] = trims[0] & 0x1F; + gtrim[1] = trims[1] & 0x1F; + gtrim[2] = trims[2] & 0x1F; + + // convert factory trims to right units + for (uint8_t i=0; i<3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + } + // Y gyro trim is negative + gyro_ftrim[1] *= -1; + + for (uint8_t i=0; i<3; i++) { + float diff = accel[i]-accel_baseline[i]; + float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*accel_baseline[i]), + (int)(1000*accel[i]), + (int)(1000*diff), + (int)(1000*accel_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + for (uint8_t i=0; i<3; i++) { + float diff = gyro[i]-gyro_baseline[i]; + float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*gyro_baseline[i]), + (int)(1000*gyro[i]), + (int)(1000*(gyro[i]-gyro_baseline[i])), + (int)(1000*gyro_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + + write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); + + _in_factory_test = false; + if (ret == OK) { + ::printf("PASSED\n"); + } + + return ret; +} + + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +MPU6000::test_error() +{ + _in_factory_test = true; + // deliberately trigger an error. This was noticed during + // development as a handy way to test the reset logic + uint8_t data[16]; + memset(data, 0, sizeof(data)); + transfer(data, data, sizeof(data)); + ::printf("error triggered\n"); + print_registers(); + _in_factory_test = false; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -874,8 +1146,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCRESET: - reset(); - return OK; + return reset(); case SENSORIOCSPOLLRATE: { switch (arg) { @@ -1094,12 +1365,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) } uint8_t -MPU6000::read_reg(unsigned reg) +MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1144,6 +1415,17 @@ MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) write_reg(reg, val); } +void +MPU6000::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + int MPU6000::set_range(unsigned max_g) { @@ -1216,26 +1498,103 @@ MPU6000::measure_trampoline(void *arg) } void +MPU6000::check_registers(void) +{ + /* + we read the register at full speed, even though it isn't + listed as a high speed register. The low speed requirement + for some registers seems to be a propgation delay + requirement for changing sensor configuration, which should + not apply to reading a single register. It is also a better + test of SPI bus health to read at the same speed as we read + the data registers. + */ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the mpu6000 in a + // bizarre state where it has all correct + // register values but large offsets on the + // accel axes + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + _reset_wait = hrt_absolute_time() + 1000; + if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) { + /* + rather bizarrely this printf() seems + to be critical for successfully + resetting the sensor. If you take + this printf out then all the + registers do get successfully reset, + but the sensor ends up with a large + fixed offset on the + accelerometers. Try a up_udelay() + of various sizes instead of a + printf() doesn't work. That makes no + sense at all, and investigating why + this is would be worthwhile. + + The rate limit on the printf to 5Hz + prevents this from causing enough + delays in interrupt context to cause + the vehicle to not be able to fly + correctly. + */ + _printf_wait = hrt_absolute_time() + 200*1000UL; + ::printf("Setting %u %02x to %02x\n", + (unsigned)_checked_next, + (unsigned)_checked_registers[_checked_next], + (unsigned)_checked_values[_checked_next]); + } + } +#if 0 + if (_register_wait == 0) { + ::printf("MPU6000: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; +} + +void MPU6000::measure() { -#pragma pack(push, 1) - /** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - } mpu_report; -#pragma pack(pop) + if (_in_factory_test) { + // don't publish any data while in factory test mode + return; + } + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; int16_t accel_y; @@ -1254,6 +1613,8 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + check_registers(); + // sensor transfer at high clock speed set_frequency(MPU6000_HIGH_BUS_SPEED); @@ -1292,6 +1653,14 @@ MPU6000::measure() } perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } /* @@ -1321,7 +1690,12 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - grb.error_count = arb.error_count = 0; // not reported + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1411,9 +1785,21 @@ MPU6000::print_info() perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED); + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } } void @@ -1497,6 +1883,8 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void testerror(bool); +void factorytest(bool); void usage(); /** @@ -1688,10 +2076,40 @@ regdump(bool external_bus) exit(0); } +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->test_error(); + + exit(0); +} + +/** + * Dump the register information + */ +void +factorytest(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->factory_self_test(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1754,5 +2172,11 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) mpu6000::regdump(external_bus); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + if (!strcmp(verb, "factorytest")) + mpu6000::factorytest(external_bus); + + if (!strcmp(verb, "testerror")) + mpu6000::testerror(external_bus); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'"); } diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 7e3461ba1..9c9c1b0f8 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -62,6 +62,8 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> +#include <conversion/rotation.h> + #include <drivers/drv_hrt.h> #include <drivers/drv_px4flow.h> #include <drivers/device/ringbuffer.h> @@ -126,7 +128,7 @@ struct i2c_integral_frame f_integral; class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS, enum Rotation rotation = (enum Rotation)0); virtual ~PX4FLOW(); virtual int init(); @@ -155,6 +157,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + + enum Rotation _sensor_rotation; /** * Test whether the device supported by the driver is present at a @@ -200,7 +204,7 @@ private: */ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); -PX4FLOW::PX4FLOW(int bus, int address) : +PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */ _reports(nullptr), _sensor_ok(false), @@ -209,7 +213,8 @@ PX4FLOW::PX4FLOW(int bus, int address) : _px4flow_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), + _sensor_rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -362,6 +367,13 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGQUEUEDEPTH: return _reports->size(); + case SENSORIOCSROTATION: + _sensor_rotation = (enum Rotation)arg; + return OK; + + case SENSORIOCGROTATION: + return _sensor_rotation; + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; @@ -536,6 +548,10 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; + + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe..6f5adb5fe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -68,6 +68,8 @@ #include <mathlib/mathlib.h> +#include <conversion/rotation.h> + #include <systemlib/param/param.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> @@ -357,6 +359,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); + + enum Rotation flow_rot; + param_get(param_find("SENS_FLOW_ROT"),&flow_rot); struct optical_flow_s f; memset(&f, 0, sizeof(f)); @@ -374,6 +379,10 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 1cc28cce1..b46d2bd35 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -47,4 +47,6 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 229bfe3ce..3c78b7b17 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -262,6 +262,25 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * PX4Flow board rotation + * + * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * Zero rotation is defined as Y on flow board pointing towards front of vehicle + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); + +/** * Board rotation Y (Pitch) offset * * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd..d8df2c8ff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -63,6 +63,7 @@ #include <drivers/drv_rc_input.h> #include <drivers/drv_adc.h> #include <drivers/drv_airspeed.h> +#include <drivers/drv_px4flow.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -263,6 +264,7 @@ private: float diff_pres_analog_scale; int board_rotation; + int flow_rotation; int external_mag_rotation; float board_offset[3]; @@ -361,6 +363,7 @@ private: param_t battery_current_scaling; param_t board_rotation; + param_t flow_rotation; param_t external_mag_rotation; param_t board_offset[3]; @@ -614,6 +617,7 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ @@ -831,8 +835,22 @@ Sensors::parameters_update() } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + /* set px4flow rotation */ + int flowfd; + flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + if (flowret) { + warnx("flow rotation could not be set"); + close(flowfd); + return ERROR; + } + close(flowfd); + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -850,20 +868,20 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int fd; - fd = open(BARO_DEVICE_PATH, 0); - if (fd < 0) { + int barofd; + barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { warn("%s", BARO_DEVICE_PATH); errx(1, "FATAL: no barometer found"); } else { - int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); - if (ret) { + int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); - close(fd); + close(barofd); return ERROR; } - close(fd); + close(barofd); } return OK; |