float *vectorFromAngles(float *angles)
{
float *result;
float **tempA;
float **tempB;
float **applyTrans;
tempA = matrixRotation('x', angles[1]);
tempB = matrixRotation('y', angles[2]);
applyTrans = matrixIdent(4);
applyTrans = matrixUpdate(applyTrans, matrixMult(applyTrans, tempA, 4), 4);
applyTrans = matrixUpdate(applyTrans, matrixMult(applyTrans, tempB, 4), 4);
result = vectorNull(4);
result[2] = 1;
result[3] = 1;
result = vectorUpdate(result, vectorMatrixMult(result, applyTrans, 4));
matrixFree(tempA, 4);
matrixFree(tempB, 4);
matrixFree(applyTrans, 4);
return result;
}
void SensorFusion::run(const Eigen::Vector3f& accel, const Eigen::Vector3f& magnetom, const Eigen::Vector3f& gyro, Eigen::Vector3f& ypr)
{
timestampPrevious = timestamp;
timestamp = highResClock.now();
//if(timestamp > timestamp_old)
// G_Dt = (float)(timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
//else G_Dt = 0;
d = timestamp - timestampPrevious;
G_Dt = (float)std::chrono::duration_cast<std::chrono::milliseconds>(d).count() / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
compassHeading(magnetom);
matrixUpdate(accel, gyro);
normalize();
driftCorrection();
eluerAngles();
ypr(0) = yaw;
ypr(1) = pitch;
ypr(2) = roll;
}