- Home /
Problems getting PID system and rotations to work together.
I am working on a Unity project involving aircraft. As the users controls the aircraft, it is stabilized by a PID system.
The way the system was set up before I started working on it is as follows. First, the pitch, roll, and yaw values are stored individually as floats using Euler angles. (For the sake of brevity, I will only be showing the code used for pitch, but roll and yaw function identically)
float pitchAngle = -AngleTransform(this.transform.rotation.eulerAngles.x);
(AngleTransform is a function that keeps the angle between -180 and 180. The pitch is set to negative because of the way user controls are stored [More below])
The target pitch is determined by the user input on the y axis of the right stick of a gamepad as per the below.
float pitchTar += Map(rightStickY, -1.0f, 1.0f, 3.5f, -3.5f);
The error between the actual pitch and the target pitch is then found.
float pitchErr = pitchTar - pitch;
The error is then fed into the PID system which has stored information about previous pitch errors and spits out a stability output.
float pitch_stab_output = (pids[PID_PITCH_STAB].Compute(pitchErr));
This output is an estimate of the rate the pitch should be changing in order to reach the target pitch.
Next, the rate the pitch is actually changing is calculated using the pitch from the last frame (prevPitchAngle)
float pitchRate = Mathf.DeltaAngle(prevPitchAngle, pitchAngle) / Time.fixedDeltaTime;
(Pitch is recalculated in FixedUpdate which is why this is divided by Time.fixedDeltaTime.)
Then, the pitch rate error is calculated.
float pitchRateErr = pitch_stab_output - pitchRate;
This is fed into a seperate PID which gives us the final pitch output.
float pitch_output = pids[PID_PITCH_RATE].Compute(pitchRateErr);
This pitch output is then used in various flight calculations.
The issue this setup has is that when the rotation about the x-axis >90 and <270, the y and z-axises flip by 180. This causes the PID system to believe the roll and the yaw have gone instantaneously from 0 to 180 causing outputs which attempt to stabilize such a scenario but invariably just make the aircraft end up crashing.
Enter me starting work on the system. I am aware that this flip of 180 is a result of the limitations of displaying the underlying quaternion that Unity uses in Euler angles.
I have tried many things from simple (subtracting 180 from the yaw and roll if the rotation around the x-axis if >90 and <270) to complex (using the underlying x, y, z, and w of the transform.rotation quaternion to calculate the Euler representation of the rotation) but nothing seems to work.
Below is my latest attempt at finding the error between the target rotation and actual rotation by keeping everything in quaternions.
Quaternion pitchRollYawErr = (Quaternion.AngleAxis(yawTar, Vector3.up) *
Quaternion.AngleAxis(pitchTar, Vector3.right) *
Quaternion.AngleAxis(rollTar, Vector3.forward)) *
Quaternion.Inverse(transform.rotation);
float pitchErr = pitchRollYawErr.eulerAngles.x;
I also find the pitch rate as follows:
Quaternion prevRot = rot;
rot = transform.rotation;
Quaternion difference = rot * Quaternion.Inverse(prevRot);
float pitchRate = difference.eulerAngles.x / Time.deltaTime;
This still does not fix the problem and actually makes the aircraft unpilotable (Which points to the possibility that I'm way off mark in what I'm attempting.) In particular, the calculations for pitch rate give me some huge results, and I can't seem to understand why.
To sum everything up in one question: Is it possible to get the Euler representation of each rotation (pitch, roll, and yaw) without the flip to 180 described above?
In the interest of keeping the problem easily digestible, I have opted not to go into too much depth about how the PID system works. I am fairly sure the problem stems from the conversion of the quaternion to Euler angles and what is happening in the PID is unrelated. If you think otherwise and that there's a chance the error could be occurring with something related to the PID, let me know and I'll give more details.
Please note that the way the PID system takes floats is something I cannot currently change. Also, I am not directly modifying the rotation of the aircraft, so just using the quaternions as is is also out of the question. If what I want is not possible in the configuration I have described, please tell me that rather than suggesting one of these things.
Thanks for reading! This is all pretty complex (at least for me!) so if I didn't describe something well enough, or if you would like me to expound on something please let me know.
Your answer
Follow this Question
Related Questions
Setting the roll of a rotation 1 Answer
How to change angle-roll object with mobile input? 0 Answers
Adjust Yaw For A Gliding (Like Superman 64) Script 1 Answer
Get pitch and roll 0 Answers