Je vous présente un petit bout de calcul qui permet d'installer ma centrale inertielle dans n'importe quelle position et de repartir avec un quaternion [1,0,0,0] à partir de sa position courante.
Je suis certain que je peux GRANDEMENT simplifier tout ça mais j'ai trouvé cette solution un peu en tâtonnant, je n'ai donc pas réussi à faire plus simple (j'avais pourtant trouver quelque chose de prometteur: multiplier mon quaternion actuel par le conjugué du quaternion de la position initiale mais ça ne donnait pas tout à fait le même résultat):
- Code: Tout sélectionner
private bool bSetSoftTareAsked = false;
private bool bResetSoftTareAsked = true;
// Bouton Tare cliquer
private void btnSetSoftTare_Click(object sender, EventArgs e)
{
bSetSoftTareAsked = true;
}
// Mise à jour du quaternion (100Hz)
void UpdateQuaternion(QuaternionData quat)
{
// Si demande de tare
if (bSetSoftTareAsked)
{
double OffsetPitch = deg2rad(quat.ConvertToEulerAngles()[0]);
double OffsetRoll = deg2rad(quat.ConvertToEulerAngles()[1]);
double OffsetYaw = deg2rad(quat.ConvertToEulerAngles()[2]);
rotPitch = new QuaternionData(new float[] { (float)Math.Cos(OffsetPitch / 2), (float)Math.Sin(OffsetPitch / 2), 0, 0 });
rotRoll = new QuaternionData(new float[] { (float)Math.Cos(OffsetRoll / 2), 0, (float)Math.Sin(OffsetRoll / 2), 0 });
rotYaw = new QuaternionData(new float[] { (float)Math.Cos(OffsetYaw / 2), 0, 0, (float)Math.Sin(OffsetYaw / 2) });
// demande traitée
bSetSoftTareAsked = false;
}
quat = (quat.ConvertToConjugate() *rotPitch.ConvertToConjugate()).ConvertToConjugate();
quat = (quat.ConvertToConjugate() * rotRoll.ConvertToConjugate()).ConvertToConjugate();
quat = (quat.ConvertToConjugate() * rotYaw.ConvertToConjugate()).ConvertToConjugate();
QuaternionDataReceived(this, quat);
}
Merci à vous si vous trouver une simplification :++: