AP_InertialSensor: use accel instance max abs offset instead of a constant value

The calibration on LSM9DS0 was giving offsets between 4.0 and 4.2 on x-axis and
around 3.6 on y-axis. It turned out that those offsets were actually right.

The maximum absolute values of calibration offset should be a sensor
characteristic rather than a constant value for all sensors.

The constant value previously used (3.5 m/s/s for all axes) is set here as a
default maximum absolute calibration offset for every instance to keep it
working.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -301,6 +301,8 @@ AP_InertialSensor::AP_InertialSensor() :
 #if INS_VIBRATION_CHECK
         _accel_clip_count[i] = 0;
 #endif
+
+        _accel_max_abs_offsets[i] = Vector3f(3.5f, 3.5f, 3.5f);
     }
     memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
     memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
@@ -578,7 +580,11 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
             continue;
         }
 
-        bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k], saved_orientation);
+        bool success = _calibrate_accel(samples[k],
+                                        new_offsets[k],
+                                        new_scaling[k],
+                                        _accel_max_abs_offsets[k],
+                                        saved_orientation);
 
         interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
                             (unsigned)k,
@@ -977,7 +983,9 @@ bool AP_InertialSensor::_check_sample_range(const Vector3f accel_sample[6], enum
 // accel_scale are output from the calibration routine
 // returns true if successful
 bool AP_InertialSensor::_calibrate_accel(const Vector3f accel_sample[6],
-                                         Vector3f& accel_offsets, Vector3f& accel_scale,
+                                         Vector3f& accel_offsets,
+                                         Vector3f& accel_scale,
+                                         Vector3f& max_abs_offsets,
                                          enum Rotation rotation)
 {
     int16_t i;
@@ -1034,8 +1042,11 @@ bool AP_InertialSensor::_calibrate_accel(const Vector3f accel_sample[6],
     if( accel_scale.is_nan() || fabsf(accel_scale.x-1.0f) > 0.1f || fabsf(accel_scale.y-1.0f) > 0.1f || fabsf(accel_scale.z-1.0f) > 0.1f ) {
         success = false;
     }
-    // sanity check offsets (3.5 is roughly 3/10th of a G, 5.0 is roughly half a G)
-    if( accel_offsets.is_nan() || fabsf(accel_offsets.x) > 3.5f || fabsf(accel_offsets.y) > 3.5f || fabsf(accel_offsets.z) > 3.5f ) {
+
+    if (accel_offsets.is_nan() ||
+            fabsf(accel_offsets.x) > max_abs_offsets.x ||
+            fabsf(accel_offsets.y) > max_abs_offsets.y ||
+            fabsf(accel_offsets.z) > max_abs_offsets.z) {
         success = false;
     }
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -248,7 +248,11 @@ private:
     // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
 
     // _calibrate_accel - perform low level accel calibration
-    bool _calibrate_accel(const Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale, enum Rotation r);
+    bool _calibrate_accel(const Vector3f accel_sample[6],
+                          Vector3f& accel_offsets,
+                          Vector3f& accel_scale,
+                          Vector3f& max_abs_offsets,
+                          enum Rotation rotation);
     bool _check_sample_range(const Vector3f accel_sample[6], enum Rotation rotation, 
                              AP_InertialSensor_UserInteract* interact);
     void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
@@ -292,6 +296,9 @@ private:
     AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
     AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
 
+    // accelerometer max absolute offsets to be used for calibration
+    Vector3f _accel_max_abs_offsets[INS_MAX_INSTANCES];
+
     // temperatures for an instance if available
     float _temperature[INS_MAX_INSTANCES];
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -78,6 +78,12 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
     }
 }
 
+void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,
+                                                          const Vector3f offset)
+{
+    _imu._accel_max_abs_offsets[instance] = offset;
+}
+
 // set accelerometer error_count
 void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
 {

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -73,6 +73,9 @@ protected:
     // rotate accel vector, scale, offset and publish
     void _publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct = true);
 
+    // set accelerometer max absolute offset for calibration
+    void _set_accel_max_abs_offset(uint8_t instance, const Vector3f offset);
+
     // publish a temperature value
     void _publish_temperature(uint8_t instance, float temperature);