Backup before cleaning package com.raspoid.additionalcomponents; import java.io.IOException; import com.pi4j.io.i2c.I2CBus; import com.pi4j.io.i2c.I2CDevice; import com.pi4j.io.i2c.I2CFactory; import com.raspoid.brickpi.Tools; import com.raspoid.exceptions.RaspoidException; /** * [datasheet - p.7] Product Overview * The MPU-60X0 is the world’s first integrated 6-axis MotionTracking device * that combines a 3-axis gyroscope, 3-axis accelerometer, and a Digital Motion * Processor™ (DMP) all in a small 4x4x0.9mm package. With its dedicated I2C * sensor bus, it directly accepts inputs from an external 3-axis compass to * provide a complete 9-axis MotionFusion™ output. * * The MPU-60X0 features three 16-bit analog-to-digital converters (ADCs) * for digitizing the gyroscope outputs and three 16-bit ADCs for digitizing * the accelerometer outputs. For precision tracking of both fast and slow * motions, the parts feature a user-programmable gyroscope full-scale range of * ±250, ±500, ±1000, and ±2000°/sec (dps) and a user-programmable accelerometer * full-scale range of ±2g, ±4g, ±8g, and ±16g. * * Communication with all registers of the device is performed using I2C at 400kHz. * * For power supply flexibility, the MPU-60X0 operates from VDD power supply voltage * range of 2.375V-3.46V. Additionally, the MPU-6050 provides a VLOGIC reference pin * (in addition to its analog supply pin: VDD), which sets the logic levels of its * I2C interface. The VLOGIC voltage may be 1.8V±5% or VDD. * * [datasheet - p.10-11] Features * - Gyroscope features (triple-axis MEMS gyroscope) * - Digital-output X-, Y-, and Z-Axis angular rate sensors (gyroscopes) with a * user-programmable fullscale range of ±250, ±500, ±1000, and ±2000°/sec * - Integrated 16-bit ADCs enable simultaneous sampling of gyros * - Enhanced bias and sensitivity temperature stability reduces the need * for user calibration * - Improved low-frequency noise performance * - Digitally-programmable low-pass filter * - Gyroscope operating current: 3.6mA * - Standby current: 5µA * - Factory calibrated sensitivity scale factor * - Accelerometer features (triple-axis MEMS accelerometer) * - Digital-output triple-axis accelerometer with a programmable full scale * range of ±2g, ±4g, ±8g and ±16g * - Integrated 16-bit ADCs enable simultaneous sampling of accelerometers while * requiring no external multiplexer * - Accelerometer normal operating current: 500µA * - Low power accelerometer mode current: 10µA at 1.25Hz, 20µA at 5Hz, 60µA * at 20Hz, 110µA at 40Hz * - Orientation detection and signaling * - Tap detection * - User-programmable interrupts * - High-G interrupt * - Additional features * - 9-Axis MotionFusion by the on-chip Digital Motion Processor (DMP) * - Auxiliary master I2C bus for reading data from external sensors (e.g., magnetometer) * - 3.9mA operating current when all 6 motion sensing axes and the DMP are enabled * - VDD supply voltage range of 2.375V-3.46V * - 1024 byte FIFO buffer reduces power consumption by allowing host processor * to read the data in bursts and then go into a low-power mode as the MPU collects * more data * - Digital-output temperature sensor * - User-programmable digital filters for gyroscope, accelerometer, and temp sensor * - 10,000 g shock tolerant * - 400kHz Fast Mode I2C for communicating with all registers * - MotionProcessing * - Internal Digital Motion Processing™ (DMP™) engine supports 3D MotionProcessing * and gesture recognition algorithms TODO * - The MPU-60X0 collects gyroscope and accelerometer data while synchronizing data * sampling at a user defined rate. The total dataset obtained by the MPU-60X0 includes * 3-Axis gyroscope data, 3-Axis accelerometer data, and temperature data. * The MPU’s calculated output to the system processor can also include heading data * from a digital 3-axis third party magnetometer. * - The FIFO buffers the complete data set, reducing timing requirements on the system * processor by allowing the processor burst read the FIFO data. After burst reading * the FIFO data, the system processor can save power by entering a low-power sleep * mode while the MPU collects more data. * - Programmable interrupt supports features such as gesture recognition, panning, * zooming, scrolling, tap detection, and shake detection. * - Digitally-programmable low-pass filters * - Low-power pedometer functionality allows the host processor to sleep while the DMP * maintains the step count. * * Datasheet: https://www.cdiweb.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf * Datasheet 2: (Register Map and Descriptions) http://43zrtwysvxb2gf29r5o0athu.wpengine.netdna-cdn.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf * * Important: The datasheet informations related to this sensor are about ~100 pages. * We don't implemented all the features in this driver, but we provide you the * updateRegisterValue(int registerAddress, int registerValue) method from which you can easily update * the update the content of a register, and the readRegisterValue(int registerAddress) method * from which you can easily read the content of a register of the component. * Enjoy ! :) */ public class MPU6050 { /** * Default address of the MPU6050 device. */ public static final int MPU6050_DEFAULT_ADDRESS = 0x68; /* ----------------------------------------------------------------------- * REGISTERS ADDRESSES * -----------------------------------------------------------------------*/ /** * [datasheet 2 - p.11] Sample Rate Divider * This register specifies the divider from the gyroscope output rate used to generate * the Sample Rate for the MPU-60X0. */ public static final int MPU6050_REG_ADDR_SMPRT_DIV = 0x19; // 25 /** * [datasheet 2 - p.13] Configuration * This register configures the external Frame Synchronization (FSYNC) pin sampling and * the Digital Low Pass Filter (DLPF) setting for both the gyroscopes and accelerometers. */ public static final int MPU6050_REG_ADDR_CONFIG = 0x1A; // 26 /** * [datasheet 2 - p.14] Gyroscope Configuration * This register is used to trigger gyroscope self-test and configure the gyroscopes’ full * scale range */ public static final int MPU6050_REG_ADDR_GYRO_CONFIG = 0x1B; // 27 /** * [datasheet 2 - p.15] Accelerometer Configuration * This register is used to trigger accelerometer self test and configure the accelerometer * full scale range. This register also configures the Digital High Pass Filter (DHPF). */ public static final int MPU6050_REG_ADDR_ACCEL_CONFIG = 0x1C; // 28 /** * [datasheet 2 - p.27] Interrupt Enable * This register enables interrupt generation by interrupt sources. */ public static final int MPU6050_REG_ADDR_INT_ENABLE = 0x1A; // 56 /** * [datasheet 2 - p.40] Power Management 1 * This register allows the user to configure the power mode and clock source. It also provides * a bit for resetting the entire device, and a bit for disabling the temperature sensor. */ public static final int MPU6050_REG_ADDR_PWR_MGMT_1 = 0x6B; // 107 /** * [datasheet 2 - p.42] Power Management 2 * This register allows the user to configure the frequency of wake-ups in Accelerometer Only Low * Power Mode. This register also allows the user to put individual axes of the accelerometer and * gyroscope into standby mode. */ public static final int MPU6050_REG_ADDR_PWR_MGMT_2 = 0x6C; // 108 /** * [datasheet 2 - p.29] Accelerometer Measurements * These registers store the most recent accelerometer measurements. */ public static final int MPU6050_REG_ADDR_ACCEL_XOUT_H = 0x3B, // 59 MPU6050_REG_ADDR_ACCEL_XOUT_L = 0x3C, // 60 MPU6050_REG_ADDR_ACCEL_YOUT_H = 0x3D, // 61 MPU6050_REG_ADDR_ACCEL_YOUT_L = 0x3E, // 62 MPU6050_REG_ADDR_ACCEL_ZOUT_H = 0x3F, // 63 MPU6050_REG_ADDR_ACCEL_ZOUT_L = 0x40; // 64 /** * [datasheet 2 - p.30] Temperature Measurement * These registers store the most recent temperature sensor measurement. */ public static final int MPU6050_REG_ADDR_TEMP_OUT_H = 0x41, // 65 MPU6050_REG_ADDR_TEMP_OUT_L = 0x42; // 66 /** * [datasheet 2 - p.31] Gyroscope Measurements * These registers store the most recent gyroscope measurements. */ public static final int MPU6050_REG_ADDR_GYRO_XOUT_H = 0x43, // 67 MPU6050_REG_ADDR_GYRO_XOUT_L = 0x44, // 68 MPU6050_REG_ADDR_GYRO_YOUT_H = 0x45, // 69 MPU6050_REG_ADDR_GYRO_YOUT_L = 0x46, // 70 MPU6050_REG_ADDR_GYRO_ZOUT_H = 0x47, // 71 MPU6050_REG_ADDR_GYRO_ZOUT_L = 0x48; // 72 private I2CBus bus; private I2CDevice mpu6050; /** * Sensisitivty of the measures. * Convert accelerometer values. */ private double accelLSBSensitivity; /** * Convert gyroscope values to degrees/sec. */ private double gyroLSBSensitivity; public static final double RADIAN_TO_DEGREE = (180. / Math.PI); private double rawAccelXOffset; private double rawAccelYOffset; private double rawAccelZOffset; private double rawGyroXOffset; private double rawGyroYOffset; private double rawGyroZOffset; /** * Last values read on the MPU6050. */ private long lastUpdateTime = 0; /** * Last time the filtered angles values are updated. */ private long lastFilteredAnglesUpdateTime = 0; // public double lastRawAccelX; // public double lastRawAccelY; // public double lastRawAccelZ; // private double lastRawGyroX; // private double lastRawGyroY; // private double lastRawGyroZ; // private double lastRawTemp; /** * Last calculated values for the angles from the accelerometer of the MPU6050. */ private double lastAngleAccelX, lastAngleAccelY, lastAngleAccelZ; /** * Last calculated values for the angles from the gyroscope of the MPU6050. * The accuracy of thos values mainly depends on the update frequency of filtered angles. */ private double lastAngleGyroX, lastAngleGyroY, lastAngleGyroZ; /** * Last calculated values for the angles from the gyroscope of the MPU6050. * Without taking the offset into account. */ private double lastScaledGyroX, lastScaledGyroY, lastScaledGyroZ; /** * Last calculated values for the angles, with a combination from the accelerometer * and the gyroscope of the MPU6050, for smooth values. */ private double filteredAngleX, filteredAngleY, filteredAngleZ; /** * Gyroscope values when the robot is not moving. * Used as calibration for the gyroscope values. */ private double gyroOffsetX, gyroOffsetY, gyroOffsetZ; private int DLPF_CFG; public MPU6050() { this(MPU6050_DEFAULT_ADDRESS, 0x06); } public MPU6050(int i2cAddress, int DLPF_CFG) { try { bus = I2CFactory.getInstance(I2CBus.BUS_1); // TODO Depends on the RasPI version => a déplacer dans une classe parent SensorI2C mpu6050 = bus.getDevice(i2cAddress); // 1. waking up the MPU6050 (0x00 = 0000 0000) // as it starts in sleep mode. We set the sleep bit to 0, to wake up the mpu6050. updateRegisterValue(MPU6050_REG_ADDR_PWR_MGMT_1, 0x00); // 2. sample rate divider // The sensor register output, FIFO output, and DMP sampling are all based on the Sample Rate. // The Sample Rate is generated by dividing the gyroscope output rate by SMPLRT_DIV: // Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) // where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7), and 1kHz // when the DLPF is enabled (see register 26). byte SMPLRT_DIV = 0x00; // set the rate to the default value : Sample Rate = Gyroscope Rate. updateRegisterValue(MPU6050_REG_ADDR_SMPRT_DIV, SMPLRT_DIV); // 3. This register configures the external Frame Synchronization (FSYNC) // pin sampling and the Digital Low Pass Filter (DLPF) setting for both // the gyroscopes and accelerometers. setDLPFConfig(DLPF_CFG); // 4. Gyroscope configuration // FS_SEL selects the full scale range of the gyroscope outputs. byte FS_SEL = 0 << 3; // +- 250 °/s gyroLSBSensitivity = 131.; // cfr [datasheet 2 - p.31] updateRegisterValue(MPU6050_REG_ADDR_GYRO_CONFIG, FS_SEL); // 5. Accelerometer configuration [datasheet 2 - p.29] byte AFS_SEL = 0; // full scale range: ± 2g. LSB sensitivity : 16384 LSB/g accelLSBSensitivity = 16384.; // LSB Sensitivity corresponding to AFS_SEL 0; updateRegisterValue(MPU6050_REG_ADDR_ACCEL_CONFIG, AFS_SEL); // 6. Disable interrupts updateRegisterValue(MPU6050_REG_ADDR_INT_ENABLE, 0x00); // 7. Disable standby mode updateRegisterValue(MPU6050_REG_ADDR_PWR_MGMT_2, 0x00); // Calibration lastUpdateTime = 0; gyroOffsetX = 0; gyroOffsetY = 0; gyroOffsetZ = 0; calibrateGyroscope(); } catch (IOException e) { throw new RaspoidException(e); } } /** * Returns the Sample Rate of the MPU6050. * * [datasheet 2 - p.12] The sensor output, FIFO output, and DMP sampling are all based on the Sample Rate ('Fs' in the datasheet). * * The Sample Rate is generated by dividing the gyroscope output rate by SMPLRT_DIV: * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7), and 1kHz * when the DLPF is enabled (see Register 26) * * Note: The accelerometer output rate is 1kHz (accelerometer and not gyroscope !). * This means that for a Sample Rate greater than 1kHz, the same accelerometer sample * may be output to the FIFO, DMP, and sensor registers more than once. * * @return the sample rate, in Hz. */ private int getSampleRate() { int gyroscopeOutputRate = DLPF_CFG == 0 || DLPF_CFG == 7 ? 8000 : 1000; // 8kHz if DLPG disabled, and 1kHz if enabled. int SMPLRT_DIV = 0x00; // the rate is setted to the default value (cfr constructor). return gyroscopeOutputRate / (1 + SMPLRT_DIV); } /** * Sets the value of the DLPF config, according to the datasheet informations. * * The accelerometer and gyroscope are filtered according to the value of DLPF_CFG as shown in the table [datasheet 2 - p.13]. * * @param config the new DLPF_CFG value. Must be in the [0; 7] range, where 0 and 7 are used to disable the DLPF. */ public void setDLPFConfig(int DLPFConfig) { if(DLPFConfig > 7 || DLPFConfig < 0) throw new IllegalArgumentException("The DLPFConfig must be in the [0;7] interval."); DLPF_CFG = DLPFConfig; updateRegisterValue(MPU6050_REG_ADDR_CONFIG, DLPF_CFG); } /** * Reads the content of a register of the MPU6050. * @param registerAddress * @return the int representation of the content of this register. */ private int readRegisterValue(int registerAddress) { try { return mpu6050.read(registerAddress); } catch (IOException e) { throw new RaspoidException(e); } } /** * Reads the content of two consecutive registers, starting at registerAddress, * and return the int representation of the combination of those registers, * with a two's complement representation. * @param registerAddress * @return */ public int readWord2C(int registerAddress) { int value = readRegisterValue(registerAddress); value = value << 8; value += readRegisterValue(registerAddress + 1); if (value >= 0x8000) value = -(65536 - value); return value; } /** * This method updates the value of the register registerAddress with registerValue. * The method also checks that the update was successfull. * * @param registerAddress * @param registerValue */ public void updateRegisterValue(int registerAddress, int registerValue) { if(registerValue > 255 || registerValue < 0) throw new IllegalArgumentException("The register value must be in the [0;255] interval."); try { mpu6050.write(registerAddress, (byte)registerValue); // we check that the value of the register has been updated int readRegisterValue = mpu6050.read(registerAddress); if(readRegisterValue != registerValue) throw new RaspoidException("Error when updating the MPU6050 register value (register: " + registerAddress + ", value: " + registerValue + ")"); } catch (IOException e) { throw new RaspoidException(e); } } /** * This method will evaluate the current position of the sensor, * and use this as an offset for futur reading values. * TODO */ public void callibrateSensor() { int nbReadings = 10; double rawAccelXOffset = 0; double rawAccelYOffset = 0; double rawAccelZOffset = 0; double rawGyroXOffset = 0; double rawGyroYOffset = 0; double rawGyroZOffset = 0; double[] accelRawValues; for(int i = 0; i < nbReadings; i++) { accelRawValues = getScaledAccelerometerValues(); rawAccelXOffset += accelRawValues[0]; rawAccelYOffset += accelRawValues[1]; rawAccelZOffset += accelRawValues[2]; } rawAccelXOffset /= nbReadings; rawAccelYOffset /= nbReadings; rawAccelZOffset /= nbReadings; rawGyroXOffset /= nbReadings; rawGyroYOffset /= nbReadings; rawGyroZOffset /= nbReadings; this.rawAccelXOffset = rawAccelXOffset; this.rawAccelYOffset = rawAccelYOffset; this.rawAccelZOffset = rawAccelZOffset; } /** * This method updates scaled values of the accelerometer and gyroscope, * with respect to the Sample Rate and the corresponding offsets form gyroscope and accelerometer values. * * To track values, frequency of measures should be between 100Hz and 1kHz or 8kHz, depending on selected DLPF mode. * * Remainder: the Sample Rate is related to the DLPF mode selected. * In the case of the accelerometer, the rate is of 1kHZ (the values are updated 1000 times pers second in the registers of the MPU6050). * In the case of the gyroscope, the rate is of 1kHZ is DLPF is enabled (mode equal to 0 or 7) or 8kHZ if DLPF is disabled. * The gyroscope rate is used to define the Sample Rate. So, in case of 8kHZ, the accelerometer values are the same for 8 consecutive measures. */ private void updateValues() { long timeDiff = System.currentTimeMillis() - lastUpdateTime; if(Math.abs(timeDiff) > 1000 / getSampleRate()) { Tools.log("New update values"); // Accelerometer - angles double[] accelValues = getScaledAccelerometerValues(); double accelX = accelValues[0]; // TODO offsets double accelY = accelValues[1]; double accelZ = accelValues[2]; lastAngleAccelX = getAccelXAngle(accelX, accelY, accelZ); lastAngleAccelY = getAccelYAngle(accelX, accelY, accelZ); lastAngleAccelZ = getAccelZAngle(accelX, accelY, accelZ); // Gyroscope - angular velocities double[] gyroValues = getScaledGyroscopeValues(); lastScaledGyroX = gyroValues[0] - gyroOffsetX; lastScaledGyroY = gyroValues[1] - gyroOffsetY; lastScaledGyroZ = gyroValues[2] - gyroOffsetZ; // Gyroscope - angles // we want angles from gyro: angular speed * time = angle long maxTimeDiff = 10; // ms. So, the minimum update frequency should be 100Hz. double gyroDeltaX, gyroDeltaY, gyroDeltaZ; // °/s if(timeDiff > maxTimeDiff) { // the last updated value is too old. We need to reset the gyro total angle. lastAngleGyroX = lastAngleAccelX; lastAngleGyroY = lastAngleAccelY; lastAngleGyroZ = lastAngleAccelZ; timeDiff = maxTimeDiff; } gyroDeltaX = lastScaledGyroX * timeDiff; gyroDeltaY = lastScaledGyroY * timeDiff; gyroDeltaZ = lastScaledGyroZ * timeDiff; lastAngleGyroX += gyroDeltaX; lastAngleGyroY += gyroDeltaY; lastAngleGyroZ += gyroDeltaZ; // Filtered angles double alpha = 0.98; filteredAngleX = alpha * (filteredAngleX + gyroDeltaX) + (1. - alpha) * lastAngleAccelX; filteredAngleY = alpha * (filteredAngleY + gyroDeltaY) + (1. - alpha) * lastAngleAccelY; // filteredAngleZ = alpha * (filteredAngleZ + gyroDeltaZ) + (1. - alpha) * lastAngleAccelZ; filteredAngleZ = filteredAngleZ + gyroDeltaZ; // Temperature //lastRawTemp = getRawTemperatureValue(); // Last update time lastUpdateTime = System.currentTimeMillis(); } } /** * Determine the value of the gyroscope offset. * This offset corresponds to values of the gyroscope, when this one is not moving. * Then, you need to calibrate the gyroscope when the MPU6050 is not moving ! * * Note: a first callibration is done when creating the java entity, but feel * free to calibrate again, when you are sure the sensor is not moving. */ public void calibrateGyroscope() { updateValues(); gyroOffsetX = lastScaledGyroX + gyroOffsetX; gyroOffsetY = lastAngleAccelY + gyroOffsetY; gyroOffsetZ = lastAngleAccelZ + gyroOffsetZ; } /** * Ths gyroTotal corresponds to a tracked value of the gyroscope. * The accuracy of the measure then depends on the frequency of measurements. * So, the gyro total need to be resetted if no meausrements are done frequently. * This is launched from the public getAngles method. */ private void resetGyroTotal() { updateValues(); } // public double[] getFilteredAngles() { // updateValues(); // // long timeDiff = System.currentTimeMillis() - lastFilteredAnglesUpdateTime; // long maxTimeDiff = 10; // ms // // // we want angles from gyro: angular speed * time = angle // double gyroDeltaX; // °/s // double gyroDeltaY; // °/s // double gyroDeltaZ; // °/s // if(timeDiff > maxTimeDiff) { // // the last updated value is too old. We need to reset the gyro total. // lastAngleGyroX = lastAngleAccelX;// TODO couille // lastAngleGyroY = lastScaledGyroY; // lastAngleGyroZ = lastScaledGyroZ; // // timeDiff = maxTimeDiff; // } else { // // the last updated valu isn't too old. We can smooth this value. // // angular speed * speed = angle. // gyroDeltaX = lastScaledGyroX * timeDiff; // gyroDeltaY = lastScaledGyroY * timeDiff; // gyroDeltaZ = lastScaledGyroZ * timeDiff; // // lastAngleGyroX += gyroDeltaX; // lastAngleGyroY += gyroDeltaY; // lastAngleGyroZ += gyroDeltaZ; // } // // double K = 0.98; // filteredAngleX = K * (filteredAngleX + gyroDeltaX) + (1-K) * lastAngleAccelX; // filteredAngleY = K * (filteredAngleY + gyroDeltaY) + (1-K) * lastAngleAccelY; // filteredAngleZ = K * (filteredAngleZ + gyroDeltaZ) + (1-K) * lastAngleAccelZ; // // lastFilteredAnglesUpdateTime = System.currentTimeMillis(); // // return new double[]{filteredAngleX, filteredAngleY, filteredAngleZ}; // } public double getAccelXAngle() { updateValues(); return lastAngleAccelX; } public double getAccelYAngle() { updateValues(); return lastAngleAccelY; } public double getAccelZAngle() { updateValues(); return lastAngleAccelZ; } public double[] getAccelAngles() { updateValues(); return new double[]{lastAngleAccelX, lastAngleAccelY, lastAngleAccelZ}; } public double getGyroXAngularVelocity() { updateValues(); return lastScaledGyroX; // TODO rename ?! } public double getGyroYAngularVelocity() { updateValues(); return lastScaledGyroY; } public double getGyroZAngularVelocity() { updateValues(); return lastScaledGyroZ; } public double[] getGyroAngularVelocities() { updateValues(); return new double[]{lastScaledGyroX, lastScaledGyroY, lastScaledGyroZ}; } /** * Read the most recent accelerometer measurements on MPU6050 for X, Y and Z. * Updates the lastRawAccelX, lastRawAccelY and lastRawAccelZ variables. * @return [ACCEL_X, ACCEL_Y, ACCEL_Z] */ private double[] getScaledAccelerometerValues() { double accelX = readWord2C(MPU6050_REG_ADDR_ACCEL_XOUT_H); accelX /= accelLSBSensitivity; double accelY = readWord2C(MPU6050_REG_ADDR_ACCEL_YOUT_H); accelY /= accelLSBSensitivity; double accelZ = readWord2C(MPU6050_REG_ADDR_ACCEL_ZOUT_H); accelZ /= accelLSBSensitivity; return new double[]{accelX, accelY, accelZ}; } // TODO déplacer private int getRawTemperatureValue() { return (readRegisterValue(MPU6050_REG_ADDR_TEMP_OUT_H) << 8) + readRegisterValue(MPU6050_REG_ADDR_TEMP_OUT_L); } /** * Raw values from MPU6050, in degrees/sec, according to the selected FS_SEL mode. * @return gyroscope scaled data (angular velocity) in degrees/sec. */ private double[] getScaledGyroscopeValues() { double gyroX = readWord2C(MPU6050_REG_ADDR_GYRO_XOUT_H); gyroX /= gyroLSBSensitivity; double gyroY = readWord2C(MPU6050_REG_ADDR_GYRO_YOUT_H); gyroY /= gyroLSBSensitivity; double gyroZ = readWord2C(MPU6050_REG_ADDR_GYRO_ZOUT_H); gyroZ /= gyroLSBSensitivity; return new double[]{gyroX, gyroY, gyroZ}; } private double distance(double a, double b) { return Math.sqrt(a * a + b * b); } /** * Inputs: raw values for x, y and z. * TODO test if the 2 versions are the same * @param x * @param y * @param z * @return */ private double getAccelYAngle(double x, double y, double z) { // v1 double radians = Math.atan2(x, distance(y, z)); return -radians * RADIAN_TO_DEGREE; // v2 // return Math.atan(-1 * x / distance(y, z)) * RADIAN_TO_DEGREE; } // TODO test if the 2 versions are the same private double getAccelXAngle(double x, double y, double z) { // v1 double radians = Math.atan2(y, distance(x, z)); return radians * RADIAN_TO_DEGREE; // v2 // return Math.atan(y / distance(x, z))* RADIAN_TO_DEGREE; } // TODO private double getAccelZAngle(double x, double y, double z) { return 0; } public static void main(String[] args) { MPU6050 mpu6050 = new MPU6050(); String result; while(true) { result = ""; // Accelerometer result += "Accelerometer:\n"; double[] accelAngles = mpu6050.getAccelAngles(); double accelX = accelAngles[0]; double accelY = accelAngles[1]; double accelZ = accelAngles[2]; result += "x: " + accelX + " y: " + accelY + " z: " + accelZ + "\n"; // Gyroscope result += "Gyroscope:\n"; double[] gyroAngularValocities = mpu6050.getGyroAngularVelocities(); double gyroX = gyroAngularValocities[0]; double gyroY = gyroAngularValocities[1]; double gyroZ = gyroAngularValocities[2]; result += "x: " + gyroX + " y: " + gyroY + " z: " + gyroZ + "\n"; // Accelerometer + gyroscope (complementary filter) // Temperature System.out.println(result); Tools.sleepMilliseconds(200); } } }