Exemplo n.º 1
0
		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;
		}
Exemplo n.º 2
0
		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;
		}
Exemplo n.º 3
0
		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;
		}
Exemplo n.º 4
0
		public float DotProduct( Quaternion q )
		{
			return w*q.w + x*q.x + y*q.y + z*q.z;
		}
Exemplo n.º 5
0
		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 );
		}
Exemplo n.º 6
0
		public Quaternion To( Quaternion b )
		{
			return this * b.Conjugate();
		}
Exemplo n.º 7
0
		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
		}
Exemplo n.º 8
0
		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;
			}
		}
Exemplo n.º 9
0
		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;
		}
Exemplo n.º 10
0
		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;
		}