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?