diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index e909e4460d..697dd27766 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -44,9 +44,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); } @@ -376,6 +373,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 diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 9f54beff1e..fa09ffe01c 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/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