diff --git a/lib/mpu9250/MPU9250.cpp b/lib/mpu9250/MPU9250.cpp index a9a6b2d..878506c 100644 --- a/lib/mpu9250/MPU9250.cpp +++ b/lib/mpu9250/MPU9250.cpp @@ -82,19 +82,19 @@ bool MPU9250_Base::testConnection() { * the MPU-6000, which does not have a VLOGIC pin. * @return I2C supply voltage level (0=VLOGIC, 1=VDD) */ -uint8_t MPU9250_Base::getAuxVDDIOLevel() { - I2Cdev::readBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, buffer); - return buffer[0]; -} +// uint8_t MPU9250_Base::getAuxVDDIOLevel() { +// I2Cdev::readBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, buffer); +// return buffer[0]; +// } /** Set the auxiliary I2C supply voltage level. * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to * the MPU-6000, which does not have a VLOGIC pin. * @param level I2C supply voltage level (0=VLOGIC, 1=VDD) */ -void MPU9250_Base::setAuxVDDIOLevel(uint8_t level) { - I2Cdev::writeBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, level); -} +// void MPU9250_Base::setAuxVDDIOLevel(uint8_t level) { +// I2Cdev::writeBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, level); +// } // SMPLRT_DIV register @@ -2786,30 +2786,58 @@ void MPU9250_Base::setZFineGain(int8_t gain) { // XA_OFFS_* registers int16_t MPU9250_Base::getXAccelOffset() { - I2Cdev::readBytes(devAddr, MPU9250_RA_XA_OFFS_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; + I2Cdev::readByte(devAddr, MPU9250_RA_XA_OFFS_TC, buffer); + return buffer[0]; } void MPU9250_Base::setXAccelOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU9250_RA_XA_OFFS_H, offset); + I2Cdev::writeByte(devAddr, MPU9250_RA_XA_OFFS_TC, offset); } // YA_OFFS_* register int16_t MPU9250_Base::getYAccelOffset() { - I2Cdev::readBytes(devAddr, MPU9250_RA_YA_OFFS_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; + I2Cdev::readByte(devAddr, MPU9250_RA_YA_OFFS_TC, buffer); + return buffer[0]; } void MPU9250_Base::setYAccelOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU9250_RA_YA_OFFS_H, offset); + I2Cdev::writeByte(devAddr, MPU9250_RA_YA_OFFS_TC, offset); } // ZA_OFFS_* register int16_t MPU9250_Base::getZAccelOffset() { - I2Cdev::readBytes(devAddr, MPU9250_RA_ZA_OFFS_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; + I2Cdev::readByte(devAddr, MPU9250_RA_ZA_OFFS_TC, buffer); + return buffer[0]; } void MPU9250_Base::setZAccelOffset(int16_t offset) { + I2Cdev::writeByte(devAddr, MPU9250_RA_ZA_OFFS_TC, offset); +} + +int16_t MPU9250_Base::getXAccelOffsetUser() { + I2Cdev::readBytes(devAddr, MPU9250_RA_XA_OFFS_H, 2, buffer); + return (((int16_t)buffer[0]) << 7) | (buffer[1] >> 1); +} +void MPU9250_Base::setXAccelOffsetUser(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU9250_RA_XA_OFFS_H, offset); +} + +// YA_OFFS_* register + +int16_t MPU9250_Base::getYAccelOffsetUser() { + I2Cdev::readBytes(devAddr, MPU9250_RA_YA_OFFS_H, 2, buffer); + return (((int16_t)buffer[0]) << 7) | (buffer[1] >> 1); +} +void MPU9250_Base::setYAccelOffsetUser(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU9250_RA_YA_OFFS_H, offset); +} + +// ZA_OFFS_* register + +int16_t MPU9250_Base::getZAccelOffsetUser() { + I2Cdev::readBytes(devAddr, MPU9250_RA_ZA_OFFS_H, 2, buffer); + return (((int16_t)buffer[0]) << 7) | (buffer[1] >> 1); +} +void MPU9250_Base::setZAccelOffsetUser(int16_t offset) { I2Cdev::writeWord(devAddr, MPU9250_RA_ZA_OFFS_H, offset); } diff --git a/lib/mpu9250/MPU9250.h b/lib/mpu9250/MPU9250.h index 9838562..ab542be 100644 --- a/lib/mpu9250/MPU9250.h +++ b/lib/mpu9250/MPU9250.h @@ -48,7 +48,8 @@ THE SOFTWARE. //Magnetometer Registers, AK8963 #define MPU9250_RA_MAG_ADDRESS 0x0C -#define MPU9250_RA_MAG_WHOAMI 0x01 +#define MPU9250_RA_MAG_WHOAMI 0x00 +#define MPU9250_RA_MAG_INFO 0x01 #define MPU9250_RA_MAG_ST1 0x02 #define MPU9250_RA_MAG_XOUT_L 0x03 #define MPU9250_RA_MAG_XOUT_H 0x04 @@ -95,7 +96,7 @@ THE SOFTWARE. #define MPU9250_MAG_CNTL2_SRST_BIT 0 - +//MPU6500 #define MPU9250_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board #define MPU9250_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) #define MPU9250_DEFAULT_ADDRESS MPU9250_ADDRESS_AD0_LOW @@ -106,28 +107,15 @@ THE SOFTWARE. #define MPU9250_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN #define MPU9250_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN #define MPU9250_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN - -#define MPU9250_RA_XA_OFFS_H 0x77 //[15:0] XA_OFFS -#define MPU9250_RA_XA_OFFS_L_TC 0x78 -#define MPU9250_RA_YA_OFFS_H 0x7A //[15:0] YA_OFFS -#define MPU9250_RA_YA_OFFS_L_TC 0x7B -#define MPU9250_RA_ZA_OFFS_H 0x7D //[15:0] ZA_OFFS -#define MPU9250_RA_ZA_OFFS_L_TC 0x7E - -// #define MPU9250_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS -// #define MPU9250_RA_XA_OFFS_L_TC 0x07 -// #define MPU9250_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS -// #define MPU9250_RA_YA_OFFS_L_TC 0x09 -// #define MPU9250_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS -// #define MPU9250_RA_ZA_OFFS_L_TC 0x0B - +#define MPU9250_RA_XA_OFFS_TC 0x0D +#define MPU9250_RA_YA_OFFS_TC 0x0E +#define MPU9250_RA_ZA_OFFS_TC 0x0F #define MPU9250_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR #define MPU9250_RA_XG_OFFS_USRL 0x14 #define MPU9250_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR #define MPU9250_RA_YG_OFFS_USRL 0x16 #define MPU9250_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR #define MPU9250_RA_ZG_OFFS_USRL 0x18 - #define MPU9250_RA_SMPLRT_DIV 0x19 #define MPU9250_RA_CONFIG 0x1A #define MPU9250_RA_GYRO_CONFIG 0x1B @@ -220,6 +208,12 @@ THE SOFTWARE. #define MPU9250_RA_FIFO_COUNTL 0x73 #define MPU9250_RA_FIFO_R_W 0x74 #define MPU9250_RA_WHO_AM_I 0x75 +#define MPU9250_RA_XA_OFFS_H 0x77 //[14:7] XA_OFFS +#define MPU9250_RA_XA_OFFS_L 0x78 //[7:1] +#define MPU9250_RA_YA_OFFS_H 0x7A //[14:7] YA_OFFS +#define MPU9250_RA_YA_OFFS_L 0x7B //[7:1] +#define MPU9250_RA_ZA_OFFS_H 0x7D //[14:7] ZA_OFFS +#define MPU9250_RA_ZA_OFFS_L 0x7E //[7:1] #define MPU9250_TC_PWR_MODE_BIT 7 #define MPU9250_TC_OFFSET_BIT 6 @@ -456,7 +450,7 @@ THE SOFTWARE. #define MPU9250_WHO_AM_I_BIT 8 #define MPU9250_WHO_AM_I_LENGTH 8 -#define MPU9250_DMP_MEMORY_BANKS 8 +// #define MPU9250_DMP_MEMORY_BANKS 8 #define MPU9250_DMP_MEMORY_BANK_SIZE 256 #define MPU9250_DMP_MEMORY_CHUNK_SIZE 16 @@ -783,6 +777,18 @@ class MPU9250_Base { int16_t getZAccelOffset(); void setZAccelOffset(int16_t offset); + // XA_OFFS_* registers + int16_t getXAccelOffsetUser(); + void setXAccelOffsetUser(int16_t offset); + + // YA_OFFS_* register + int16_t getYAccelOffsetUser(); + void setYAccelOffsetUser(int16_t offset); + + // ZA_OFFS_* register + int16_t getZAccelOffsetUser(); + void setZAccelOffsetUser(int16_t offset); + // XG_OFFS_USR* registers int16_t getXGyroOffsetUser(); void setXGyroOffsetUser(int16_t offset); diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index aa58ed6..ac75afb 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain, S.J. Remington + Copyright (c) 2022 Eiren Rain, S.J. Remington, SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights @@ -168,14 +168,12 @@ void MPU9250Sensor::getMPUScaled() float temp[3]; int i; int16_t ax,ay,az,gx,gy,gz,mx,my,mz; - imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - // Gxyz[0] = ((float)gx - calibration->G_off[0]) * gscale; //250 LSB(d/s) default to radians/s - // Gxyz[1] = ((float)gy - calibration->G_off[1]) * gscale; - // Gxyz[2] = ((float)gz - calibration->G_off[2]) * gscale; - Gxyz[0] = (float)gx * gscale; //250 LSB(d/s) default to radians/s - Gxyz[1] = (float)gy * gscale; - Gxyz[2] = (float)gz * gscale; +#if defined(_MAHONY_H_) || defined(_MADGWICK_H_) + imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); + Gxyz[0] = ((float)gx - calibration->G_off[0]) * gscale; //250 LSB(d/s) default to radians/s + Gxyz[1] = ((float)gy - calibration->G_off[1]) * gscale; + Gxyz[2] = ((float)gz - calibration->G_off[2]) * gscale; Axyz[0] = (float)ax; Axyz[1] = (float)ay; @@ -193,6 +191,11 @@ void MPU9250Sensor::getMPUScaled() Axyz[i] = (Axyz[i] - calibration->A_B[i]); #endif +#else + // with DMP, we just need mag data + imu.getMagnetometer(&mx, &my, &mz); +#endif + // Orientations of axes are set in accordance with the datasheet // See Section 9.1 Orientation of Axes // https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf @@ -214,6 +217,44 @@ void MPU9250Sensor::getMPUScaled() void MPU9250Sensor::startCalibration(int calibrationType) { ledManager.on(); +#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_)) + // with DMP, we just need mag data + constexpr int calibrationSamples = 300; + DeviceConfig *config = getConfigPtr(); + // Blink calibrating led before user should rotate the sensor + m_Logger.info("Gently rotate the device while it's gathering magnetometer data"); + ledManager.pattern(15, 300, 3000/310); + float *calibrationDataMag = (float*)malloc(calibrationSamples * 3 * sizeof(float)); + for (int i = 0; i < calibrationSamples; i++) + { + ledManager.on(); + int16_t ax,ay,az,gx,gy,gz,mx,my,mz; + imu.getMagnetometer(&mx, &my, &mz); + calibrationDataMag[i * 3 + 0] = my; + calibrationDataMag[i * 3 + 1] = mx; + calibrationDataMag[i * 3 + 2] = -mz; + Network::sendRawCalibrationData(calibrationDataMag, CALIBRATION_TYPE_EXTERNAL_MAG, 0); + ledManager.off(); + delay(250); + } + m_Logger.debug("Calculating calibration data..."); + + float M_BAinv[4][3]; + CalculateCalibration(calibrationDataMag, calibrationSamples, M_BAinv); + free(calibrationDataMag); + + m_Logger.debug("[INFO] Magnetometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + config->calibration[sensorId].M_B[i] = M_BAinv[0][i]; + config->calibration[sensorId].M_Ainv[0][i] = M_BAinv[1][i]; + config->calibration[sensorId].M_Ainv[1][i] = M_BAinv[2][i]; + config->calibration[sensorId].M_Ainv[2][i] = M_BAinv[3][i]; + m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); + } + m_Logger.debug("}"); + +#elif m_Logger.debug("Gathering raw data for device calibration..."); constexpr int calibrationSamples = 300; @@ -298,6 +339,8 @@ void MPU9250Sensor::startCalibration(int calibrationType) { m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); } m_Logger.debug("}"); +#endif + m_Logger.debug("Now Saving EEPROM"); setConfig(*config); ledManager.off();