// This function performs the integration of the gyroscope data.
// It writes the gyroscope based orientation into gyroOrientation.
void gyroFunction(void)
{
//static double NS2S = 1.0f / 1000000000.0f;
//double timestamp;
// don't start until first accelerometer/magnetometer orientation has been acquired
//if (accMagOrientation == null)
//return;
// initialisation of the gyroscope based rotation matrix
static bool initState = true;
if (initState) {
double initMatrix[9];
getRotationMatrixFromOrientation(initMatrix, accMagOrientation);
double test[3];
getOrientation(test, initMatrix);
matrixMultiplication(gyroMatrix, gyroMatrix, initMatrix);
initState = false;
}
// copy the new gyro values into the gyro array
// convert the raw gyro data into a rotation vector
double deltaVector[4];
//const double dT = (timer_systime() - timestamp) * NS2S;
//System.arraycopy(event.values, 0, gyro, 0, 3);
getRotationVectorFromGyro(deltaVector, G_Dt);
// measurement done, save current time for next interval
//timestamp = timer_systime();
// convert rotation vector into rotation matrix
double deltaMatrix[9];
getRotationMatrixFromVector(deltaMatrix, deltaVector);
// apply the new rotation interval on the gyroscope based rotation matrix
matrixMultiplication(gyroMatrix, gyroMatrix, deltaMatrix);
// get the gyroscope based orientation from the rotation matrix
getOrientation(gyroMatrix, gyroOrientation);
}
void SensorCollector::ProcessSensorEvents()
{
int result = -1;
ASensorEvent sensorEvent;
//Assume this is all gyro for now
while (ASensorEventQueue_getEvents(sensorQueue,&sensorEvent, 1) > 0 )
{
LOGV(LOGTAG_SENSOR,"Processing sensor event");
//First time reading an event, store the timestamp and go to the next one
if (lastTimestamp == 0)
{
lastTimestamp = sensorEvent.timestamp;
rotationVector = cv::Mat::eye(4,4,CV_32F);
continue;
}
if (resetNextTime)
{
rotationVector = cv::Mat::eye(4,4,CV_32F);
resetNextTime = false;
}
double deltaT = (sensorEvent.timestamp - lastTimestamp) / 1000000000.0;
float axisX = -sensorEvent.data[1];
float axisY = -sensorEvent.data[0];
float axisZ = -sensorEvent.data[2];
// Calculate the angular speed of the sample
float omegaMagnitude = sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);
LOGD(LOGTAG_SENSOR,"OmegaMagnitue=%f",omegaMagnitude);
// Normalize the rotation vector if it's big enough to get the axis
if (omegaMagnitude > .001f)
{
axisX /= omegaMagnitude;
axisY /= omegaMagnitude;
axisZ /= omegaMagnitude;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
float thetaOverTwo = omegaMagnitude * deltaT / 2.0f;
float sinThetaOverTwo = sin(thetaOverTwo);
float cosThetaOverTwo = cos(thetaOverTwo);
float deltaRotationVector[] = {0,0,0,0};
deltaRotationVector[0] = sinThetaOverTwo * axisX;
deltaRotationVector[1] = sinThetaOverTwo * axisY;
deltaRotationVector[2] = sinThetaOverTwo * axisZ;
deltaRotationVector[3] = cosThetaOverTwo;
/* double deltaT = (sensorEvent.timestamp - lastTimestamp) / 1000000000.0;
double data[] = {sensorEvent.data[0],sensorEvent.data[1],sensorEvent.data[2]};
cv::Mat deltaVector = cv::Mat(3,1,CV_64F,data);
deltaVector *= deltaT;
rotationVector += deltaVector;*/
lastTimestamp = sensorEvent.timestamp;
LOGD(LOGTAG_SENSOR,"Creating rotation matrix from vector (%f,%f,%f,%f)",deltaRotationVector[0],deltaRotationVector[1],deltaRotationVector[2],deltaRotationVector[3]);
cv::Mat deltaMat = getRotationMatrixFromVector(deltaRotationVector);
rotationVector *= deltaMat;
}
}
