int MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { /* 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; } irqstate_t flags = px4_enter_critical_section(); if (!_gyro_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); case GYROIOCGSAMPLERATE: return _sample_rate; case GYROIOCSSAMPLERATE: _set_sample_rate(arg); return OK; case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: // set software filtering _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: /* XXX not implemented */ // XXX change these two values on set: // _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); case GYROIOCSELFTEST: return gyro_self_test(); #ifdef GYROIOCSHWLOWPASS case GYROIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef GYROIOCGHWLOWPASS case GYROIOCGHWLOWPASS: return _dlpf_freq; #endif default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCRESET: return reset(); case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ if (ticks < 1000) { return -EINVAL; } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); 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); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); _set_dlpf_filter(cutoff_freq_hz_gyro); _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; /* set call interval faster than 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 mpu9250 clock */ _call.period = _call_interval - MPU9250_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ if (want_start) { start(); } return OK; } } } case SENSORIOCGPOLLRATE: 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; } irqstate_t flags = px4_enter_critical_section(); if (!_accel_reports->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } px4_leave_critical_section(flags); return OK; } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); case ACCELIOCGSAMPLERATE: return _sample_rate; case ACCELIOCSSAMPLERATE: _set_sample_rate(arg); return OK; case ACCELIOCGLOWPASS: return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_calibration_s *s = (struct accel_calibration_s *) 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; } } case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: return set_accel_range(arg); case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2) / MPU9250_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); #ifdef ACCELIOCSHWLOWPASS case ACCELIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef ACCELIOCGHWLOWPASS case ACCELIOCGHWLOWPASS: return _dlpf_freq; #endif default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } }
int MPU9250::reset_mpu() { uint8_t retries; switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); write_checked_reg(MPUREG_PWR_MGMT_2, 0); usleep(1000); break; } // Enable I2C bus or Disable I2C bus (recommended on data sheet) write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); // Gyro scale 2000 deg/s () switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); break; } // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; set_accel_range(ACCEL_RANGE_G); // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); #else bool bypass = false; #endif /* INT: Clear on any read. * If this instance is for a device is on I2C bus the Mag will have an i2c interface * that it will use to access the either: a) the internal mag device on the internal I2C bus * or b) it could be used to access a downstream I2C devices connected to the chip on * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master * controller that chip provides as a SPI to I2C bridge. * so bypass is true if the mag has an i2c non null interfaces. */ write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); retries = 3; bool all_ok = false; while (!all_ok && retries--) { // Assume all checked values are as expected all_ok = true; uint8_t reg; uint8_t bankcheck = 0; for (uint8_t i = 0; i < _num_checked_registers; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries, REG_BANK(_checked_registers[i]), bankcheck); all_ok = false; } } } return all_ok ? OK : -EIO; }
int MPU9250::reset() { write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); up_udelay(1000); write_checked_reg(MPUREG_PWR_MGMT_2, 0); up_udelay(1000); // SAMPLE RATE _set_sample_rate(_sample_rate); usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; set_accel_range(16); usleep(1000); // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); #else bool bypass = false; #endif write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed usleep(1000); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); usleep(1000); uint8_t retries = 10; while (retries--) { bool all_ok = true; for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { if (read_reg(_checked_registers[i]) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); all_ok = false; } } if (all_ok) { break; } } return OK; }
int MPU9250::reset() { irqstate_t state; // Hold off sampling for 4 ms state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 10000; write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); write_checked_reg(MPUREG_PWR_MGMT_2, 0); px4_leave_critical_section(state); usleep(1000); // Enable I2C bus or Disable I2C bus (recommended on data sheet) write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); // Gyro scale 2000 deg/s () write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; set_accel_range(ACCEL_RANGE_G); // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); #else bool bypass = false; #endif /* INT: Clear on any read. * If this instance is for a device is on I2C bus the Mag will have an i2c interface * that it will use to access the either: a) the internal mag device on the internal I2C bus * or b) it could be used to access a downstream I2C devices connected to the chip on * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master * controller that chip provides as a SPI to I2C bridge. * so bypass is true if the mag has an i2c non null interfaces. */ write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); uint8_t retries = 3; bool all_ok = false; while (!all_ok && retries--) { // Assume all checked values are as expected all_ok = true; uint8_t reg; for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries); all_ok = false; } } } return all_ok ? OK : -EIO; }