From 2d2165936a0da53baa479d16de780b1436c9f21e Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 23 Apr 2022 12:16:08 +0100
Subject: [PATCH] AP_InertialSensor: use pitch to guess which axis the user is
 trying to calibrate, warn about Q_TRIM_PITCH on plane

---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 78 ++++++++++---------
 1 file changed, 41 insertions(+), 37 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index f3e90daf34ce7..30086da60f2b9 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -11,6 +11,7 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_AHRS/AP_AHRS_View.h>
 #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
 #if !APM_BUILD_TYPE(APM_BUILD_Rover)
 #include <AP_Motors/AP_Motors_Class.h>
@@ -1189,48 +1190,51 @@ void AP_InertialSensor::periodic()
 */
 bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, Vector3f &trim)
 {
-    // allow multiple rotations, this allows us to cope with tailsitters
-    const enum Rotation rotations[] = {ROTATION_NONE,
-#ifndef HAL_BUILD_AP_PERIPH
-                                       AP::ahrs().get_view_rotation()
+    Rotation rotation = ROTATION_NONE;
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    AP_AHRS_View *view = AP::ahrs().get_view();
+    if (view != nullptr) {
+        // Use pitch to guess which axis the user is trying to trim
+        // 5 deg buffer to favor normal AHRS and avoid floating point funny business
+        if (fabsf(view->pitch) < (fabsf(AP::ahrs().pitch)+radians(5)) ) {
+            // user is trying to calibrate view
+            rotation = view->get_rotation();
+            if (!is_zero(view->get_pitch_trim())) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Cannot calibrate with Q_TRIM_PITCH set");
+                return false;
+            }
+        }
+    }
 #endif
-    };
-    bool good_trim = false;
-    Vector3f newtrim;
-    for (const auto r : rotations) {
-        newtrim = trim;
-        switch (r) {
-        case ROTATION_NONE:
-            newtrim.y = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z));
-            newtrim.x = atan2f(-accel_sample.y, -accel_sample.z);
-            break;
 
-        case ROTATION_PITCH_90: {
-            newtrim.y = atan2f(accel_sample.z, norm(accel_sample.y, -accel_sample.x));
-            newtrim.z = atan2f(-accel_sample.y, accel_sample.x);
-            break;
-        }
-        default:
-            // unsupported
-            continue;
-        }
-        if (fabsf(newtrim.x) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
-            fabsf(newtrim.y) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
-            fabsf(newtrim.z) <= radians(HAL_INS_TRIM_LIMIT_DEG)) {
-            good_trim = true;
-            break;
-        }
+    Vector3f newtrim = trim;
+    switch (rotation) {
+    case ROTATION_NONE:
+        newtrim.y = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z));
+        newtrim.x = atan2f(-accel_sample.y, -accel_sample.z);
+        break;
+
+    case ROTATION_PITCH_90: {
+        newtrim.y = atan2f(accel_sample.z, norm(accel_sample.y, -accel_sample.x));
+        newtrim.z = atan2f(-accel_sample.y, accel_sample.x);
+        break;
     }
-    if (!good_trim) {
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "trim over maximum of 10 degrees");
+    default:
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "unsupported trim rotation");
         return false;
     }
-    trim = newtrim;
-    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Trim OK: roll=%.2f pitch=%.2f yaw=%.2f",
-                  (double)degrees(trim.x),
-                  (double)degrees(trim.y),
-                  (double)degrees(trim.z));
-    return true;
+    if (fabsf(newtrim.x) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
+        fabsf(newtrim.y) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
+        fabsf(newtrim.z) <= radians(HAL_INS_TRIM_LIMIT_DEG)) {
+        trim = newtrim;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Trim OK: roll=%.2f pitch=%.2f yaw=%.2f",
+                        (double)degrees(trim.x),
+                        (double)degrees(trim.y),
+                        (double)degrees(trim.z));
+        return true;
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "trim over maximum of 10 degrees");
+    return false;
 }
 
 void