public void run() {
accMagMatrix = getRotationMatrixFromOrientation(accMagOrientation);
// Convert from float[9] to Matrix
NCompassMatrix mat = new NCompassMatrix();
mat.set(
accMagMatrix[0],
accMagMatrix[1],
accMagMatrix[2],
accMagMatrix[3],
accMagMatrix[4],
accMagMatrix[5],
accMagMatrix[6],
accMagMatrix[7],
accMagMatrix[8]);
// Find position relative to magnetic north //
magneticCompensatedCoord.toIdentity(); // Identity matrix
synchronized (mageticNorthCompensation) {
magneticCompensatedCoord.prod(
mageticNorthCompensation); // Cross product the matrix with the magnetic north
// compensation
}
magneticCompensatedCoord.prod(mat); // Cross product with the world coordinates
magneticCompensatedCoord.invert(); // Invert the matrix
NCompassARData.setRotationMatrix(
magneticCompensatedCoord); // Set the rotation matrix (used to translate all object from
// lat/lon to x/y/z)
}
/** Call this onStart */
public void onStart() {
// Counter-clockwise rotation at -90 degrees around the x-axis
float angleX = -90 * toRadians;
xAxisRotation.set(
1f,
0f,
0f,
0f,
FloatMath.cos(angleX),
-FloatMath.sin(angleX),
0f,
FloatMath.sin(angleX),
FloatMath.cos(angleX));
}
public void run() {
/* Fix for 180 <--> -180 transition problem:
* Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive.
* If so, add 360 (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360 from the result
* if it is greater than 180. This stabilizes the output in positive-to-negative-transition cases.
*/
// azimuth
if (gyroOrientation[0] < -0.5f * Pi && accMagOrientation[0] > 0f) {
fusedOrientation[0] =
COMBINATION_FILTER_COEFFICIENT * (gyroOrientation[0] + TwoPi)
+ ONEMINUS_COMBINATION_COEFF * accMagOrientation[0];
fusedOrientation[0] -= (fusedOrientation[0] > Pi) ? TwoPi : 0f;
} else if (accMagOrientation[0] < -0.5f * Pi && gyroOrientation[0] > 0f) {
fusedOrientation[0] =
COMBINATION_FILTER_COEFFICIENT * gyroOrientation[0]
+ ONEMINUS_COMBINATION_COEFF * (accMagOrientation[0] + TwoPi);
fusedOrientation[0] -= (fusedOrientation[0] > Pi) ? TwoPi : 0f;
} else {
fusedOrientation[0] =
COMBINATION_FILTER_COEFFICIENT * gyroOrientation[0]
+ ONEMINUS_COMBINATION_COEFF * accMagOrientation[0];
}
// pitch
if (gyroOrientation[1] < -0.5f * Pi && accMagOrientation[1] > 0f) {
fusedOrientation[1] =
COMBINATION_FILTER_COEFFICIENT * (gyroOrientation[1] + TwoPi)
+ ONEMINUS_COMBINATION_COEFF * accMagOrientation[1];
fusedOrientation[1] -= (fusedOrientation[1] > Pi) ? TwoPi : 0f;
} else if (accMagOrientation[1] < -0.5f * Pi && gyroOrientation[1] > 0f) {
fusedOrientation[1] =
COMBINATION_FILTER_COEFFICIENT * gyroOrientation[1]
+ ONEMINUS_COMBINATION_COEFF * (accMagOrientation[1] + TwoPi);
fusedOrientation[1] -= (fusedOrientation[1] > Pi) ? TwoPi : 0f;
} else {
fusedOrientation[1] =
COMBINATION_FILTER_COEFFICIENT * gyroOrientation[1]
+ ONEMINUS_COMBINATION_COEFF * accMagOrientation[1];
}
// roll
if (gyroOrientation[2] < -0.5f * Pi && accMagOrientation[2] > 0f) {
fusedOrientation[2] =
COMBINATION_FILTER_COEFFICIENT * (gyroOrientation[2] + TwoPi)
+ ONEMINUS_COMBINATION_COEFF * accMagOrientation[2];
fusedOrientation[2] -= (fusedOrientation[2] > Pi) ? TwoPi : 0f;
} else if (accMagOrientation[2] < -0.5f * Pi && gyroOrientation[2] > 0f) {
fusedOrientation[2] =
COMBINATION_FILTER_COEFFICIENT * gyroOrientation[2]
+ ONEMINUS_COMBINATION_COEFF * (accMagOrientation[2] + TwoPi);
fusedOrientation[2] -= (fusedOrientation[2] > Pi) ? TwoPi : 0f;
} else {
fusedOrientation[2] =
COMBINATION_FILTER_COEFFICIENT * gyroOrientation[2]
+ ONEMINUS_COMBINATION_COEFF * accMagOrientation[2];
}
// overwrite gyro matrix and orientation with fused orientation to compensate gyro drift
gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
// Convert from float[9] to Matrix
NCompassMatrix mat = new NCompassMatrix();
mat.set(
gyroMatrix[0],
gyroMatrix[1],
gyroMatrix[2],
gyroMatrix[3],
gyroMatrix[4],
gyroMatrix[5],
gyroMatrix[6],
gyroMatrix[7],
gyroMatrix[8]);
// Find position relative to magnetic north
magneticCompensatedCoord.toIdentity(); // Identity matrix
synchronized (mageticNorthCompensation) {
magneticCompensatedCoord.prod(
mageticNorthCompensation); // Cross product the matrix with the magnetic north
// compensation
}
magneticCompensatedCoord.prod(mat); // Cross product with the world coordinates
magneticCompensatedCoord.invert(); // Invert the matrix
NCompassARData.setRotationMatrix(
magneticCompensatedCoord); // Set the rotation matrix (used to translate all object from
// lat/lon to x/y/z)
}
/** This function registers sensor listeners for the accelerometer, magnetometer and gyroscope. */
private void initListeners() {
sensorMgr = (SensorManager) mContext.getSystemService(Activity.SENSOR_SERVICE);
fuseTimer = new Timer();
sensors = sensorMgr.getSensorList(Sensor.TYPE_ACCELEROMETER);
if (sensors.size() > 0) {
sensorGravRunning = false;
sensorGrav = sensorMgr.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
sensorMgr.registerListener(this, sensorGrav, sensorRate);
}
sensors = sensorMgr.getSensorList(Sensor.TYPE_MAGNETIC_FIELD);
if (sensors.size() > 0) {
sensorMagRunning = false;
sensorMag = sensorMgr.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
sensorMgr.registerListener(this, sensorMag, sensorRate);
}
sensors = sensorMgr.getSensorList(Sensor.TYPE_GYROSCOPE);
if (sensors.size() > 0) {
sensorGyroRunning = false;
sensorGyro = sensorMgr.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
sensorMgr.registerListener(this, sensorGyro, sensorRate);
fuseTimer.scheduleAtFixedRate(
new calculateFusedOrientationTask(), 1000, TIME_CONSTANT); // use the gyro
NLog.d(TAG, "initListeners() - " + "Gyro found");
} else {
fuseTimer.scheduleAtFixedRate(
new calculateUnfusedOrientationTask(), 1000, TIME_CONSTANT); // don't use the gyro
NLog.d(TAG, "initListeners() - " + "No gyro found");
}
locationMgr = (LocationManager) mContext.getSystemService(Context.LOCATION_SERVICE);
locationMgr.requestLocationUpdates(LocationManager.GPS_PROVIDER, MIN_TIME, MIN_DISTANCE, this);
try {
Location gps = locationMgr.getLastKnownLocation(LocationManager.GPS_PROVIDER);
Location network = locationMgr.getLastKnownLocation(LocationManager.NETWORK_PROVIDER);
if (gps != null) onLocationChanged(gps);
else if (network != null) onLocationChanged(network);
else onLocationChanged(NCompassARData.hardFix);
} catch (Exception ex2) {
onLocationChanged(NCompassARData.hardFix);
}
float angleY = -90 * toRadians;
gmf =
new GeomagneticField(
(float) NCompassARData.getCurrentLocation().getLatitude(),
(float) NCompassARData.getCurrentLocation().getLongitude(),
(float) NCompassARData.getCurrentLocation().getAltitude(),
System.currentTimeMillis());
angleY = -gmf.getDeclination() * toRadians;
synchronized (mageticNorthCompensation) {
mageticNorthCompensation.toIdentity();
// Counter-clockwise rotation at negative declination around the y-axis
// note2: declination is the difference between true north and magnetic north
mageticNorthCompensation.set(
FloatMath.cos(angleY),
0f,
FloatMath.sin(angleY),
0f,
1f,
0f,
-FloatMath.sin(angleY),
0f,
FloatMath.cos(angleY));
// Rotate the matrix to match the orientation
mageticNorthCompensation.prod(xAxisRotation);
}
}