AP_InertialNav: added constraint to how large local accelerometer offset corrections can become

libraries/AP_InertialNav/AP_InertialNav.cpp

@@ -53,9 +53,6 @@ void AP_InertialNav::init()
 void AP_InertialNav::save_params()
 {
     Vector3f accel_corr = accel_correction.get();
-    accel_corr.x = constrain(accel_corr.x,-100,100);
-    accel_corr.y = constrain(accel_corr.y,-100,100);
-    accel_corr.z = constrain(accel_corr.z,-100,100);
     accel_correction.set_and_save(accel_corr);
 }
 
@@ -385,6 +382,13 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
     accel_corr.x += accel_ef_z_correction * dcm.c.x;
     accel_corr.y += accel_ef_z_correction * dcm.c.y;
     accel_corr.z += accel_ef_z_correction * dcm.c.z;
+
+    // ensure corrections are reasonable
+    accel_corr.x = constrain(accel_corr.x,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
+    accel_corr.y = constrain(accel_corr.y,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
+    accel_corr.z = constrain(accel_corr.z,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
+
+    // set the parameter to include the corrections
     accel_correction.set(accel_corr);
 
     // correct velocity

libraries/AP_InertialNav/AP_InertialNav.h

@@ -12,6 +12,8 @@
 #define AP_INTERTIALNAV_TC_XY   3.0 // default time constant for complementary filter's X & Y axis
 #define AP_INTERTIALNAV_TC_Z    7.0 // default time constant for complementary filter's Z axis
 
+#define AP_INTERTIALNAV_ACCEL_CORR_MAX 100.0    // max allowed accelerometer offset correction
+
 // #defines to control how often historical accel based positions are saved
 // so they can later be compared to laggy gps readings
 #define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS   10