Yaw controller using Euler angles


I'm working on a yaw controller for a T-copter using an Arduino/9dof sensor stick from sparkfun. The hard part of the project is done which was implementing a Kalman filter to estimate Euler angles (roll, pitch, and yaw). The next step is to implement a yaw controller that holds the heading. My problem is mapping the yaw angle to a PWM command and dealing with discontiuity at +180/-180. when the sign changes, the command jumps from the lower limit, say 1100ms, to the upper limit of 1900 ms. The controller right now is a simple proportional controller with the gain set to 1. The reference is obtained as an input speed reference then integrated to a reference angle. The error is found by subtracting the reference angle from the sensor angle then multiplied by the P gain, as in standard linear control. The controller output is then mapped to a PWM value using the Arduino map command e.g. pwm = map(ctrl_val, -180,180,1100,1900). Any suggestions on preventing the jump near the yaw limits?



Error should be calculated by subtracting the current angle from the target angle. This number can be anywhere between -360 and +360 as long as the current angle and target angle are ranged between -180 and +180

This presents the problem of “taking the long way around”

So if your error is lower than -180 or higher than +180, then recalculate your error

if (error < -180) error = (360 + error);
else if (error > 180) error = -(360 - error);

this operation bounds your error between -180 and +180 while taking into account discontinuities


I'm not sure this will solve the problem. I have code that bounds the yaw error, similar to what you describe. The problem arises when the angle changes from -180 to 180, which is basically the same direction relative to magnetic north. When this happens, the PWM output jumps to the opposite limit, -180 == 1100ms, 180 == 1900ms.



What you're describing should only happen when the ERROR (not the heading) approaches 180 deg. It would be a pretty extreme condition for your copter to be pointing completely the wrong way, but what this situation means is that the 'shortest angle' back to the desired heading has switched to the other direction. That is the correct behaviour in that situation.

If you're seeing this behaviour change depending on absolute heading then you are calculating your error value wrongly; it should be completely separate from the absolute heading.


I think your right, switching between +180 and -180 shouldn't cause problems if the error is calculated properly. What I was noticing might have been due to not having the actuator connected, therefore the controller could not correct the error.



Hi Ed,

I am also facing same problem for my T-copter! i am using magnetometer instead of gyro for yaw correction. So i am facing discontinuity while transition from 0° to 359° or vicre versa. The PID show crazy behaviour..

Can we eliminate this error by 2 PIDs? If we experienced discontinuity, then switch to direct/reverse PID. How about this? Will this work?

Use quaternions.

Can u explain how to implement it in code?

Two words: Google, Wikipedia