Measure rotation as a gyroscope does (without a real gyroscope)

I decided to edit my question to be less abstract.

I want to emulate the readings of a gyroscope in unity using C# and the engine.

The following code works for most cases:

``````public float rollAngle;
public float rollRate;

void AngleRoll(Quaternion rotationOne, Quaternion rotationTwo){
var rotationA = rotationOne * Vector3.left;
var rotationB = rotationTwo * Vector3.left;
var angleA = Mathf.Atan2(rotationA.x, rotationA.y) * Mathf.Rad2Deg;
var angleB = Mathf.Atan2(rotationB.x, rotationB.y) * Mathf.Rad2Deg;
float prevRollAngle = rollAngle;
rollAngle = Mathf.DeltaAngle(angleA,angleB);
RollRate(rollAngle,prevRollAngle);
}

void RollRate(float newValue,float prevValue){
rollRate = ((newValue - prevValue) / Time.deltaTime);
}
``````

There is one of these for each axis X,Y,Z.

One issue that I am facing with this current model is that as the rotation around Y changes the readings for X and Z angles start to change rapidly and the changes become huge when the rotation around Y goes past +90 or -90.

To correct this I have tried to isolate the X,Y,Z parts of the rotation by creating three quaternions that represent the three axis rotations.

``````Quaternion theRot = this.transform.rotation; // Object's rotation
Quaternion xPart = Quaternion.Euler(theRot.eulerAngles.x,0.0f,0.0f); // X part
Quaternion yPart = Quaternion.Euler(0.0f,theRot.eulerAngles.y,0.0f); // Y part
Quaternion zPart = Quaternion.Euler(0.0f,0.0f,theRot.eulerAngles.z); // Z part
Quaternion yBase = Quaternion.identity * yPart; // Quaternion representing current Y rot

AnglePitch(xPart,yBase); //Calc gyro values for x (pitch) using the x/y plane
AngleRoll(zPart,yBase); //Calc gyro values for z (roll) using the z/y plane
AngleYaw(yPart,Quaternion.identity); //Calc values for y (yaw) using the z/x plane
``````

This doesn’t seem to be working and I am not sure where to go from here. I have tried a couple other ways to get the desired results but this is the closest I have come.

Any insight is welcome

Edit:

They way this is used:

``````private GyroScope gyro;
private Rigidbody physBody;

void Start(){
gyro = new GyroScope(); // Create a new gyroscope
physBody = gameObject.GetComponent<Rigidbody>(); // Get the rigidbody
}

// This maps a value with range (inMin <-> inMax) to the equivalent value in range (outMin <-> outMax)
float Map(float value, float inMin, float inMax, float outMin, float outMax){
return (value - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
}

void FixedUpdate(){
float rollTar = Map(Input.GetAxis("Horizontal"),-1.0f,1.0f,maxRoll,minRoll); //Get user input for desired roll value
float rollError = rollTar - gyro.rollAngle; // Calc the error between the roll value we want and the gyro rollAngle value
// A PID is a mathematical operation that combines the Proportional/Integral/Derivative of the value into a smooth balanced change
stabiliyRollValue = PID.Compute(rollError); //Compute the value needed to balance our current value combined with our new value

rollRateError = stabiliyRollValue - gyro.rollRate; //Get the error between our desired stability and our actual stability rate
rollOutputCorrection = PID.Compute(rollRateError); //Compute the correction value needed to balance out desired rate and teh actual gyro rate

``````rollAngle = difference in A.z and B.z;