com.diozero.sandpit.imu.invensense.MPU9150DriverMqttPublisher.java Source code

Java tutorial

Introduction

Here is the source code for com.diozero.sandpit.imu.invensense.MPU9150DriverMqttPublisher.java

Source

package com.diozero.sandpit.imu.invensense;

/*
 * #%L
 * Organisation: mattjlewis
 * Project:      Device I/O Zero - IMU Sample App
 * Filename:     MPU9150DriverMqttPublisher.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 java.nio.ByteBuffer;

import org.apache.commons.math3.complex.Quaternion;
import org.apache.commons.math3.geometry.euclidean.threed.*;
import org.eclipse.paho.client.mqttv3.*;
import org.eclipse.paho.client.mqttv3.persist.MemoryPersistence;
import org.pmw.tinylog.Logger;

import com.diozero.api.I2CConstants;
import com.diozero.api.imu.*;
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 MPU9150DriverMqttPublisher implements MqttConstants {
    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;

    // MQTT variables
    private String mqttServer;
    private MqttClient mqttClient;

    // 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) {
        String mqtt_server = null;
        if (args.length > 0) {
            if (args[0].startsWith("--" + MQTT_SERVER_OPTION + "=")) {
                mqtt_server = args[0].substring(MQTT_SERVER_OPTION.length() + 3);
            }
        }
        new MPU9150DriverMqttPublisher(mqtt_server).run();
    }

    public MPU9150DriverMqttPublisher(String mqttServer) {
        // 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();
        }

        this.mqttServer = mqttServer;
    }

    public void run() {
        try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7,
                MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) {
            mpuInit(mpu);
            mqttInit();
            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]));

                mqttPublish(imu_data, ypr);
            } while (true);

        } catch (RuntimeIOException ioe) {
            Logger.error(ioe, "Error: {}", ioe);
        } catch (MqttException me) {
            Logger.error(me, "Error: {}", me);
        } finally {
            try {
                mqttClient.disconnect();
            } catch (Exception e) {
            }
        }
    }

    private void mqttPublish(ImuData imu_data, double[] ypr) throws MqttException {
        if (mqttClient != null) {
            MqttMessage message = new MqttMessage();
            message.setQos(MQTT_QOS_AT_MOST_ONCE);

            // 4 sets of 3 doubles, 1 set of 4 doubles
            byte[] bytes = new byte[4 * 3 * 8 + 1 * 4 * 8];
            ByteBuffer buffer = ByteBuffer.wrap(bytes);

            buffer.putDouble(imu_data.getCompass().getX());
            buffer.putDouble(imu_data.getCompass().getY());
            buffer.putDouble(imu_data.getCompass().getZ());

            buffer.putDouble(imu_data.getAccel().getX());
            buffer.putDouble(imu_data.getAccel().getY());
            buffer.putDouble(imu_data.getAccel().getZ());

            buffer.putDouble(imu_data.getGyro().getX());
            buffer.putDouble(imu_data.getGyro().getY());
            buffer.putDouble(imu_data.getGyro().getZ());

            buffer.putDouble(imu_data.getQuaternion().getQ0());
            buffer.putDouble(imu_data.getQuaternion().getQ1());
            buffer.putDouble(imu_data.getQuaternion().getQ2());
            buffer.putDouble(imu_data.getQuaternion().getQ3());

            buffer.putDouble(ypr[0]);
            buffer.putDouble(ypr[1]);
            buffer.putDouble(ypr[2]);

            buffer.flip();
            message.setPayload(bytes);
            mqttClient.publish(MQTT_TOPIC_IMU, message);
        }
    }

    private void mqttInit() throws MqttException {
        if (mqttServer == null) {
            return;
        }

        mqttClient = new MqttClient(mqttServer, "MPU9150", new MemoryPersistence());
        MqttConnectOptions con_opts = new MqttConnectOptions();
        con_opts.setCleanSession(true);
        mqttClient.connect(con_opts);
        Logger.debug("Connected to MQTT server '{}'", mqttServer);
    }

    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(MPU9150DriverMqttPublisher::tapCallback);
        dmp.dmp_register_android_orient_cb(MPU9150DriverMqttPublisher::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
    }
}