Reading a yaw navigation angle and set it to 0

Question

I am working with a drone, and I can read the yaw navigation angle from its sensors. However, I would like to establish this angle as the "0" angle when I start my process. The range of this angle is between -180 degrees to 180 degrees.

initial_yaw = read_yaw_angle()
current_yaw = read_yaw_angle() - initial_yaw

but if initial_yaw is 180 degrees, and the measured angle is, let's say, -50 degrees. Now I have that the current_yaw is -230 which is out of the range -180 to 180 degrees. How can I solve this issue? (is it modulo operator what I need to use?)

Answer 1

current_yaw = read_yaw_angle() - initial_yaw;
if (current_yaw < -180) {
  current_yaw += 360;
} else if (currrent_yaw > 180) {
  current_yaw -= 360;
}