com.diozero.sandpit.imu.invensense.MPU9150DriverTest Maven / Gradle / Ivy
package com.diozero.sandpit.imu.invensense;
/*
* #%L
* Organisation: mattjlewis
* Project: Device I/O Zero - IMU Sample App
* Filename: MPU9150DriverTest.java
*
* This file is part of the diozero project. More information about this project
* can be found at http://www.diozero.com/
* %%
* Copyright (C) 2016 - 2017 mattjlewis
* %%
* 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
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
* #L%
*/
import org.apache.commons.math3.complex.Quaternion;
import org.apache.commons.math3.geometry.euclidean.threed.*;
import org.pmw.tinylog.Logger;
import com.diozero.api.I2CConstants;
import com.diozero.api.imu.ImuData;
import com.diozero.api.imu.OrientationEvent;
import com.diozero.api.imu.TapEvent;
import com.diozero.imu.drivers.invensense.*;
import com.diozero.util.RuntimeIOException;
import com.diozero.util.SleepUtil;
/**
* From https://github.com/richards-tech/RTIMULib/blob/master/RTIMULib/IMUDrivers/
*
* MPU6050/MPU6500/MPU9150/MPU9250 over I2c for RaspberryPi using official
* Invensense libraries (v5.1):
* http://www.invensense.com/developers/index.php?_r=downloads
*/
public class MPU9150DriverTest {
private static final float RTIMU_FUZZY_ACCEL_ZERO = 0.05f;
private static final float RTIMU_FUZZY_GYRO_ZERO = 0.20f;
private static final int DEFAULT_FIFO_RATE = 20;
// MPU driver variables
private MPU9150DMPDriver dmp;
private GyroFullScaleRange gyroFsr;
private AccelFullScaleRange accelFsr;
private int gyroAccelSampleRate;
private int sampleRate;
private int compassSampleRate;
private LowPassFilter lpf;
private int fifoRate;
// Time to sleep between reading from the FIFO (= 1000 / fifoRate)
private int fifoReadDelayMs;
private long lastFifoRead;
private AxisRotation axisRotation;
// RTIMULib variables
private FusionAlgorithmType fusionType;
private FusionInterface fusion;
private Vector3D previousAccel;
private int gyroSampleCount;
private float gyroLearningAlpha;
private float gyroContinuousAlpha;
private Vector3D gyroBias;
private boolean gyroBiasValid;
public static void main(String[] args) {
new MPU9150DriverTest().run();
}
public MPU9150DriverTest() {
// General defaults
//axisRotation = AxisRotation.RTIMU_XNORTH_YEAST;
axisRotation = AxisRotation.RTIMU_XEAST_YSOUTH;
//axisRotation = AxisRotation.RTIMU_XNORTH_YWEST;
//mpu9150AxisRotation = MPU9150AxisRotation.;
// MPU9150 defaults
gyroAccelSampleRate = 50;
compassSampleRate = 25;
lpf = LowPassFilter.INV_FILTER_20HZ;
gyroFsr = GyroFullScaleRange.INV_FSR_1000DPS;
accelFsr = AccelFullScaleRange.INV_FSR_8G;
fifoRate = DEFAULT_FIFO_RATE;
fifoReadDelayMs = 1000 / fifoRate;
gyroBiasValid = false;
gyroBias = Vector3D.ZERO;
previousAccel = Vector3D.ZERO;
sampleRate = gyroAccelSampleRate;
fusionType = FusionAlgorithmType.RTFUSION_TYPE_RTQF;
switch (fusionType) {
case RTFUSION_TYPE_KALMANSTATE4:
fusion = new RTFusionKalman4();
break;
case RTFUSION_TYPE_RTQF:
fusion = new RTFusionRTQF();
break;
default:
fusion = new RTFusion();
}
}
public void run() {
try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7,
MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) {
mpuInit(mpu);
System.err.println("Ready.");
do {
ImuData imu_data = update(mpu);
System.out.print("Got IMU data: compass=[" + imu_data.getCompass() +
"], temp=" + imu_data.getTemperature() + ", gyro=[" + imu_data.getGyro() +
"], accel=[" + imu_data.getAccel() + "], quat=[" + imu_data.getQuaternion().getQ0() +
", " + imu_data.getQuaternion().getQ1() + ", " + imu_data.getQuaternion().getQ2() +
", " + imu_data.getQuaternion().getQ3() + "], timestamp=" + imu_data.getTimestamp() + ", ");
Quaternion q = imu_data.getQuaternion();
//double[] ypr = q.toEuler();
//double[] ypr = quat.getYawPitchRoll();
Rotation r = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true);
double[] ypr = null;
try {
ypr = r.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR);
} catch (CardanEulerSingularityException e) {
ypr = new double[] { 2*Math.atan2(q.getQ1(), q.getQ0()), Math.PI/2, 0};
System.out.print("Singularity detected, ");
}
Logger.info("ypr=[{}, {}, {}]", Double.valueOf(ypr[0]), Double.valueOf(ypr[1]), Double.valueOf(ypr[2]));
} while (true);
} catch (RuntimeIOException ioe) {
Logger.error(ioe, "Error: {}", ioe);
}
}
private void mpuInit(MPU9150Driver mpu) throws RuntimeIOException {
// initialise device
Logger.debug("Initialising MPU...");
mpu.mpu_init();
dmp = new MPU9150DMPDriver(mpu);
Logger.debug("Setting MPU sensors...");
// Can be bitwise combination of INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO,
// INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_XYZ_COMPASS
mpu.mpu_set_sensors((byte) (MPU9150Constants.INV_XYZ_GYRO | MPU9150Constants.INV_XYZ_ACCEL |
MPU9150Constants.INV_XYZ_COMPASS));
//Logger.debug("Setting LPF...");
//mpu.mpu_set_lpf(lpf);
Logger.debug("Setting GYRO sensitivity...");
mpu.mpu_set_gyro_fsr(gyroFsr);
Logger.debug("Setting ACCEL sensitivity...");
mpu.mpu_set_accel_fsr(accelFsr);
// verify connection
Logger.debug("Powering up MPU...");
boolean dev_status = mpu.mpu_get_power_state();
Logger.debug(dev_status ? "MPU9150 connection successful" : "MPU9150 connection failed");
// fifo config
Logger.debug("Setting MPU fifo...");
// Note compass data doesn't go into the FIFO, no need trying to set INV_XYZ_COMPASS
mpu.mpu_configure_fifo((byte) (MPU9150Constants.INV_XYZ_GYRO | MPU9150Constants.INV_XYZ_ACCEL));
// load and configure the DMP
Logger.debug("Loading DMP firmware...");
dmp.dmp_load_motion_driver_firmware();
// Configure the orientation
dmp.dmp_set_orientation(MPU9150DMPDriver.inv_orientation_matrix_to_scalar(
axisRotation.getOrientationMatrix()));
Logger.debug("Activating DMP...");
mpu.mpu_set_dmp_state(true);
Logger.debug("Configuring DMP...");
dmp.dmp_register_tap_cb(MPU9150DriverTest::tapCallback);
dmp.dmp_register_android_orient_cb(MPU9150DriverTest::androidOrientCallback);
//int hal_dmp_features = MPU9150DMPConstants.DMP_FEATURE_6X_LP_QUAT |
// MPU9150DMPConstants.DMP_FEATURE_SEND_RAW_ACCEL|
// MPU9150DMPConstants.DMP_FEATURE_SEND_CAL_GYRO |
// MPU9150DMPConstants.DMP_FEATURE_GYRO_CAL;
int hal_dmp_features =
MPU9150DMPConstants.DMP_FEATURE_TAP |
MPU9150DMPConstants.DMP_FEATURE_ANDROID_ORIENT |
MPU9150DMPConstants.DMP_FEATURE_PEDOMETER |
MPU9150DMPConstants.DMP_FEATURE_6X_LP_QUAT |
MPU9150DMPConstants.DMP_FEATURE_SEND_RAW_ACCEL |
MPU9150DMPConstants.DMP_FEATURE_SEND_CAL_GYRO |
MPU9150DMPConstants.DMP_FEATURE_GYRO_CAL;
dmp.dmp_enable_feature(hal_dmp_features);
Logger.debug("Setting DMP fifo rate...");
// MPU sample rate is ignored if DMP is enabled
//mpu.mpu_set_sample_rate(rate);
//mpu.mpu_set_compass_sample_rate(compassSampleRate);
dmp.dmp_set_fifo_rate(fifoRate);
/* When the DMP is used, the hardware sampling rate is fixed at
* 200Hz, and the DMP is configured to downsample the FIFO output
* using the function dmp_set_fifo_rate. However, when the DMP is
* turned off, the sampling rate remains at 200Hz. This could be
* handled in inv_mpu.c, but it would need to know that
* inv_mpu_dmp_motion_driver.c exists. To avoid this, we'll just
* put the extra logic in the application layer.
*/
//int dmp_rate = dmp.dmp_get_fifo_rate();
//mpu.mpu_set_sample_rate(dmp_rate);
Logger.debug("Resetting fifo queue...");
mpu.mpu_reset_fifo();
gyroBiasInit();
Logger.debug("Sleep time={}", Integer.valueOf(fifoReadDelayMs));
Logger.debug("Waiting for first FIFO data item... ");
MPU9150FIFOData fifo_data = null;
do {
SleepUtil.sleepMillis(fifoReadDelayMs);
fifo_data = dmp.dmp_read_fifo();
} while (fifo_data == null);
lastFifoRead = fifo_data.getTimestamp();
Logger.debug("Done.");
}
private ImuData update(MPU9150Driver mpu) throws RuntimeIOException {
// Wait for FIFO data to be available
// FIXME Use a fixed period scheduler instead
long delay = fifoReadDelayMs - (System.currentTimeMillis() - lastFifoRead);
if (delay > 0) {
Logger.debug("Sleeping for {}ms", Long.valueOf(delay));
SleepUtil.sleepMillis(delay);
} else {
Logger.debug("Not sleeping, delay={}", Long.valueOf(delay));
}
//gyro and accel can be null because of being disabled in the features
// FIXME move this logic to a scheduler within MPU-9150 driver?
MPU9150FIFOData fifo_data;
// dmp_read_fifo(g, a, _q, &sensors, &fifoCount)
do {
fifo_data = dmp.dmp_read_fifo();
} while (fifo_data == null);
Logger.debug("Time between FIFO reads = {}ms", Long.valueOf(System.currentTimeMillis() - lastFifoRead));
lastFifoRead = fifo_data.getTimestamp();
ImuData imu_data = MPU9150DataFactory.newInstance(fifo_data, mpu.mpu_get_compass_reg(),
gyroFsr.getScale(), accelFsr.getScale(), AK8975Constants.COMPASS_SCALE,
MPU9150Constants.QUATERNION_SCALE, mpu.mpu_get_temperature());
// Now do standard processing
//handleGyroBias(imu_data);
//calibrateAverageCompass();
//calibrateAccel();
// now update the filter
//updateFusion(imu_data);
return imu_data;
}
private void updateFusion(ImuData imuData) {
fusion.newIMUData(imuData);
}
private void gyroBiasInit() {
gyroLearningAlpha = 2.0f / sampleRate;
gyroContinuousAlpha = 0.01f / sampleRate;
gyroSampleCount = 0;
}
private void handleGyroBias(ImuData imuData) {
/*
* Shouldn't need to do any of this if we call dmp_set_orientation...
// do axis rotation
if (axisRotation != null) {
// need to do an axis rotation
byte[][] matrix = axisRotation.getOrientationMatrix();
IMUData tempIMU = imuData;
// do new x value
if (matrix[0][0] != 0) {
imuData.gyro.setX(tempIMU.gyro.x() * matrix[0][0]);
imuData.accel.setX(tempIMU.accel.x() * matrix[0][0]);
imuData.compass.setX(tempIMU.compass.x() * matrix[0][0]);
} else if (matrix[0][1] != 0) {
imuData.gyro.setX(tempIMU.gyro.y() * matrix[0][1]);
imuData.accel.setX(tempIMU.accel.y() * matrix[0][1]);
imuData.compass.setX(tempIMU.compass.y() * matrix[0][1]);
} else if (matrix[0][2] != 0) {
imuData.gyro.setX(tempIMU.gyro.z() * matrix[0][2]);
imuData.accel.setX(tempIMU.accel.z() * matrix[0][2]);
imuData.compass.setX(tempIMU.compass.z() * matrix[0][2]);
}
// do new y value
if (matrix[1][0] != 0) {
imuData.gyro.setY(tempIMU.gyro.x() * matrix[1][0]);
imuData.accel.setY(tempIMU.accel.x() * matrix[1][0]);
imuData.compass.setY(tempIMU.compass.x() * matrix[1][0]);
} else if (matrix[1][1] != 0) {
imuData.gyro.setY(tempIMU.gyro.y() * matrix[1][1]);
imuData.accel.setY(tempIMU.accel.y() * matrix[1][1]);
imuData.compass.setY(tempIMU.compass.y() * matrix[1][1]);
} else if (matrix[1][2] != 0) {
imuData.gyro.setY(tempIMU.gyro.z() * matrix[1][2]);
imuData.accel.setY(tempIMU.accel.z() * matrix[1][2]);
imuData.compass.setY(tempIMU.compass.z() * matrix[1][2]);
}
// do new z value
if (matrix[2][0] != 0) {
imuData.gyro.setZ(tempIMU.gyro.x() * matrix[2][0]);
imuData.accel.setZ(tempIMU.accel.x() * matrix[2][0]);
imuData.compass.setZ(tempIMU.compass.x() * matrix[2][0]);
} else if (matrix[2][1] != 0) {
imuData.gyro.setZ(tempIMU.gyro.y() * matrix[2][1]);
imuData.accel.setZ(tempIMU.accel.y() * matrix[2][1]);
imuData.compass.setZ(tempIMU.compass.y() * matrix[2][1]);
} else if (matrix[2][2] != 0) {
imuData.gyro.setZ(tempIMU.gyro.z() * matrix[2][2]);
imuData.accel.setZ(tempIMU.accel.z() * matrix[2][2]);
imuData.compass.setZ(tempIMU.compass.z() * matrix[2][2]);
}
}
*/
Vector3D deltaAccel = previousAccel.subtract(imuData.getAccel()); // compute difference
previousAccel = imuData.getAccel();
if ((deltaAccel.getNorm() < RTIMU_FUZZY_ACCEL_ZERO) && (imuData.getGyro().getNorm() < RTIMU_FUZZY_GYRO_ZERO)) {
// What we are seeing on the gyros should be bias only so learn from this
if (gyroSampleCount < (5 * sampleRate)) {
gyroBias = new Vector3D((1.0 - gyroLearningAlpha) * gyroBias.getX() + gyroLearningAlpha * imuData.getGyro().getX(),
(1.0 - gyroLearningAlpha) * gyroBias.getY() + gyroLearningAlpha * imuData.getGyro().getY(),
(1.0 - gyroLearningAlpha) * gyroBias.getZ() + gyroLearningAlpha * imuData.getGyro().getZ());
gyroSampleCount++;
if (gyroSampleCount == (5 * sampleRate)) {
// this could have been true already of course
gyroBiasValid = true;
//saveSettings();
}
} else {
gyroBias = new Vector3D((1.0 - gyroContinuousAlpha) * gyroBias.getX() + gyroContinuousAlpha * imuData.getGyro().getX(),
(1.0 - gyroContinuousAlpha) * gyroBias.getY() + gyroContinuousAlpha * imuData.getGyro().getY(),
(1.0 - gyroContinuousAlpha) * gyroBias.getZ() + gyroContinuousAlpha * imuData.getGyro().getZ());
}
}
imuData.setGyro(imuData.getGyro().subtract(gyroBias));
}
public static void tapCallback(TapEvent event) {
Logger.debug("tapCallback({})", event);
}
public static void androidOrientCallback(OrientationEvent event) {
Logger.debug("androidOrientCallback({})", event);
}
/** Axis rotation defs
* These allow the IMU to be virtually repositioned if it is in a non-standard configuration
* Standard configuration is X pointing at north, Y pointing east and Z pointing down
* with the IMU horizontal. There are 24 different possible orientations as defined
* below. Setting the axis rotation code to non-zero values performs the repositioning.
*
* See Orientation Matrix Transformation chart.pdf
* XYZ 010_001_000 Identity Matrix
* XZY 001_010_000
* YXZ 010_000_001
* YZX 000_010_001
* ZXY 001_000_010
* ZYX 000_001_010
*
*/
public static enum AxisRotation {
RTIMU_XNORTH_YEAST(new byte[][] {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}), // this is the default identity matrix
RTIMU_XEAST_YSOUTH(new byte[][] {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}),
RTIMU_XSOUTH_YWEST(new byte[][] {{-1, 0, 0}, {0, -1, 0}, {0, 0, 1}}),
RTIMU_XWEST_YNORTH(new byte[][] {{0, 1, 0}, {-1, 0, 0}, {0, 0, 1}}),
RTIMU_XNORTH_YWEST(new byte[][] {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}),
RTIMU_XEAST_YNORTH(new byte[][] {{0, 1, 0}, {1, 0, 0}, {0, 0, -1}}),
RTIMU_XSOUTH_YEAST(new byte[][] {{-1, 0, 0}, {0, 1, 0}, {0, 0, -1}}),
RTIMU_XWEST_YSOUTH(new byte[][] {{0, -1, 0}, {-1, 0, 0}, {0, 0, -1}}),
RTIMU_XUP_YNORTH(new byte[][] {{0, 1, 0}, {0, 0, -1}, {-1, 0, 0}}),
RTIMU_XUP_YEAST(new byte[][] {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}),
RTIMU_XUP_YSOUTH(new byte[][] {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}),
RTIMU_XUP_YWEST(new byte[][] {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}),
RTIMU_XDOWN_YNORTH(new byte[][] {{0, 1, 0}, {0, 0, 1}, {1, 0, 0}}),
RTIMU_XDOWN_YEAST(new byte[][] {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}),
RTIMU_XDOWN_YSOUTH(new byte[][] {{0, -1, 0}, {0, 0, -1}, {1, 0, 0}}),
RTIMU_XDOWN_YWEST(new byte[][] {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}),
RTIMU_XNORTH_YUP(new byte[][] {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}),
RTIMU_XEAST_YUP(new byte[][] {{0, 0, -1}, {1, 0, 0}, {0, -1, 0}}),
RTIMU_XSOUTH_YUP(new byte[][] {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}),
RTIMU_XWEST_YUP(new byte[][] {{0, 0, 1}, {-1, 0, 0}, {0, -1, 0}}),
RTIMU_XNORTH_YDOWN(new byte[][] {{1, 0, 0}, {0, 0, -1}, {0, 1, 0}}),
RTIMU_XEAST_YDOWN(new byte[][] {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}),
RTIMU_XSOUTH_YDOWN(new byte[][] {{-1, 0, 0}, {0, 0, 1}, {0, 1, 0}}),
RTIMU_XWEST_YDOWN(new byte[][] {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}});
private byte[][] orientationMatrix;
private AxisRotation(byte[][] orientationMatrix) {
this.orientationMatrix = orientationMatrix;
}
public byte[][] getOrientationMatrix() {
return orientationMatrix;
}
}
public static enum FusionAlgorithmType {
RTFUSION_TYPE_KALMANSTATE4, // kalman state is the quaternion pose
RTFUSION_TYPE_RTQF; // RT quaternion fusion
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy