From b91f5e377b73df75454073f2cdec33d9df04dbb7 Mon Sep 17 00:00:00 2001 From: Moe Bataineh Date: Thu, 8 Dec 2022 11:09:49 -0600 Subject: [PATCH] AP_Mount: Align received data to AP frame for Storm32 Serial (Note: reversed pitch and yaw) to match NED. Update AP_Mount_SToRM32_serial.cpp --- libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index e6502f878e..98cdc26921 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -271,9 +271,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;