diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index cfa5212633..ad117fe1d8 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -269,9 +269,10 @@ void AP_Mount_SToRM32_serial::parse_reply() { break; } + // Parse angles (Note: reversed pitch and yaw) to match ardupilot coordinate system _current_angle.x = _buffer.data.imu1_roll; - _current_angle.y = _buffer.data.imu1_pitch; - _current_angle.z = _buffer.data.imu1_yaw; + _current_angle.y = -_buffer.data.imu1_pitch; + _current_angle.z = -_buffer.data.imu1_yaw; break; default: break;