public static Quaternion operator * (Quaternion q1, Quaternion q2)
{
Quaternion r = new Quaternion();
r.x = q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x;
r.y = -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y;
r.z = q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z;
r.w = -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w;
return r;
}
public Quaternion Slerp( Quaternion To, float Tween )
{
float cosOmega , startWeight , endWeight , absCosOmega;
float oneOverSineOmega;
float omega , tweenAngle;
// Recover the cosine of the angle between input quaternions.
cosOmega = this.DotProduct( To );
// If this cosine is negative , we want to interpolate to -end rather than end.
absCosOmega = Math.Abs( cosOmega );
// Compute the intepolation weights.
if( absCosOmega <= 0.9998f ) {
// The quaternions were far enough apart to use the real slerp operation.
omega = (float)Math.Acos( absCosOmega );
// Now determine the tween angle.
tweenAngle = Tween * omega;
// Lastly , calculate the weights.
//#if 1
oneOverSineOmega = (float)(1.0 / Math.Sin( omega ));
//#else
//oneOverSineOmega = reciprocal_sqrt( 1 - (cosOmega * cosOmega) );
//#endif
startWeight = (float)Math.Sin( omega - tweenAngle ) * oneOverSineOmega;
endWeight = (float)Math.Sin( tweenAngle ) * oneOverSineOmega;
} else {
// The quaternions were _really_ close together (less than one degree); use a linear blend.
// This avoids the roundoff-error-prone quotient of two very small numbers in the slerp ratios.
startWeight = 1.0f - Tween;
endWeight = Tween;
}
// If we need to negate , do so now.
//__fsel( cosOmega, endWeight, -endWeight );
endWeight = cosOmega >= 0 ? endWeight : -endWeight ;
// Now blend the quaternions.
Quaternion r = new Quaternion();
r.w = startWeight * w + endWeight * To.w;
r.x = startWeight * x + endWeight * To.x;
r.y = startWeight * y + endWeight * To.y;
r.z = startWeight * z + endWeight * To.z;
return r;
}
public void From( Quaternion Q )
{
float qx2 = Q.x * Q.x * 2;
float qy2 = Q.y * Q.y * 2;
float qz2 = Q.z * Q.z * 2;
float qxqy2 = Q.x * Q.y * 2;
float qxqz2 = Q.x * Q.z * 2;
float qxqw2 = Q.x * Q.w * 2;
float qyqz2 = Q.y * Q.z * 2;
float qyqw2 = Q.y * Q.w * 2;
float qzqw2 = Q.z * Q.w * 2;
m[0, 0] = 1 - qy2 - qz2;
m[0, 1] = qxqy2 - qzqw2;
m[0, 2] = qxqz2 + qyqw2;
m[1, 0] = qxqy2 + qzqw2;
m[1, 1] = 1 - qx2 - qz2;
m[1, 2] = qyqz2 - qxqw2;
m[2, 0] = qxqz2 - qyqw2;
m[2, 1] = qyqz2 + qxqw2;
m[2, 2] = 1 - qx2 - qy2;
}
public float DotProduct( Quaternion q )
{
return w*q.w + x*q.x + y*q.y + z*q.z;
}
void ComputeAngularVelocities()
{
Vector Axis;
Quaternion qrot = follow.Conjugate() * quat;
float angle = qrot.ToAngleAxis( out Axis );
Vector angularDisplacement = Axis * angle; // * Mathf.Deg2Rad;
//Vector angularSpeed = angularDisplacement / Time.deltaTime;
Velocities = angularDisplacement; // It's possible for this to accumulate extra spins, apparently - will need to be clamped -PI to +PI
follow = follow.Slerp( quat, 0.05f );
matFollow.From( follow );
}
public Quaternion To( Quaternion b )
{
return this * b.Conjugate();
}
private void UpdateRadio()
{
const float RadioScale = 1024.0f;
const float Deg2Rad = 3.141592654f / 180.0f;
float MaxRate = 180.0f; // degrees per second
float UpdateRate = 250.0f / 8.0f;
float RateScale = ((MaxRate / UpdateRate) / RadioScale) * Deg2Rad * 0.5f; // Quaternions use half-angles, so mult everything by 0.5
if(radio.Gear < -300)
{
// Manual mode
float xrot = (float)radio.Elev * RateScale; // Individual scalars for channel sensitivity
float yrot = (float)radio.Rudd * RateScale;
float zrot = (float)radio.Aile * -RateScale;
// Take Aile/Elev/Rudd and create an incremental quaternion
Quaternion qrot = new Quaternion( 0, xrot, yrot, zrot );
// rotate the current desired orientation by it
desired = desired + (desired * qrot); // Overall scale (meant as time increment)
desired = desired.Normalize();
// Need to extract "heading" from the IMU in case we switch to auto again
ocCube.Quat = desired;
}
else if(radio.Gear > 300)
{
// Auto-level
float MaxBank = 45.0f;
float ScaleMult = (MaxBank / RadioScale) * Deg2Rad * 0.5f; // Quaternions use half-angles, so mult everything by 0.5
heading += (float)radio.Rudd * RateScale;
float rx = (float)radio.Elev * ScaleMult; // Individual scalars for channel sensitivity
float ry = heading;
float rz = (float)radio.Aile * -ScaleMult;
// Convert Aile/Elev/heading directly into desired orientation quaternion
float csx = (float)Math.Cos(rx);
float csy = (float)Math.Cos(ry);
float csz = (float)Math.Cos(rz);
float snx = (float)Math.Sin(rx);
float sny = (float)Math.Sin(ry);
float snz = (float)Math.Sin(rz);
//Quaternion qz = new Quaternion( csz, 0, 0, snz );
//Quaternion qy = new Quaternion( csy, 0, sny, 0 );
//Quaternion qx = new Quaternion( csx, snx, 0, 0 );
//desired = qy * qz * qx;
// Simplifies to:
float snycsx = sny * csx;
float snysnx = sny * snx;
float csycsz = csy * csz;
float csysnz = csy * snz;
desired.x = snycsx * snz + csycsz * snx;
desired.y = snycsx * csz + csysnz * snx;
desired.z = csysnz * csx - snysnx * csz;
desired.w = csycsz * csx - snysnx * snz;
ocCube.Quat = desired;
}
// Compute rotation from current orientation to desired
// Vector Axis;
// Quaternion qrot = follow.To( quat );
// float angle = qrot.ToAngleAxis( out Axis );
// Vector Velocities = Axis * angle;
// apply through PIDs
}
private void ProcessPacket( byte packetType )
{
QTail += 3; // Skip the header signature and the packet type values
// Figure out what mode we're in and read the complete packet
switch( packetType )
{
case 0x01: // Receiver test packet - currently 4 words of data
Thro = GetCommWord();
Aile = GetCommWord();
Elev = GetCommWord();
Rudd = GetCommWord();
Gear = GetCommWord();
Aux1 = GetCommWord();
Aux2 = GetCommWord();
Aux3 = GetCommWord();
if(currentMode == Mode.RadioTest)
{
rsLeft.XValue = (float)Rudd;
rsLeft.YValue = (float)Thro;
rsRight.XValue = (float)Aile;
rsRight.YValue = (float)Elev;
lblGear.Text = Gear.ToString();
lblAux1.Text = Aux1.ToString();
lblAux2.Text = Aux2.ToString();
lblAux3.Text = Aux3.ToString();
if(Gear < -512) {
lblFlightMode.Text = "Assisted";
}
else if(Gear > 512) {
lblFlightMode.Text = "Manual";
}
else {
lblFlightMode.Text = "Assisted"; // Will become auto
}
}
//GotPacket = true;
break;
case 0x02: // Sensor test packet
GyroTemp = GetCommWord();
GyroX = GetCommWord();
GyroY = GetCommWord();
GyroZ = GetCommWord();
AccelX = GetCommWord();
AccelY = GetCommWord();
AccelZ = GetCommWord();
MagX = GetCommWord();
MagY = GetCommWord();
MagZ = GetCommWord();
Battery = GetCommWord();
prevAlt = Alt;
Alt = GetCommLong();
AltTemp = GetCommLong();
AltEst = GetCommLong();
// If we're on the sensor test tab, update those UI controls
if(currentMode == Mode.SensorTest)
{
gGyroX.Value = (float)GyroX;
gGyroY.Value = (float)GyroY;
gGyroZ.Value = (float)GyroZ;
gAccelX.Value = (float)AccelX;
gAccelY.Value = (float)AccelY;
gAccelZ.Value = (float)AccelZ;
gGyroTemp.Value = (float)GyroTemp;
gMagX.Value = (float)MagX;
gMagY.Value = (float)MagY;
gMagZ.Value = (float)MagZ;
lblBattery.Text = string.Format( "{0:0.00} v", (float)Battery / 100.0 );
// Compute a heading from the magnetometer readings (not tilt compensated)
//gHeading.Value = (float)(Math.Atan2( MagX, MagY ) * 1800.0/Math.PI);
gHeading.Value = ComputeTiltCompensatedHeading();
float altTempDegrees = 42.5f + (float)AltTemp / 480.0f;
lblAltimeter.Text = string.Format( "{0:0.000} m", (float)AltEst / 1000.0f ); // Altimeter output is in mm
lblAltimeterTemp.Text = string.Format( "{0:0.00}*C", altTempDegrees );
int[] sample = {AltEst, Alt, Alt };
SampleCounter++;
bool DoUpdate = (SampleCounter & 15) == 15;
gAltimeter.AddSample( sample, true );
if( DoUpdate ) gAltimeter.UpdateStats();
}
// If we're on the sensor calibration tab, update the graph / line fit controls
if(currentMode == Mode.GyroCalibration)
{
LineFit.Sample lfSample = new LineFit.Sample();
lfSample.t = GyroTemp;
lfSample.x = GyroX;
lfSample.y = GyroY;
lfSample.z = GyroZ;
SampleCounter++;
bool DoRedraw = (tcMainTabs.SelectedIndex == 3) && ((SampleCounter & 7) == 7);
lfGraph.AddSample( lfSample, DoRedraw );
if( DoRedraw == false )
{
gCalibTemp.Value = (float)GyroTemp;
gCalibX.Value = (float)GyroX;
gCalibY.Value = (float)GyroY;
gCalibZ.Value = (float)GyroZ;
int scaleX = 0;
int scaleY = 0;
int scaleZ = 0;
if( Math.Abs( lfGraph.dSlope.x) > 0.00001)
scaleX = (int)Math.Round( 1.0 / lfGraph.dSlope.x );
if( Math.Abs( lfGraph.dSlope.y) > 0.00001)
scaleY = (int)Math.Round( 1.0 / lfGraph.dSlope.y );
if( Math.Abs( lfGraph.dSlope.z) > 0.00001)
scaleZ = (int)Math.Round(1.0 / lfGraph.dSlope.z);
int offsetX = (int)Math.Round( lfGraph.dIntercept.x );
int offsetY = (int)Math.Round( lfGraph.dIntercept.y );
int offsetZ = (int)Math.Round( lfGraph.dIntercept.z );
if(Math.Abs( scaleX ) < 1024.0f)
gxScale.Text = scaleX.ToString();
else
gxScale.Text = "0";
if(Math.Abs( scaleY ) < 1024.0f)
gyScale.Text = scaleY.ToString();
else
gyScale.Text = "0";
if(Math.Abs( scaleZ ) < 1024.0f)
gzScale.Text = scaleZ.ToString();
else
gzScale.Text = "0";
gxOffset.Text = offsetX.ToString();
gyOffset.Text = offsetY.ToString();
gzOffset.Text = offsetZ.ToString();
}
}
if(currentMode == Mode.AccelCalibration)
{
gAccelXCal.Value = (float)AccelX;
gAccelYCal.Value = (float)AccelY;
gAccelZCal.Value = (float)AccelZ;
}
break;
case 0x06: // Vibration test packet
GyroX = GetCommWord();
GyroY = GetCommWord();
GyroZ = GetCommWord();
if(currentMode == Mode.VibrationTest)
{
int[] gySample = new int[3];
gySample[0] = GyroX;
gySample[1] = GyroY;
gySample[2] = GyroZ;
grGyro.AddSample( gySample, true );
SampleCounter++;
if((SampleCounter & 255) == 255)
{
grGyro.UpdateStats();
lblGXMin.Text = grGyro.Mins[0].ToString();
lblGXMax.Text = grGyro.Maxs[0].ToString();
lblGXAvg.Text = grGyro.Avgs[0].ToString( "00.0000" );
lblGXVar.Text = grGyro.Vars[0].ToString( "0.00000" );
lblGYMin.Text = grGyro.Mins[1].ToString();
lblGYMax.Text = grGyro.Maxs[1].ToString();
lblGYAvg.Text = grGyro.Avgs[1].ToString( "00.0000" );
lblGYVar.Text = grGyro.Vars[1].ToString( "0.00000" );
lblGZMin.Text = grGyro.Mins[2].ToString();
lblGZMax.Text = grGyro.Maxs[2].ToString();
lblGZAvg.Text = grGyro.Avgs[2].ToString( "00.0000" );
lblGZVar.Text = grGyro.Vars[2].ToString( "0.00000" );
}
}
break;
case 0x04: // IMU Test - Update the orientation quaternion
Quaternion q = new Quaternion();
q.x = GetCommFloat();
q.y = GetCommFloat();
q.z = GetCommFloat();
q.w = GetCommFloat();
Pitch = GetCommWord();
Roll = GetCommWord();
Yaw = GetCommWord();
ocOrientation.Quat = q;
gPitch.Value = Pitch;
gRoll.Value = Roll;
gYaw.Value = Yaw;
break;
case 0x05: // IMU Comparison - Test different orientation update methods
GyroX = GetCommWord();
GyroY = GetCommWord();
GyroZ = GetCommWord();
AccelX = GetCommWord();
AccelY = GetCommWord();
AccelZ = GetCommWord();
prevAlt = Alt;
Alt = GetCommLong();
ComputeQuaternionOrientations();
ocCompQ1.Quat = Q1;
ocCompQ2.Quat = Q2;
break;
case 7: // Everything mode
{
int phase = GetCommByte();
switch(phase)
{
case 0:
Everything.Thro = GetCommShort();
Everything.Aile = GetCommShort();
Everything.Elev = GetCommShort();
Everything.Rudd = GetCommShort();
Everything.Gear = GetCommShort(); // First 20 bytes
Everything.Aux1 = GetCommShort();
Everything.Aux2 = GetCommShort();
Everything.Aux3 = GetCommShort();
Everything.BatteryVolts = GetCommShort();
Everything.padding = GetCommShort();
rjE_Left.XValue = Everything.Rudd;
rjE_Left.YValue = Everything.Thro;
rjE_Right.XValue = Everything.Aile;
rjE_Right.YValue = Everything.Elev;
tbGear.Value = clamp( Everything.Gear + 1024, 0, 2048);
tbAux1.Value = clamp( Everything.Aux1 + 1024, 0, 2048);
tbAux2.Value = clamp( Everything.Aux2 + 1024, 0, 2048);
tbAux3.Value = clamp( Everything.Aux3 + 1024, 0, 2048);
break;
case 1:
Everything.Temp = GetCommShort();
Everything.GyroX = GetCommShort();
Everything.GyroY = GetCommShort();
Everything.GyroZ = GetCommShort();
Everything.AccelX = GetCommShort(); // 2nd 20 bytes
Everything.AccelY = GetCommShort();
Everything.AccelZ = GetCommShort();
Everything.MagX = GetCommShort();
Everything.MagY = GetCommShort();
Everything.MagZ = GetCommShort();
int[] gySample = { Everything.GyroX, Everything.GyroY, Everything.GyroZ };
grE_Gyro.AddSample( gySample, true );
int[] accSample = { Everything.AccelX, Everything.AccelY, Everything.AccelZ };
grE_Accel.AddSample( accSample, true );
SampleCounter++;
bool DoUpdate = (SampleCounter & 15) == 15;
grE_Gyro.UpdateStats();
grE_Accel.UpdateStats();
break;
case 2:
Everything.Alt = GetCommLong();
Everything.AltRate = GetCommLong();
Everything.Pressure = GetCommLong(); // 3rd 20 bytes
Everything.Pitch = GetCommLong();
Everything.Roll = GetCommLong();
break;
case 3:
Everything.Yaw = GetCommLong();
Everything.q.x = GetCommFloat();
Everything.q.y = GetCommFloat(); // Last 20 bytes
Everything.q.z = GetCommFloat();
Everything.q.w = GetCommFloat();
ocCube.Quat = Everything.q;
break;
}
}
break;
}
}
void ComputeQuatAlternateMethod()
{
// Trapezoidal integration of gyro readings
float rx = (float)((GyroX+lastGx)*0.5f - GZeroX) * GyroScale + errCorrX2;
float ry = (float)((GyroZ+lastGz)*0.5f - GZeroZ) * -GyroScale + errCorrY2;
float rz = (float)((GyroY+lastGy)*0.5f - GZeroY) * -GyroScale + errCorrZ2;
lastGx = GyroX;
lastGy = GyroY;
lastGz = GyroZ;
float rMag = (float)(Math.Sqrt(rx * rx + ry * ry + rz * rz + 0.0000000001) * 0.5);
float cosr = (float)Math.Cos(rMag);
float sinr = (float)Math.Sin(rMag) / rMag;
qdot.w = -(rx * Q2.x + ry * Q2.y + rz * Q2.z) * 0.5f;
qdot.x = (rx * Q2.w + rz * Q2.y - ry * Q2.z) * 0.5f;
qdot.y = (ry * Q2.w - rz * Q2.x + rx * Q2.z) * 0.5f;
qdot.z = (rz * Q2.w + ry * Q2.x - rx * Q2.y) * 0.5f;
Q2.w = cosr * Q2.w + sinr * qdot.w;
Q2.x = cosr * Q2.x + sinr * qdot.x;
Q2.y = cosr * Q2.y + sinr * qdot.y;
Q2.z = cosr * Q2.z + sinr * qdot.z;
Q2 = Q2.Normalize();
// Convert to matrix form
m2.From(Q2);
// Compute the difference between the accelerometer vector and the matrix Y (up) vector
acc = new Vector( -AccelX, AccelZ, AccelY );
rMag = acc.Length * AccScale;
acc = acc.Normalize();
float accWeight = 1.0f - Math.Min( Math.Abs( 2.0f - rMag * 2.0f ), 1.0f );
// accWeight *= accWeight * 4.0f;
float errDiffX = acc.y * m2.m[1,2] - acc.z * m2.m[1,1];
float errDiffY = acc.z * m2.m[1,0] - acc.x * m2.m[1,2];
float errDiffZ = acc.x * m2.m[1,1] - acc.y * m2.m[1,0];
accWeight *= 1.0f / 512.0f;
errCorrX2 = errDiffX * accWeight;
errCorrY2 = errDiffY * accWeight;
errCorrZ2 = errDiffZ * accWeight;
// At this point, errCorr represents a very small correction rotation vector, but in the WORLD frame
// Rotate it into the current BODY frame
//Vector errVect = new Vector( errCorrX2, errCorrY2, errCorrZ2 );
//errVect = m.Transpose().Mul( errVect );
//errCorrX2 = errVect.x;
//errCorrY2 = errVect.y;
//errCorrZ2 = errVect.z;
}
void ComputeQuatOriginalMethod()
{
float rx = (float)(GyroX - GZeroX) * GyroScale + errCorrX1;
float ry = (float)(GyroZ - GZeroZ) * -GyroScale + errCorrY1;
float rz = (float)(GyroY - GZeroY) * -GyroScale + errCorrZ1;
float rMag = (float)(Math.Sqrt(rx * rx + ry * ry + rz * rz + 0.0000000001) * 0.5);
float cosr = (float)Math.Cos(rMag);
float sinr = (float)Math.Sin(rMag) / rMag;
qdot.w = -(rx * Q1.x + ry * Q1.y + rz * Q1.z) * 0.5f;
qdot.x = (rx * Q1.w + rz * Q1.y - ry * Q1.z) * 0.5f;
qdot.y = (ry * Q1.w - rz * Q1.x + rx * Q1.z) * 0.5f;
qdot.z = (rz * Q1.w + ry * Q1.x - rx * Q1.y) * 0.5f;
Q1.w = cosr * Q1.w + sinr * qdot.w;
Q1.x = cosr * Q1.x + sinr * qdot.x;
Q1.y = cosr * Q1.y + sinr * qdot.y;
Q1.z = cosr * Q1.z + sinr * qdot.z;
Q1 = Q1.Normalize();
// Convert to matrix form
m.From(Q1);
// Compute the difference between the accelerometer vector and the matrix Y (up) vector
acc = new Vector( -AccelX, AccelZ, AccelY );
rMag = acc.Length * AccScale;
acc = acc.Normalize();
float accWeight = 1.0f - Math.Min( Math.Abs( 2.0f - rMag * 2.0f ), 1.0f );
float errDiffX = acc.y * m.m[1,2] - acc.z * m.m[1,1];
float errDiffY = acc.z * m.m[1,0] - acc.x * m.m[1,2];
float errDiffZ = acc.x * m.m[1,1] - acc.y * m.m[1,0];
accWeight *= 1.0f / 512.0f;
errCorrX1 = errDiffX * accWeight;
errCorrY1 = errDiffY * accWeight;
errCorrZ1 = errDiffZ * accWeight;
}
