From 5dcfa95444a114c2363f781b6a9dd37a3bc50495 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 17 Aug 2021 12:52:16 +1000 Subject: [PATCH] GCS_Mavlink: allow setting of AHRS_TRIM_Z --- libraries/GCS_MAVLink/GCS_Common.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 7335f921e3..c5fceb5ba4 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3776,12 +3776,12 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm if (!calibrate_gyros()) { return MAV_RESULT_FAILED; } - float trim_roll, trim_pitch; - if (!AP::ins().calibrate_trim(trim_roll, trim_pitch)) { + Vector3f trim_rad = AP::ahrs().get_trim(); + if (!AP::ins().calibrate_trim(trim_rad)) { return MAV_RESULT_FAILED; } // reset ahrs's trim to suggested values from calibration routine - AP::ahrs().set_trim(Vector3f(trim_roll, trim_pitch, 0)); + AP::ahrs().set_trim(trim_rad); return MAV_RESULT_ACCEPTED; }