1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33
|
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);
} |
Partager