//--------------------------------------------------------------
	// Convert gyro rates to radians, add in the previous cycle error corrections
	//--------------------------------------------------------------
	// float rx = Float(gx) * const_GyroScale + errCorrX;
	opFloat, 	gx, 0, Temp_lhs,
	opMul, 	Temp_lhs, const_GyroScale, Temp_lhs,
	opAdd, 	Temp_lhs, errCorrX, rx,
	// float ry = Float(gy) * const_GyroScale + errCorrY;
	opFloat, 	gy, 0, Temp_lhs,
	opMul, 	Temp_lhs, const_GyroScale, Temp_lhs,
	opAdd, 	Temp_lhs, errCorrY, ry,
	// float rz = Float(gz) * const_GyroScale + errCorrZ;
	opFloat, 	gz, 0, Temp_lhs,
	opMul, 	Temp_lhs, const_GyroScale, Temp_lhs,
	opAdd, 	Temp_lhs, errCorrZ, rz,
	// 
	//--------------------------------------------------------------
	// Update the orientation quaternion
	//--------------------------------------------------------------
	// float rmag = Sqrt(rx*rx + ry*ry + rz*rz + const_Epsilon) * 0.5f;
	opMul, 	rx, rx, Temp_lhs,
	opMul, 	ry, ry, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	rz, rz, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opAdd, 	Temp_lhs, const_Epsilon, Temp_lhs,
	opSqrt, 	Temp_lhs, 0, Temp_lhs,
	opMul, 	Temp_lhs, 0.5, rmag,
	// 
	// float sinr, cosr;
	// cosr = SinCos(rmag, sinr);
	opSinCos, 	rmag, sinr, cosr,
	// sinr /= rmag;
	opDiv, 	sinr, rmag, sinr,
	// 
	// float qdw = -(rx*qx + ry*qy + rz*qz) * 0.5f;		// Could neg first, then shift, but will take an instruction
	opMul, 	rx, qx, Temp_lhs,
	opMul, 	ry, qy, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	rz, qz, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_rhs,
	opNeg, 	0, Temp_rhs, Temp_lhs,
	opMul, 	Temp_lhs, 0.5, qdw,
	// float qdx = (rx*qw + rz*qy + ry*qz) * 0.5f;
	opMul, 	rx, qw, Temp_lhs,
	opMul, 	rz, qy, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	ry, qz, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	Temp_lhs, 0.5, qdx,
	// float qdy = (ry*qw - rz*qx + rx*qz) * 0.5f;
	opMul, 	ry, qw, Temp_lhs,
	opMul, 	rz, qx, Temp_rhs,
	opSub, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	rx, qz, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	Temp_lhs, 0.5, qdy,
	// float qdz = (rz*qw + ry*qx - rx*qy) * 0.5f;
	opMul, 	rz, qw, Temp_lhs,
	opMul, 	ry, qx, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	rx, qy, Temp_rhs,
	opSub, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	Temp_lhs, 0.5, qdz,
	// 
	// qw = cosr * qw + sinr * qdw;
	opMul, 	cosr, qw, Temp_lhs,
	opMul, 	sinr, qdw, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, qw,
	// qx = cosr * qx + sinr * qdx;
	opMul, 	cosr, qx, Temp_lhs,
	opMul, 	sinr, qdx, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, qx,
	// qy = cosr * qy + sinr * qdy;
	opMul, 	cosr, qy, Temp_lhs,
	opMul, 	sinr, qdy, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, qy,
	// qz = cosr * qz + sinr * qdz;
	opMul, 	cosr, qz, Temp_lhs,
	opMul, 	sinr, qdz, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, qz,
	// 
	// rmag = Sqrt(qx*qx + qy*qy + qz*qz + qw*qw + const_Epsilon);
	opMul, 	qx, qx, Temp_lhs,
	opMul, 	qy, qy, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	qz, qz, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	qw, qw, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opAdd, 	Temp_lhs, const_Epsilon, Temp_lhs,
	opSqrt, 	Temp_lhs, 0, rmag,
	// qw /= rmag;
	opDiv, 	qw, rmag, qw,
	// qx /= rmag;
	opDiv, 	qx, rmag, qx,
	// qy /= rmag;
	opDiv, 	qy, rmag, qy,
	// qz /= rmag;
	opDiv, 	qz, rmag, qz,
	// 
	//--------------------------------------------------------------
	// Convert the updated quaternion to a rotation matrix
	//--------------------------------------------------------------
	// float fx2 = qx*qx;
	opMul, 	qx, qx, fx2,
	// float fy2 = qy*qy;
	opMul, 	qy, qy, fy2,
	// float fz2 = qz*qz;
	opMul, 	qz, qz, fz2,
	// 
	// float fwx = qw*qx;
	opMul, 	qw, qx, fwx,
	// float fwy = qw*qy;
	opMul, 	qw, qy, fwy,
	// float fwz = qw*qz;
	opMul, 	qw, qz, fwz,
	// 
	// float fxy = qx*qy;
	opMul, 	qx, qy, fxy,
	// float fxz = qx*qz;
	opMul, 	qx, qz, fxz,
	// float fyz = qy*qz;
	opMul, 	qy, qz, fyz,
	// 
	// m00 = 1.0f - 2.0f * (fy2 + fz2);
	opAdd, 	fy2, fz2, Temp_rhs,
	opMul, 	2.0, Temp_rhs, Temp_rhs,
	opSub, 	1.0, Temp_rhs, m00,
	// m01 =        2.0f * (fxy - fwz);
	opSub, 	fxy, fwz, Temp_rhs,
	opMul, 	2.0, Temp_rhs, m01,
	// m02 =        2.0f * (fxz + fwy);
	opAdd, 	fxz, fwy, Temp_rhs,
	opMul, 	2.0, Temp_rhs, m02,
	// 
	// m10 =        2.0f * (fxy + fwz);
	opAdd, 	fxy, fwz, Temp_rhs,
	opMul, 	2.0, Temp_rhs, m10,
	// m11 = 1.0f - 2.0f * (fx2 + fz2);
	opAdd, 	fx2, fz2, Temp_rhs,
	opMul, 	2.0, Temp_rhs, Temp_rhs,
	opSub, 	1.0, Temp_rhs, m11,
	// m12 =        2.0f * (fyz - fwx);
	opSub, 	fyz, fwx, Temp_rhs,
	opMul, 	2.0, Temp_rhs, m12,
	// 
	// m20 =        2.0f * (fxz - fwy);
	opSub, 	fxz, fwy, Temp_rhs,
	opMul, 	2.0, Temp_rhs, m20,
	// m21 =        2.0f * (fyz + fwx);
	opAdd, 	fyz, fwx, Temp_rhs,
	opMul, 	2.0, Temp_rhs, m21,
	// m22 = 1.0f - 2.0f * (fx2 + fy2);
	opAdd, 	fx2, fy2, Temp_rhs,
	opMul, 	2.0, Temp_rhs, Temp_rhs,
	opSub, 	1.0, Temp_rhs, m22,
	// 
	// 
	//--------------------------------------------------------------
	// Grab the accelerometer values as floats
	//--------------------------------------------------------------
	// float fax = -Float(ax);
	opFloat, 	ax, 0, Temp_rhs,
	opNeg, 	0, Temp_rhs, fax,
	// float fay = Float(az);
	opFloat, 	az, 0, fay,
	// float faz = Float(ay);
	opFloat, 	ay, 0, faz,
	// 
	//--------------------------------------------------------------
	// Rotate accelerometer vector by the level correction angles
	//--------------------------------------------------------------
	// float axRot = (fax * accRollCorrCos) - (fay * accRollCorrSin);
	opMul, 	fax, accRollCorrCos, Temp_lhs,
	opMul, 	fay, accRollCorrSin, Temp_rhs,
	opSub, 	Temp_lhs, Temp_rhs, axRot,
	// float ayRot = (fax * accRollCorrSin) + (fay * accRollCorrCos);
	opMul, 	fax, accRollCorrSin, Temp_lhs,
	opMul, 	fay, accRollCorrCos, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, ayRot,
	// fax = axRot;
	// fay = ayRot;
	// 
	// axRot = (faz * accPitchCorrCos) - (fay * accPitchCorrSin);
	opMul, 	faz, accPitchCorrCos, Temp_lhs,
	opMul, 	fay, accPitchCorrSin, Temp_rhs,
	opSub, 	Temp_lhs, Temp_rhs, axRot,
	// ayRot = (fax * accPitchCorrSin) + (fay * accPitchCorrCos);
	opMul, 	fax, accPitchCorrSin, Temp_lhs,
	opMul, 	fay, accPitchCorrCos, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, ayRot,
	// faz = axRot;
	// fay = ayRot;
	// 
	//--------------------------------------------------------------
	// Compute length of the accelerometer vector and normalize it.
	// Use the computed length to decide weighting, IE how likely is
	// it a good reading to use to correct our rotation estimate.
	// If it's too long/short, weight it less.
	//--------------------------------------------------------------
	// 
	// rmag = Sqrt( fax*fax + fay*fay + faz*faz + const_Epsilon );
	opMul, 	fax, fax, Temp_lhs,
	opMul, 	fay, fay, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	faz, faz, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opAdd, 	Temp_lhs, const_Epsilon, Temp_lhs,
	opSqrt, 	Temp_lhs, 0, rmag,
	// fax /= rmag;
	opDiv, 	fax, rmag, fax,
	// fay /= rmag;
	opDiv, 	fay, rmag, fay,
	// faz /= rmag;
	opDiv, 	faz, rmag, faz,
	// 
	// float accWeight = 1.0f - FMin( FAbs( 2.0f - const_AccScale * rmag * 2.0f ), 1.0f );
	opMul, 	const_AccScale, rmag, Temp_lhs,
	opMul, 	Temp_lhs, 2.0, Temp_rhs,
	opSub, 	2.0, Temp_rhs, Temp_lhs,
	opFAbs, 	Temp_lhs, 0, Temp_lhs,
	opFMin, 	Temp_lhs, 1.0, Temp_rhs,
	opSub, 	1.0, Temp_rhs, accWeight,
	// 
	// 
	//--------------------------------------------------------------
	// Compute the cross product of our normalized accelerometer vector
	// and our current orientation "up" vector.  If they're identical,
	// the cross product will be zeros.  Any difference produces an
	// axis of rotation between the two vectors, and the magnitude of
	// the vector is the amount to rotate to align them.
	//--------------------------------------------------------------
	// 
	// float errDiffX = fay * m12 - faz * m11;
	opMul, 	fay, m12, Temp_lhs,
	opMul, 	faz, m11, Temp_rhs,
	opSub, 	Temp_lhs, Temp_rhs, errDiffX,
	// float errDiffY = faz * m10 - fax * m12;
	opMul, 	faz, m10, Temp_lhs,
	opMul, 	fax, m12, Temp_rhs,
	opSub, 	Temp_lhs, Temp_rhs, errDiffY,
	// float errDiffZ = fax * m11 - fay * m10;
	opMul, 	fax, m11, Temp_lhs,
	opMul, 	fay, m10, Temp_rhs,
	opSub, 	Temp_lhs, Temp_rhs, errDiffZ,
	// 
	// accWeight *= const_AccErrScale;
	opMul, 	accWeight, const_AccErrScale, accWeight,
	// 
	//--------------------------------------------------------------
	// Scale the resulting difference by the weighting factor.  This
	// gets mixed in with the gyro values on the next update to pull
	// the "up" part of our rotation back into alignment with gravity
	// over time.
	//--------------------------------------------------------------
	// 
	// errCorrX = errDiffX * accWeight;
	opMul, 	errDiffX, accWeight, errCorrX,
	// errCorrY = errDiffY * accWeight;
	opMul, 	errDiffY, accWeight, errCorrY,
	// errCorrZ = errDiffZ * accWeight;
	opMul, 	errDiffZ, accWeight, errCorrZ,
	// 
	// compute heading using Atan2 and the Z vector of the orientation matrix
	// FloatYaw = -ATan2(m20, m22);
	opATan2, 	m20, m22, Temp_rhs,
	opNeg, 	0, Temp_rhs, FloatYaw,
	// 
	// When switching between manual and auto, or just lifting off, I need to
	// know the half-angle of the craft so I can use it as my initial Heading value
	// to be fed into the quaternion construction code.  This HalfYaw value serves that purpose
	// 
	// HalfYaw = FloatYaw * 0.5f;
	opMul, 	FloatYaw, 0.5, HalfYaw,
	// 
	// 
	// Compute pitch and roll in integer form, used by compass calibration, possible user code
	// 
	// Pitch = Trunc( ASin(m12) * const_outAngleScale );
	opASinCos, 	m12, 1, Temp_lhs,
	opMul, 	Temp_lhs, const_outAngleScale, Temp_lhs,
	opTruncRound, 	Temp_lhs, 0, Pitch,
	// Roll = Trunc( ASin(m10) * const_outNegAngleScale );
	opASinCos, 	m10, 1, Temp_lhs,
	opMul, 	Temp_lhs, const_outNegAngleScale, Temp_lhs,
	opTruncRound, 	Temp_lhs, 0, Roll,
	// 
	// 1.0/m11 = scale factor for thrust - this will be infinite if perpendicular to ground
	// ThrustFactor = Trunc( (1.0f / m11) * 256.0f );
	opDiv, 	1.0, m11, Temp_lhs,
	opMul, 	Temp_lhs, 256.0, Temp_lhs,
	opTruncRound, 	Temp_lhs, 0, ThrustFactor,
	// 
	// 
	//--------------------------------------------------------------
	// Compute the running height estimate - this is a fusion of the
	// height computed directly from barometric pressure, and and
	// running estimate of vertical velocity computed from the
	// accelerometer, integrated to produce a height estimate.
	// The two different values are used to correct each other.
	//--------------------------------------------------------------
	// 
	// forceX = fax / 4096.0f;
	opDiv, 	fax, 4096.0, forceX,
	// forceY = fay / 4096.0f;
	opDiv, 	fay, 4096.0, forceY,
	// forceZ = faz / 4096.0f;
	opDiv, 	faz, 4096.0, forceZ,
	// 
	//Orient force vector into world frame & subtract gravity (1G)
	// forceWY = m01*forceX + m11*forceY + m21*forceZ - 1.0f;
	opMul, 	m01, forceX, Temp_lhs,
	opMul, 	m11, forceY, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opMul, 	m21, forceZ, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, Temp_lhs,
	opSub, 	Temp_lhs, 1.0, forceWY,
	// 
	//Convert G to mm/sec^2
	// forceWY *= const_G_mm_PerSec;
	opMul, 	forceWY, const_G_mm_PerSec, forceWY,
	// 
	// velocityEstimate += forceWY * const_UpdateScale;
	opMul, 	forceWY, const_UpdateScale, Temp_rhs,
	opAdd, 	velocityEstimate, Temp_rhs, velocityEstimate,
	// 
	// velocityEstimate = (velocityEstimate * const_velAccScale) + (Float(altRate) * const_velAltiScale);
	opMul, 	velocityEstimate, const_velAccScale, Temp_lhs,
	opFloat, 	altRate, 0, Temp_lhs,
	opMul, 	Temp_lhs, const_velAltiScale, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, velocityEstimate,
	// altitudeEstimate += velocityEstimate * const_UpdateScale;
	opMul, 	velocityEstimate, const_UpdateScale, Temp_rhs,
	opAdd, 	altitudeEstimate, Temp_rhs, altitudeEstimate,
	// 
	// altitudeEstimate = (altitudeEstimate * const_velAccTrust) + Float(alt) * const_velAltiTrust;
	opMul, 	altitudeEstimate, const_velAccTrust, Temp_lhs,
	opFloat, 	alt, 0, Temp_lhs,
	opMul, 	Temp_lhs, const_velAltiTrust, Temp_rhs,
	opAdd, 	Temp_lhs, Temp_rhs, altitudeEstimate,
	// 
	// output integer values for PIDs
	// AltitudeEstMM = Trunc(altitudeEstimate);
	opTruncRound, 	altitudeEstimate, 0, AltitudeEstMM,
	// VelocityEstMM = Trunc(velocityEstimate);
	opTruncRound, 	velocityEstimate, 0, VelocityEstMM,
	//