void MPU9250::write_reg(unsigned reg, uint8_t value) { // general register transfer at low clock speed if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(reg)); _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); } else { _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); } }
uint8_t MPU9250::read_reg(unsigned reg, uint32_t speed) { uint8_t buf; if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(reg)); _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); } else { _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); } return buf; }
uint16_t MPU9250::read_reg16(unsigned reg) { uint8_t buf[2]; // general register transfer at low clock speed if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(reg)); _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); } else { _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); } return (uint16_t)(buf[0] << 8) | buf[1]; }
uint8_t MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count) { uint8_t ret; if (buf == NULL) { return PX4_ERROR; } if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(start_reg)); ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); } else { ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); } return ret; }
int MPU9250::probe() { int ret = PX4_ERROR; // Try first for mpu9250/6500 _whoami = read_reg(MPUREG_WHOAMI); // If it's not an MPU it must be an ICM if ((_whoami != MPU_WHOAMI_9250) && (_whoami != MPU_WHOAMI_6500)) { // Make sure selected register bank is bank 0 (which contains WHOAMI) select_register_bank(REG_BANK(ICMREG_20948_WHOAMI)); _whoami = read_reg(ICMREG_20948_WHOAMI); } if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { _num_checked_registers = MPU9250_NUM_CHECKED_REGISTERS; _checked_registers = _mpu9250_checked_registers; memset(_checked_values, 0, MPU9250_NUM_CHECKED_REGISTERS); memset(_checked_bad, 0, MPU9250_NUM_CHECKED_REGISTERS); ret = PX4_OK; } else if (_whoami == ICM_WHOAMI_20948) { _num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS; _checked_registers = _icm20948_checked_registers; memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS); memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS); ret = PX4_OK; } _checked_values[0] = _whoami; _checked_bad[0] = _whoami; if (ret != PX4_OK) { PX4_DEBUG("unexpected whoami 0x%02x", _whoami); } return ret; }
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; }
void MPU9250::measure() { if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; } struct MPUReport mpu_report; struct ICMReport icm_report; struct Report { int16_t accel_x; int16_t accel_y; int16_t accel_z; int16_t temp; int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; } report; /* start measuring */ perf_begin(_sample_perf); /* * Fetch the full set of measurements from the MPU9250 in one pass */ if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) { if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) { perf_end(_sample_perf); return; } } else { // ICM20948 select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report, sizeof(icm_report))) { perf_end(_sample_perf); return; } } check_registers(); if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) { return; } } /* * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else, * try to read a magnetometer report. */ # ifdef USE_I2C if (_mag->is_passthrough()) { # endif _mag->_measure(mpu_report.mag); # ifdef USE_I2C } else { _mag->measure(); } # endif /* * Continue evaluating gyro and accelerometer results */ if (!_magnetometer_only && _register_wait == 0) { /* * Convert from big to little endian */ if (_whoami == ICM_WHOAMI_20948) { report.accel_x = int16_t_from_bytes(icm_report.accel_x); report.accel_y = int16_t_from_bytes(icm_report.accel_y); report.accel_z = int16_t_from_bytes(icm_report.accel_z); report.temp = int16_t_from_bytes(icm_report.temp); report.gyro_x = int16_t_from_bytes(icm_report.gyro_x); report.gyro_y = int16_t_from_bytes(icm_report.gyro_y); report.gyro_z = int16_t_from_bytes(icm_report.gyro_z); } else { // MPU9250/MPU6500 report.accel_x = int16_t_from_bytes(mpu_report.accel_x); report.accel_y = int16_t_from_bytes(mpu_report.accel_y); report.accel_z = int16_t_from_bytes(mpu_report.accel_z); report.temp = int16_t_from_bytes(mpu_report.temp); report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); } if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) { return; } } 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; } /* * Get sensor temperature */ _last_temperature = (report.temp) / 333.87f + 21.0f; /* * Convert and publish accelerometer and gyrometer data. */ if (!_magnetometer_only) { /* * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation */ if (_whoami != ICM_WHOAMI_20948) { /* * Swap axes and negate y */ int16_t accel_xt = report.accel_y; int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); int16_t gyro_xt = report.gyro_y; int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); /* * Apply the swap */ report.accel_x = accel_xt; report.accel_y = accel_yt; report.gyro_x = gyro_xt; report.gyro_y = gyro_yt; } /* * Report buffers. */ sensor_accel_s arb; sensor_gyro_s grb; /* * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); // 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. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ /* NOTE: Axes have been swapped to match the board a few lines above. */ arb.x_raw = report.accel_x; arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; float xraw_f = report.accel_x; float yraw_f = report.accel_y; float zraw_f = report.accel_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((zraw_f * _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); matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); arb.y_integral = aval_integrated(1); arb.z_integral = aval_integrated(2); arb.scaling = _accel_range_scale; arb.temperature = _last_temperature; /* return device ID */ arb.device_id = _accel->_device_id.devid; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; xraw_f = report.gyro_x; yraw_f = report.gyro_y; zraw_f = report.gyro_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((zraw_f * _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); matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); grb.y_integral = gval_integrated(1); grb.z_integral = gval_integrated(2); grb.scaling = _gyro_range_scale; grb.temperature = _last_temperature; /* return device ID */ grb.device_id = _gyro->_device_id.devid; _accel_reports->force(&arb); _gyro_reports->force(&grb); /* notify anyone waiting for data */ if (accel_notify) { _accel->poll_notify(POLLIN); } if (gyro_notify) { _gyro->parent_poll_notify(); } if (accel_notify && !(_accel->_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (gyro_notify && !(_gyro->_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } } /* stop measuring */ perf_end(_sample_perf); }