diff options
Diffstat (limited to 'src/drivers/mpu6000/mpu6000.cpp')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 155 |
1 files changed, 89 insertions, 66 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea9..e322e8b3a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -175,6 +175,12 @@ #define MPU6000_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + /* the MPU6000 can only handle high SPI bus speeds on the sensor and interrupt status registers. All other registers have a maximum 1MHz @@ -234,7 +240,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -361,6 +367,13 @@ private: uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** * Measurement self test * * @return 0 on success, 1 on failure @@ -433,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_INT_ENABLE, MPUREG_INT_PIN_CFG }; - + /** * Helper class implementing the gyro driver node. @@ -457,7 +470,7 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; - orb_id_t _gyro_orb_id; + int _gyro_orb_class_instance; int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ @@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_scale{}, @@ -510,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev // disable debug() calls _debug_enabled = false; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000; + // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -596,6 +615,7 @@ MPU6000::init() _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; + /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); /* if probe/setup failed, bail now */ @@ -613,22 +633,8 @@ MPU6000::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic < 0) { warnx("ADVERT FAIL"); @@ -639,22 +645,8 @@ MPU6000::init() struct gyro_report grp; _gyro_reports->get(&grp); - switch (_gyro->_gyro_class_instance) { - case CLASS_DEVICE_PRIMARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); - break; - - } - - _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic < 0) { warnx("ADVERT FAIL"); @@ -683,7 +675,7 @@ int MPU6000::reset() // 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); @@ -741,7 +733,7 @@ int MPU6000::reset() case MPU6000_REV_D9: case MPU6000_REV_D10: // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug + // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); @@ -819,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - /* + /* choose next highest filter frequency available */ if (frequency_hz == 0) { @@ -921,26 +913,50 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ + const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ return 1; + } 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% @@ -958,7 +974,7 @@ MPU6000::factory_self_test() // gyro self test has to be done at 250DPS write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - struct MPUReport mpu_report; + struct MPUReport mpu_report; float accel_baseline[3]; float gyro_baseline[3]; float accel[3]; @@ -988,10 +1004,10 @@ MPU6000::factory_self_test() } #if 1 - write_reg(MPUREG_GYRO_CONFIG, - BITS_FS_250DPS | - BITS_GYRO_ST_X | - BITS_GYRO_ST_Y | + 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 @@ -1061,7 +1077,7 @@ MPU6000::factory_self_test() ::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]; @@ -1076,7 +1092,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); @@ -1223,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_accel_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -1512,13 +1528,13 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + 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. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1631,7 +1647,7 @@ MPU6000::measure() _register_wait--; return; } - + /* * Swap axes and negate y @@ -1692,7 +1708,7 @@ MPU6000::measure() float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - + arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); @@ -1713,7 +1729,7 @@ MPU6000::measure() float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - + grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); @@ -1739,12 +1755,12 @@ MPU6000::measure() perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (!(_pub_blocked)) { /* publish it */ - orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1767,7 +1783,7 @@ MPU6000::print_info() 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", + ::printf("reg %02x:%02x should be %02x\n", (unsigned)_checked_registers[i], (unsigned)v, (unsigned)_checked_values[i]); @@ -1794,7 +1810,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), - _gyro_orb_id(nullptr), + _gyro_orb_class_instance(-1), _gyro_class_instance(-1) { } @@ -1839,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen) int MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { - return _parent->gyro_ioctl(filp, cmd, arg); + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } } /** |