mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-20 17:11:20 -08:00
113 lines
4.0 KiB
Java
113 lines
4.0 KiB
Java
package dev.slimevr.vr.trackers;
|
|
|
|
import java.nio.ByteBuffer;
|
|
|
|
public class MPUTracker extends IMUTracker {
|
|
|
|
public ConfigurationData newCalibrationData;
|
|
|
|
public MPUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) {
|
|
super(trackerId, name, descriptiveName, server);
|
|
}
|
|
|
|
public static class ConfigurationData {
|
|
|
|
//acel offsets and correction matrix
|
|
float[] A_B = new float[3];
|
|
float[][] A_Ainv = new float[3][3];
|
|
// mag offsets and correction matrix
|
|
float[] M_B = new float[3];
|
|
float[][] M_Ainv = new float[3][3];
|
|
//raw offsets, determined for gyro at rest
|
|
float[] G_off = new float[3];
|
|
int deviceId = -1;
|
|
int deviceMode = -1;
|
|
|
|
public ConfigurationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) {
|
|
A_B[0] = (float) accelBasis[0];
|
|
A_B[1] = (float) accelBasis[1];
|
|
A_B[2] = (float) accelBasis[2];
|
|
|
|
A_Ainv[0][0] = (float) accelAInv[0];
|
|
A_Ainv[0][1] = (float) accelAInv[1];
|
|
A_Ainv[0][2] = (float) accelAInv[2];
|
|
A_Ainv[1][0] = (float) accelAInv[3];
|
|
A_Ainv[1][1] = (float) accelAInv[4];
|
|
A_Ainv[1][2] = (float) accelAInv[5];
|
|
A_Ainv[2][0] = (float) accelAInv[6];
|
|
A_Ainv[2][1] = (float) accelAInv[7];
|
|
A_Ainv[2][2] = (float) accelAInv[8];
|
|
|
|
M_B[0] = (float) magBasis[0];
|
|
M_B[1] = (float) magBasis[1];
|
|
M_B[2] = (float) magBasis[2];
|
|
|
|
M_Ainv[0][0] = (float) magAInv[0];
|
|
M_Ainv[0][1] = (float) magAInv[1];
|
|
M_Ainv[0][2] = (float) magAInv[2];
|
|
M_Ainv[1][0] = (float) magAInv[3];
|
|
M_Ainv[1][1] = (float) magAInv[4];
|
|
M_Ainv[1][2] = (float) magAInv[5];
|
|
M_Ainv[2][0] = (float) magAInv[6];
|
|
M_Ainv[2][1] = (float) magAInv[7];
|
|
M_Ainv[2][2] = (float) magAInv[8];
|
|
|
|
G_off[0] = (float) gyroOffset[0];
|
|
G_off[1] = (float) gyroOffset[1];
|
|
G_off[2] = (float) gyroOffset[2];
|
|
}
|
|
|
|
public ConfigurationData(ByteBuffer buffer) {
|
|
deviceMode = buffer.getInt();
|
|
deviceId = buffer.getInt();
|
|
// Data is read in reverse, because it was reversed when sending
|
|
G_off[2] = buffer.getFloat();
|
|
G_off[1] = buffer.getFloat();
|
|
G_off[0] = buffer.getFloat();
|
|
|
|
M_Ainv[2][2] = buffer.getFloat();
|
|
M_Ainv[2][1] = buffer.getFloat();
|
|
M_Ainv[2][0] = buffer.getFloat();
|
|
M_Ainv[1][2] = buffer.getFloat();
|
|
M_Ainv[1][1] = buffer.getFloat();
|
|
M_Ainv[1][0] = buffer.getFloat();
|
|
M_Ainv[0][2] = buffer.getFloat();
|
|
M_Ainv[0][1] = buffer.getFloat();
|
|
M_Ainv[0][0] = buffer.getFloat();
|
|
|
|
M_B[2] = buffer.getFloat();
|
|
M_B[1] = buffer.getFloat();
|
|
M_B[0] = buffer.getFloat();
|
|
|
|
A_Ainv[2][2] = buffer.getFloat();
|
|
A_Ainv[2][1] = buffer.getFloat();
|
|
A_Ainv[2][0] = buffer.getFloat();
|
|
A_Ainv[1][2] = buffer.getFloat();
|
|
A_Ainv[1][1] = buffer.getFloat();
|
|
A_Ainv[1][0] = buffer.getFloat();
|
|
A_Ainv[0][2] = buffer.getFloat();
|
|
A_Ainv[0][1] = buffer.getFloat();
|
|
A_Ainv[0][0] = buffer.getFloat();
|
|
|
|
A_B[2] = buffer.getFloat();
|
|
A_B[1] = buffer.getFloat();
|
|
A_B[0] = buffer.getFloat();
|
|
}
|
|
|
|
public String toTextMatrix() {
|
|
StringBuilder sb = new StringBuilder();
|
|
sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", A_B[0], A_B[1], A_B[2]));
|
|
sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", A_Ainv[0][0], A_Ainv[0][1], A_Ainv[0][2]));
|
|
sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", A_Ainv[1][0], A_Ainv[1][1], A_Ainv[1][2]));
|
|
sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", A_Ainv[2][0], A_Ainv[2][1], A_Ainv[2][2]));
|
|
sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", M_B[0], M_B[1], M_B[2]));
|
|
sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", M_Ainv[0][0], M_Ainv[0][1], M_Ainv[0][2]));
|
|
sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", M_Ainv[1][0], M_Ainv[1][1], M_Ainv[1][2]));
|
|
sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", M_Ainv[2][0], M_Ainv[2][1], M_Ainv[2][2]));
|
|
sb.append(String.format("{%8.2f, %8.2f, %8.2f}};\n", G_off[0], G_off[1], G_off[2]));
|
|
|
|
return sb.toString();
|
|
}
|
|
}
|
|
}
|