AP_Compass: use a separate slot for the custom compass rotation

TForce1 · May 23, 2020 · 63b5711 · 63b5711
1 parent acff7da
commit 63b5711
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 9 deletions.
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
@@ -224,9 +224,7 @@ friend class AP_Compass_Backend;
     // set overall board orientation
     void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
         _board_orientation = orientation;
-        if (custom_rotation) {
-            _custom_rotation = custom_rotation;
-        }
+        _custom_rotation = custom_rotation;
     }
 
     /// Set the motor compensation type
@@ -438,9 +436,13 @@ friend class AP_Compass_Backend;
 
     // board orientation from AHRS
     enum Rotation _board_orientation = ROTATION_NONE;
-    // custom rotation matrix
+
+    // custom board rotation matrix
     Matrix3f* _custom_rotation;
 
+    // custom external compass rotation matrix
+    Matrix3f* _custom_external_rotation;
+
     // declination in radians
     AP_Float    _declination;
 

diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -32,8 +32,8 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
         // add user selectable orientation
 #if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
         Rotation rotation = Rotation(state.orientation.get());
-        if (rotation == ROTATION_CUSTOM && _compass._custom_rotation) {
-            mag = *_compass._custom_rotation * mag;
+        if (rotation == ROTATION_CUSTOM && _compass._custom_external_rotation) {
+            mag = *_compass._custom_external_rotation * mag;
         } else {
             mag.rotate(rotation);
         }
@@ -234,9 +234,11 @@ void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation)
     _compass._state[Compass::StateIndex(instance)].rotation = rotation;
 #if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
     // lazily create the custom rotation matrix
-    if (!_compass._custom_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) {
-        _compass._custom_rotation = new Matrix3f();
-        _compass._custom_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw));
+    if (!_compass._custom_external_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) {
+        _compass._custom_external_rotation = new Matrix3f();
+        if (_compass._custom_external_rotation) {
+            _compass._custom_external_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw));
+        }
     }
 #endif
 }