Beispiel #1
0
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;
}
Beispiel #2
0
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;

}