AP_InertialSensor: add support for custom board orientations

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1781,7 +1781,11 @@ bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vec
         return false;
     }
     _accel_calibrator[_acc_body_aligned-1].get_sample_corrected(sample_num, ret);
-    ret.rotate(_board_orientation);
+    if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
+        ret = *_custom_rotation * ret;
+    } else {
+        ret.rotate(_board_orientation);
+    }
     return true;
 }
 
@@ -1806,7 +1810,11 @@ bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vec
     }
     avg /= count;
     ret = avg;
-    ret.rotate(_board_orientation);
+    if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
+        ret = *_custom_rotation * ret;
+    } else {
+        ret.rotate(_board_orientation);
+    }
     return true;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -187,8 +187,9 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
     // set overall board orientation
-    void set_board_orientation(enum Rotation orientation) {
+    void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
         _board_orientation = orientation;
+        _custom_rotation = custom_rotation;
     }
 
     // return the selected sample rate
@@ -464,6 +465,7 @@ private:
     
     // board orientation from AHRS
     enum Rotation _board_orientation;
+    Matrix3f* _custom_rotation;
 
     // per-sensor orientation to allow for board type defaults at runtime
     enum Rotation _gyro_orientation[INS_MAX_INSTANCES];

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -105,7 +105,11 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
     accel.z *= accel_scale.z;
 
     // rotate to body frame
-    accel.rotate(_imu._board_orientation);
+    if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
+        accel = *_imu._custom_rotation * accel;
+    } else {
+        accel.rotate(_imu._board_orientation);
+    }
 }
 
 void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) 
@@ -116,7 +120,11 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
     // gyro calibration is always assumed to have been done in sensor frame
     gyro -= _imu._gyro_offset[instance];
 
-    gyro.rotate(_imu._board_orientation);
+    if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
+        gyro = *_imu._custom_rotation * gyro;
+    } else {
+        gyro.rotate(_imu._board_orientation);
+    }
 }
 
 /*