AP_Compass: use a separate slot for the custom compass rotation

libraries/AP_Compass/AP_Compass.h

@@ -224,10 +224,8 @@ public:
     // 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;
     }
-    }
 
     /// Set the motor compensation type
     ///
@@ -438,9 +436,13 @@ private:
 
     // 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;
 

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) {
+        if (rotation == ROTATION_CUSTOM && _compass._custom_external_rotation) {
-            mag = *_compass._custom_rotation * mag;
+            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) {
+    if (!_compass._custom_external_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) {
-        _compass._custom_rotation = new Matrix3f();
+        _compass._custom_external_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) {
+            _compass._custom_external_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw));
+        }
     }
 #endif
 }