diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index fddaa90aa0..f78a3135fd 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -113,7 +113,7 @@ extern const AP_HAL::HAL& hal; #define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f // maximum gyro bias in rad/sec that can be compensated for -#define MAX_GYRO_BIAS 0.1f +#define MAX_GYRO_BIAS 0.1745f // Define tuning parameters const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -4133,6 +4133,11 @@ void NavEKF::ConstrainStates() state.position.z = constrain_float(state.position.z,-4.0e4f,1.0e4f); // gyro bias limit ~6 deg/sec (this needs to be set based on manufacturers specs) for (uint8_t i=10; i<=12; i++) states[i] = constrain_float(states[i],-0.1f*dtIMUavg,0.1f*dtIMUavg); + // when the vehicle arms we adjust the limits so that in flight the bias can change by the same amount in either direction + float delAngBiasLim = MAX_GYRO_BIAS*dtIMUavg; + state.gyro_bias.x = constrain_float(state.gyro_bias.x, (delAngBiasAtArming.x - delAngBiasLim), (delAngBiasAtArming.x + delAngBiasLim)); + state.gyro_bias.y = constrain_float(state.gyro_bias.y, (delAngBiasAtArming.y - delAngBiasLim), (delAngBiasAtArming.y + delAngBiasLim)); + state.gyro_bias.z = constrain_float(state.gyro_bias.z, (delAngBiasAtArming.z - delAngBiasLim), (delAngBiasAtArming.z + delAngBiasLim)); // Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data) states[13] = constrain_float(states[13],-1.0f*dtIMUavg,1.0f*dtIMUavg); states[22] = constrain_float(states[22],-1.0f*dtIMUavg,1.0f*dtIMUavg); @@ -4855,6 +4860,7 @@ void NavEKF::InitialiseVariables() memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus)); posDownDerivative = 0.0f; posDown = 0.0f; + delAngBiasAtArming.zero(); } // return true if we should use the airspeed sensor @@ -5103,6 +5109,8 @@ void NavEKF::performArmingChecks() // store vertical position at arming to use as a reference for ground relative cehcks if (vehicleArmed) { posDownAtArming = state.position.z; + // save the gyro bias so that the in-flight gyro bias state limits can be adjusted to provide the same amount of offset change in either direction + delAngBiasAtArming = state.gyro_bias; } // zero stored velocities used to do dead-reckoning heldVelNE.zero(); diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 63eb49a02f..065a4898c0 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -724,6 +724,7 @@ private: uint32_t lastConstPosFuseTime_ms; // last time in msec the constant position constraint was applied float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD. float posDown; // Down position state used in calculation of posDownRate + Vector3f delAngBiasAtArming; // value of the gyro delta angle bias at arming // Used by smoothing of state corrections Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement