From c483b26737b5877cc7fda21ee4d1a39d4c6b8d16 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 9 Feb 2023 09:15:16 +1100
Subject: [PATCH] AP_Compass: fixed zero compass diagonals

this fixes a regression from 4.2 to 4.3.

previously we automatically set the diagnoals to 1,1,1 if they were
0,0,0. We don't do that any more. I was helping a user who had copied
an old config with 0,0,0 for diagonals and did not understand two
things:

- the compass did not work in 4.3
- large vehicle mag cal didn't work
---
 libraries/AP_Compass/AP_Compass_Backend.cpp   | 14 +++++-----
 .../AP_Compass/AP_Compass_Calibration.cpp     | 27 ++++++++++---------
 2 files changed, 23 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
index c318aa0f55..5ea08e92b8 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.cpp
+++ b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -87,13 +87,15 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
     }
 
     // apply eliptical correction
-    Matrix3f mat(
-        diagonals.x, offdiagonals.x, offdiagonals.y,
-        offdiagonals.x,    diagonals.y, offdiagonals.z,
-        offdiagonals.y, offdiagonals.z,    diagonals.z
-    );
+    if (!diagonals.is_zero()) {
+        Matrix3f mat(
+            diagonals.x,    offdiagonals.x, offdiagonals.y,
+            offdiagonals.x, diagonals.y,    offdiagonals.z,
+            offdiagonals.y, offdiagonals.z, diagonals.z
+            );
 
-    mag = mat * mag;
+        mag = mat * mag;
+    }
 
 #if COMPASS_MOT_ENABLED
     const Vector3f &mot = state.motor_compensation.get();
diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index 27db528c35..a48edc3036 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -450,22 +450,25 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
     // form eliptical correction matrix and invert it. This is
     // needed to remove the effects of the eliptical correction
     // when calculating new offsets
-    const Vector3f &diagonals = get_diagonals(instance);
-    const Vector3f &offdiagonals = get_offdiagonals(instance);
-    Matrix3f mat {
-        diagonals.x, offdiagonals.x, offdiagonals.y,
-        offdiagonals.x,    diagonals.y, offdiagonals.z,
-        offdiagonals.y, offdiagonals.z,    diagonals.z
-    };
-    if (!mat.invert()) {
-        return false;
-    }
 
     // get corrected field
     field = get_field(instance);
 
-    // remove impact of diagonals and off-diagonals
-    field = mat * field;
+    const Vector3f &diagonals = get_diagonals(instance);
+    if (!diagonals.is_zero()) {
+        const Vector3f &offdiagonals = get_offdiagonals(instance);
+        Matrix3f mat {
+            diagonals.x, offdiagonals.x, offdiagonals.y,
+            offdiagonals.x,    diagonals.y, offdiagonals.z,
+            offdiagonals.y, offdiagonals.z,    diagonals.z
+        };
+        if (!mat.invert()) {
+            return false;
+        }
+
+        // remove impact of diagonals and off-diagonals
+        field = mat * field;
+    }
 
     // remove impact of offsets
     field -= get_offsets(instance);