AP_InertialSensor: added INS_CALSENSFRAME parameter

this allows us to detect if accel calibration was done in sensor frame
or not. If it was done in sensor frame then the accel calibration is
independent of AHRS_ORIENTATION, which makes it easier to move a board
to a new airframe without having to recalibrate.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -226,6 +226,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
     AP_GROUPINFO("GYR3OFFS",   10, AP_InertialSensor, _gyro_offset[2],   0),
 #endif
 
+    // @Param: INS_CALSENSFRAME
+    // @DisplayName: Calibration is in sensor frame
+    // @Description: This is an internal parameter that records whether accelerometer calibration was done in sensor frame. It is used to detect an old accel calibration which was done in body frame. This parameter is automatically set during accelerometer calibration and should not be changed by the user.
+    // @Values: 0:BoardFrame,1:SensorFrame
+    // @User: Advanced
+    AP_GROUPINFO("CALSENSFRAME", 11, AP_InertialSensor, _cal_sensor_frame, 0),
+
     AP_GROUPEND
 };
 
@@ -411,6 +418,13 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
     Vector3f orig_scale[INS_MAX_INSTANCES];
     uint8_t num_ok = 0;
 
+    /*
+      we do the accel calibration with no board rotation. This avoids
+      having to rotate readings during the calibration
+    */
+    enum Rotation saved_orientation = _board_orientation;
+    _board_orientation = ROTATION_NONE;
+
     for (uint8_t k=0; k<num_accels; k++) {
         // backup original offsets and scaling
         orig_offset[k] = _accel_offset[k].get();
@@ -505,8 +519,20 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
 
         check_3D_calibration();
 
-        // calculate the trims as well from primary accels and pass back to caller
-        _calculate_trim(samples[0][0], trim_roll, trim_pitch);
+        /*
+          calculate the trims as well from primary accels 
+          We use the original board rotation for this sample
+        */
+        Vector3f level_sample = samples[0][0];
+        level_sample.rotate(saved_orientation);
+
+        _calculate_trim(level_sample, trim_roll, trim_pitch);
+
+        _board_orientation = saved_orientation;
+
+        // now set and save the INS_CALSENSFRAME parameter so we know
+        // the calibration was done in sensor frame
+        _cal_sensor_frame.set_and_save(1);
 
         return true;
     }
@@ -518,6 +544,7 @@ failed:
         _accel_offset[k].set(orig_offset[k]);
         _accel_scale[k].set(orig_scale[k]);
     }
+    _board_orientation = saved_orientation;
     return false;
 }
 #endif
@@ -732,6 +759,13 @@ AP_InertialSensor::_init_gyro()
     // cold start
     hal.console->print_P(PSTR("Init Gyro"));
 
+    /*
+      we do the gyro calibration with no board rotation. This avoids
+      having to rotate readings during the calibration
+    */
+    enum Rotation saved_orientation = _board_orientation;
+    _board_orientation = ROTATION_NONE;
+
     // remove existing gyro offsets
     for (uint8_t k=0; k<num_gyros; k++) {
         _gyro_offset[k] = Vector3f(0,0,0);
@@ -827,6 +861,9 @@ AP_InertialSensor::_init_gyro()
         }
     }
 
+    // restore orientation
+    _board_orientation = saved_orientation;
+
     // record calibration complete
     _calibrating = false;
 
@@ -982,11 +1019,11 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float
 }
 
 // _calculate_trim  - calculates the x and y trim angles (in radians) given a raw accel sample (i.e. no scaling or offsets applied) taken when the vehicle was level
-void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch)
+void AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch)
 {
     // scale sample and apply offsets
-    Vector3f accel_scale = _accel_scale[0].get();
-    Vector3f accel_offsets = _accel_offset[0].get();
+    const Vector3f &accel_scale = _accel_scale[0].get();
+    const Vector3f &accel_offsets = _accel_offset[0].get();
     Vector3f scaled_accels_x( accel_sample.x * accel_scale.x - accel_offsets.x,
                               0,
                               accel_sample.z * accel_scale.z - accel_offsets.z );

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -232,7 +232,7 @@ private:
     void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
     void _calibrate_reset_matrices(float dS[6], float JS[6][6]);
     void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
-    void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
+    void _calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch);
 #endif
 
     // check if we have 3D accel calibration
@@ -275,6 +275,9 @@ private:
     // filtering frequency (0 means default)
     AP_Int8     _mpu6000_filter;
 
+    // was the accel cal done in sensor frame?
+    AP_Int8    _cal_sensor_frame;
+
     // board orientation from AHRS
     enum Rotation _board_orientation;
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -9,18 +9,37 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
     _product_id(AP_PRODUCT_ID_NONE)
 {}
 
-void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) {
-    const Vector3f &accel_scale = _imu._accel_scale[instance].get();
+void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) 
+{
+    /*
+      we rotate before or after offset and scaling based on the
+      INS_CALSENSFRAME parameter. This allows us to use older
+      calibrations, while making all new calibrations operate in
+      sensor frame, and thus be independent of AHRS_ORIENTATION
+     */
+    if (_imu._cal_sensor_frame == 0) {
         accel.rotate(_imu._board_orientation);
+    }
+
+    // apply scaling
+    const Vector3f &accel_scale = _imu._accel_scale[instance].get();
     accel.x *= accel_scale.x;
     accel.y *= accel_scale.y;
     accel.z *= accel_scale.z;
+
+    // apply offsets
     accel -= _imu._accel_offset[instance];
+
+    if (_imu._cal_sensor_frame != 0) {
+        accel.rotate(_imu._board_orientation);
+    }
 }
 
-void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) {
-    gyro.rotate(_imu._board_orientation);
+void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) 
+{
+    // gyro calibration is always assumed to have been done in sensor frame
     gyro -= _imu._gyro_offset[instance];
+    gyro.rotate(_imu._board_orientation);
 }
 
 void AP_InertialSensor_Backend::_publish_delta_angle(uint8_t instance, const Vector3f &delta_angle)
@@ -55,6 +74,7 @@ void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const
  */
 void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct)
 {
+    const Vector3f &accel_scale = _imu._accel_scale[instance].get();
     _imu._accel[instance] = accel;
     _imu._accel_healthy[instance] = true;