aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Re-center gyro bias limits on arming

Browse Source 
Allows the gyro bias to vary by up to +-10 deg/second before and after arming.
This commit is contained in:
Paul Riseborough 2015-10-19 15:33:33 +11:00 committed by Andrew Tridgell
parent 8515dda727
commit 240ea92947
2 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -113,7 +113,7 @@ extern const AP_HAL::HAL& hal;
#define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f #define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f
// maximum gyro bias in rad/sec that can be compensated for // 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 // Define tuning parameters
const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { 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); 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) // 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); 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) // 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[13] = constrain_float(states[13],-1.0f*dtIMUavg,1.0f*dtIMUavg);
states[22] = constrain_float(states[22],-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)); memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus));
posDownDerivative = 0.0f; posDownDerivative = 0.0f;
posDown = 0.0f; posDown = 0.0f;
delAngBiasAtArming.zero();
} }
// return true if we should use the airspeed sensor // 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 // store vertical position at arming to use as a reference for ground relative cehcks
if (vehicleArmed) { if (vehicleArmed) {
posDownAtArming = state.position.z; 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 // zero stored velocities used to do dead-reckoning
heldVelNE.zero(); heldVelNE.zero();

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -724,6 +724,7 @@ private:
uint32_t lastConstPosFuseTime_ms; // last time in msec the constant position constraint was applied 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 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 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 // 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 Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement