diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4af1bec586..ab2eede349 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -348,7 +348,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED); + uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -403,7 +403,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -417,7 +417,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set low pass filter frequency @@ -435,8 +435,8 @@ private: void check_registers(void); /* do not allow to copy this class due to pointer data members */ - MPU6000(const MPU6000&); - MPU6000 operator=(const MPU6000&); + MPU6000(const MPU6000 &); + MPU6000 operator=(const MPU6000 &); #pragma pack(push, 1) /** @@ -465,11 +465,12 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_PWR_MGMT_1, MPUREG_USER_CTRL, MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; + MPUREG_INT_PIN_CFG + }; @@ -499,8 +500,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - MPU6000_gyro(const MPU6000_gyro&); - MPU6000_gyro operator=(const MPU6000_gyro&); + MPU6000_gyro(const MPU6000_gyro &); + MPU6000_gyro operator=(const MPU6000_gyro &); }; /** driver 'main' command */ @@ -588,13 +589,17 @@ MPU6000::~MPU6000() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -623,15 +628,20 @@ MPU6000::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + + if (_accel_reports == nullptr) { goto out; + } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_gyro_reports == nullptr) - goto out; - if (reset() != OK) + if (_gyro_reports == nullptr) { goto out; + } + + if (reset() != OK) { + goto out; + } /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -651,6 +661,7 @@ MPU6000::init() /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -667,7 +678,7 @@ MPU6000::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -679,7 +690,7 @@ MPU6000::init() _gyro_reports->get(&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); + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); @@ -696,6 +707,7 @@ int MPU6000::reset() // frequenctly comes up in a bad state where all transfers // come as zero uint8_t tries = 5; + while (--tries != 0) { irqstate_t state; state = irqsave(); @@ -716,9 +728,11 @@ int MPU6000::reset() 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; } @@ -768,6 +782,7 @@ MPU6000::probe() { uint8_t whoami; whoami = read_reg(MPUREG_WHOAMI); + if (whoami != MPU_WHOAMI_6000) { DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; @@ -807,15 +822,18 @@ void MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) { if (desired_sample_rate_hz == 0 || - desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; } @@ -830,25 +848,34 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) /* choose next highest filter frequency available */ - if (frequency_hz == 0) { + if (frequency_hz == 0) { filter = BITS_DLPF_CFG_2100HZ_NOLPF; - } else if (frequency_hz <= 5) { + + } else if (frequency_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 42) { filter = BITS_DLPF_CFG_42HZ; + } else if (frequency_hz <= 98) { filter = BITS_DLPF_CFG_98HZ; + } else if (frequency_hz <= 188) { filter = BITS_DLPF_CFG_188HZ; + } else if (frequency_hz <= 256) { filter = BITS_DLPF_CFG_256HZ_NOLPF2; + } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } + write_checked_reg(MPUREG_CONFIG, filter); } @@ -858,8 +885,9 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -868,17 +896,21 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + 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; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -901,24 +933,34 @@ MPU6000::self_test() int MPU6000::accel_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -926,8 +968,9 @@ MPU6000::accel_self_test() int MPU6000::gyro_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -947,27 +990,35 @@ MPU6000::gyro_self_test() 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) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* 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)) { + (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; } @@ -1003,14 +1054,14 @@ MPU6000::factory_self_test() float gyro_ftrim[3]; // get baseline values without self-test enabled - set_frequency(MPU6000_HIGH_BUS_SPEED); + 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>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); + 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); + 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]; + 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)(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]; + + 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)(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; @@ -1118,6 +1173,7 @@ MPU6000::factory_self_test() write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); _in_factory_test = false; + if (ret == OK) { ::printf("PASSED\n"); } @@ -1149,8 +1205,9 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -1159,17 +1216,21 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + 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; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -1189,27 +1250,27 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -1218,12 +1279,13 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -1240,17 +1302,18 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - /* - set call interval faster then the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu6000 clock - */ - _call.period = _call_interval - MPU6000_TIMER_REDUCTION; + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu6000 clock + */ + _call.period = _call_interval - MPU6000_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1258,25 +1321,29 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* 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; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -1300,14 +1367,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -1322,7 +1390,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / MPU6000_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -1338,26 +1406,29 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_gyro_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -1371,6 +1442,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); + case GYROIOCSLOWPASS: // set hardware filtering _set_dlpf_filter(arg); @@ -1395,6 +1467,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -1412,8 +1485,8 @@ 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(speed); + // general register transfer at low clock speed + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1425,8 +1498,8 @@ MPU6000::read_reg16(unsigned reg) { uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -1441,8 +1514,8 @@ 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); + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, nullptr, sizeof(cmd)); } @@ -1462,7 +1535,8 @@ void MPU6000::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1525,9 +1602,9 @@ MPU6000::start() /* start polling at the specified rate */ hrt_call_every(&_call, - 1000, - _call_interval-MPU6000_TIMER_REDUCTION, - (hrt_callout)&MPU6000::measure_trampoline, this); + 1000, + _call_interval - MPU6000_TIMER_REDUCTION, + (hrt_callout)&MPU6000::measure_trampoline, this); } void @@ -1565,7 +1642,8 @@ 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 @@ -1591,7 +1669,8 @@ MPU6000::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; + _checked_next = 0; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); // waiting 3ms between register writes seems @@ -1599,9 +1678,11 @@ MPU6000::check_registers(void) // recovering considerably _reset_wait = hrt_absolute_time() + 3000; } + _register_wait = 20; } - _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; + + _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; } void @@ -1618,6 +1699,7 @@ MPU6000::measure() } struct MPUReport mpu_report; + struct Report { int16_t accel_x; int16_t accel_y; @@ -1636,13 +1718,14 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - set_frequency(MPU6000_HIGH_BUS_SPEED); + // 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))) + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; + } - check_registers(); + check_registers(); /* see if this is duplicate accelerometer data. Note that we @@ -1652,13 +1735,14 @@ MPU6000::measure() sampled at 8kHz, so we would incorrectly think we have new data when we are in fact getting duplicate accelerometer data. */ - if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { // it isn't new data - wait for next timer perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; return; } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); _got_duplicate = false; @@ -1686,10 +1770,10 @@ MPU6000::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } @@ -1736,7 +1820,7 @@ MPU6000::measure() // 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); + 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. @@ -1866,16 +1950,19 @@ MPU6000::print_info() perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1982,51 +2074,59 @@ void start(bool external_bus, enum Rotation rotation, int range) { int fd; - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ + { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif + } else { *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - if (ioctl(fd, ACCELIOCSRANGE, range) < 0) + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -2035,14 +2135,17 @@ fail: void stop(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ warnx("already stopped."); } + exit(0); } @@ -2054,8 +2157,8 @@ stop(bool external_bus) void test(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; @@ -2070,12 +2173,14 @@ test(bool external_bus) /* get the driver */ int fd_gyro = open(path_gyro, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", path_gyro); + } /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); @@ -2117,8 +2222,9 @@ test(bool external_bus) warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); /* reset to default polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); + } close(fd); close(fd_gyro); @@ -2135,19 +2241,22 @@ test(bool external_bus) void reset(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; int fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } - close(fd); + close(fd); exit(0); } @@ -2158,9 +2267,11 @@ reset(bool external_bus) void info(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_info(); @@ -2174,9 +2285,11 @@ info(bool external_bus) void regdump(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_registers(); @@ -2190,9 +2303,11 @@ regdump(bool external_bus) void testerror(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + 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(); @@ -2205,9 +2320,11 @@ testerror(bool external_bus) void factorytest(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + 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(); @@ -2240,12 +2357,15 @@ mpu6000_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': accel_range = atoi(optarg); break; + default: mpu6000::usage(); exit(0);