magnetic.updateSensorPhysics(rollDegree, pitchDegree, yawDegree);
magnetic.updateSensorReadoutValues();
magnetic.updateUserSettings();
- magnetic.setAzimuth();
- mPage.sendMessage(Magnetic.sensorNumber, magnetic.getAzimuth(), 0, 0 , 4, magnetic.getReadCompassX(), magnetic.getReadCompassY(), magnetic.getReadCompassZ());
+ //magnetic.setAzimuth();
+
+ /* orientation logic via accelerometer value */
+ int orient_yaw, orient_pitch, orient_roll;
+ orient_yaw = (int)(Math.atan2(-read_accelx, -read_accely) * (180.0/3.14) + 360) % 360;
+ orient_pitch = (int)(Math.atan2(-read_accely, -read_accelz) * (180.0/3.14)) % 180;
+ orient_roll = (int)(Math.atan2(read_accelx, read_accelz) * (180.0/3.14)) % 180;
+
+ if(orient_roll > 90)
+ orient_roll = 180 - orient_roll;
+ else if(orient_roll < -90)
+ orient_roll = -180 - orient_roll;
+
+ mPage.sendMessage(Magnetic.sensorNumber, orient_yaw, -orient_pitch, orient_roll , 4, magnetic.getReadCompassX(), magnetic.getReadCompassY(), magnetic.getReadCompassZ());
+ //mPage.sendMessage(Magnetic.sensorNumber, magnetic.getAzimuth(), 0, 0 , 4, magnetic.getReadCompassX(), magnetic.getReadCompassY(), magnetic.getReadCompassZ());
//System.out.println("geomagnetic : X " + magnetic.getReadCompassX() + " Y : " + magnetic.getReadCompassY() + " Z : " + magnetic.getReadCompassZ());
}
});