diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp
index 4cc5e96b9d10e9508d2ecd0993e03c82374afcec..9724f29914e5a52eb8f958740d5c93f2b9dc066e 100644
--- a/src/drivers/vmount/input_mavlink.cpp
+++ b/src/drivers/vmount/input_mavlink.cpp
@@ -278,6 +278,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 					// both specs have yaw on channel 2
 					_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
 
+					// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
+					if (_control_data.type_data.angle.angles[2] > M_PI_F) {
+						_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
+					}
+
 					*control_data = &_control_data;
 					break;