Properly normalize both ends of absolute and body yaw

Author: jnomikos

src/Gimbal/GimbalController.cc

@@ -254,20 +254,14 @@ void GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t
     const bool yaw_in_vehicle_frame = _yawInVehicleFrame(attitude_status.flags);
     if (yaw_in_vehicle_frame) {
         const float bodyYaw = qRadiansToDegrees(yaw);
-        float absoluteYaw = bodyYaw + headingDeg;
-        if (absoluteYaw > 180.0f) {
-            absoluteYaw -= 360.0f;
-        }
+        const float absoluteYaw = std::remainder(bodyYaw + headingDeg, 360.0f);
 
         gimbal->setBodyYaw(bodyYaw);
         gimbal->setAbsoluteYaw(absoluteYaw);
 
     } else {
         const float absoluteYaw = qRadiansToDegrees(yaw);
-        float bodyYaw = absoluteYaw - headingDeg;
-        if (bodyYaw < -180.0f) {
-            bodyYaw += 360.0f;
-        }
+        const float bodyYaw = std::remainder(absoluteYaw - headingDeg, 360.0f);
 
         gimbal->setBodyYaw(bodyYaw);
         gimbal->setAbsoluteYaw(absoluteYaw);