// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// Determine if learning of magnetic field states has been requested by the user
bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring) || (frontend._magCal == 3);
// Deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user
bool magCalDenied = !use_compass() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2);
// Inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
}
// Update the filter status
void NavEKF2_core::updateFilterStatus(void)
{
// init return value
filterStatus.value = 0;
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
// set individual flags
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
}
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
allMagSensorsFailed = true;
return;
}
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
}
}
}
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// save magnetometer measurement to buffer to be fused later
storedMag.push(magDataNew);
}
}
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
memset(&storedMag[0], 0, sizeof(storedMag));
}
}
}
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms);
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// save magnetometer measurement to buffer to be fused later
StoreMag();
}
}
// Check the tilt and yaw alignmnent status
// Used during initial bootstrap alignment of the filter
void NavEKF3_core::checkAttitudeAlignmentStatus()
{
// Check for tilt convergence - used during initial alignment
if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.035f) && !tiltAlignComplete) {
tiltAlignComplete = true;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index);
}
// submit yaw and magnetic field reset request depending on whether we have compass data
if (use_compass()&& !yawAlignComplete && tiltAlignComplete) {
magYawResetRequest = true;
}
}
// Check the tilt and yaw alignmnent status
// Used during initial bootstrap alignment of the filter
void NavEKF3_core::checkAttitudeAlignmentStatus()
{
// Check for tilt convergence - used during initial alignment
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if (!tiltAlignComplete) {
Vector3f angleErrVarVec = calcRotVecVariances();
if ((angleErrVarVec.x + angleErrVarVec.y) < sq(0.05235f)) {
tiltAlignComplete = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index);
}
}
// submit yaw and magnetic field reset request
if (!yawAlignComplete && tiltAlignComplete && use_compass()) {
magYawResetRequest = true;
}
}
// correct a bearing in centi-degrees for wind
void AP_AHRS::wind_correct_bearing(int32_t &nav_bearing_cd)
{
if (!use_compass() || !_flags.wind_estimation) {
// we are not using the compass - no wind correction,
// as GPS gives course over ground already
return;
}
// if we are using a compass for navigation, then adjust the
// heading to account for wind
Vector3f wind = wind_estimate();
Vector2f wind2d = Vector2f(wind.x, wind.y);
float speed;
if (airspeed_estimate(&speed)) {
Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
Vector2f nav_adjusted = nav_vector - wind2d;
nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;
}
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// Determine if learning of magnetic field states has been requested by the user
bool magCalRequested =
((frontend->_magCal == 0) && inFlight) || // when flying
((frontend->_magCal == 1) && manoeuvring) || // when manoeuvring
((frontend->_magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
(frontend->_magCal == 4); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because we want it re-done for each takeoff
if (onGround) {
firstMagYawInit = false;
}
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
}
// Check the tilt and yaw alignmnent status
// Used during initial bootstrap alignment of the filter
void NavEKF2_core::checkAttitudeAlignmentStatus()
{
// Check for tilt convergence - used during initial alignment
float alpha = 1.0f*imuDataDelayed.delAngDT;
float temp=tiltErrVec.length();
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index);
}
// submit yaw and magnetic field reset requests depending on whether we have compass data
if (tiltAlignComplete && !yawAlignComplete) {
if (use_compass()) {
magYawResetRequest = true;
gpsYawResetRequest = false;
} else {
magYawResetRequest = false;
gpsYawResetRequest = true;
}
}
}
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec();
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms);
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// check if compass offsets have been changed and adjust EKF bias states to maintain consistent innovations
if (_ahrs->get_compass()->healthy(0)) {
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
if (changeDetected && firstMagYawInit) {
stateStruct.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
stateStruct.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
stateStruct.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;
}
lastMagOffsets = nowMagOffsets;
}
// save magnetometer measurement to buffer to be fused later
StoreMag();
}
}
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
// http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
Vector3f velocity;
uint32_t last_correction_time;
// perform yaw drift correction if we have a new yaw reference
// vector
drift_correction_yaw();
// rotate accelerometer values into the earth frame
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (_ins.get_accel_health(i)) {
_accel_ef[i] = _dcm_matrix * _ins.get_accel(i);
// integrate the accel vector in the earth frame between GPS readings
_ra_sum[i] += _accel_ef[i] * deltat;
}
}
// keep a sum of the deltat values, so we know how much time
// we have integrated over
_ra_deltat += deltat;
if (!have_gps() ||
_gps.status() < AP_GPS::GPS_OK_FIX_3D ||
_gps.num_sats() < _gps_minsats) {
// no GPS, or not a good lock. From experience we need at
// least 6 satellites to get a really reliable velocity number
// from the GPS.
//
// As a fallback we use the fixed wing acceleration correction
// if we have an airspeed estimate (which we only have if
// _fly_forward is set), otherwise no correction
if (_ra_deltat < 0.2f) {
// not enough time has accumulated
return;
}
float airspeed;
if (_airspeed && _airspeed->use()) {
airspeed = _airspeed->get_airspeed();
} else {
airspeed = _last_airspeed;
}
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
// add in wind estimate
velocity += _wind;
last_correction_time = hal.scheduler->millis();
_have_gps_lock = false;
} else {
if (_gps.last_fix_time_ms() == _ra_sum_start) {
// we don't have a new GPS fix - nothing more to do
return;
}
velocity = _gps.velocity();
last_correction_time = _gps.last_fix_time_ms();
if (_have_gps_lock == false) {
// if we didn't have GPS lock in the last drift
// correction interval then set the velocities equal
_last_velocity = velocity;
}
_have_gps_lock = true;
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
}
if (have_gps()) {
// use GPS for positioning with any fix, even a 2D fix
_last_lat = _gps.location().lat;
_last_lng = _gps.location().lng;
_position_offset_north = 0;
_position_offset_east = 0;
// once we have a single GPS lock, we can update using
// dead-reckoning from then on
_have_position = true;
} else {
// update dead-reckoning position estimate
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
}
// see if this is our first time through - in which case we
// just setup the start times and return
if (_ra_sum_start == 0) {
_ra_sum_start = last_correction_time;
_last_velocity = velocity;
return;
}
// equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f GA_e;
GA_e = Vector3f(0, 0, -1.0f);
bool using_gps_corrections = false;
float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);
if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
float v_scale = gps_gain.get() * ra_scale;
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
GA_e += vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
_last_failure_ms = hal.scheduler->millis();
return;
}
using_gps_corrections = true;
}
// calculate the error term in earth frame.
// we do this for each available accelerometer then pick the
// accelerometer that leads to the smallest error term. This takes
// advantage of the different sample rates on different
// accelerometers to dramatically reduce the impact of aliasing
// due to harmonics of vibrations that match closely the sampling
// rate of our accelerometers. On the Pixhawk we have the LSM303D
// running at 800Hz and the MPU6000 running at 1kHz, by combining
// the two the effects of aliasing are greatly reduced.
Vector3f error[INS_MAX_INSTANCES];
Vector3f GA_b[INS_MAX_INSTANCES];
int8_t besti = -1;
float best_error = 0;
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (!_ins.get_accel_health(i)) {
// only use healthy sensors
continue;
}
_ra_sum[i] *= ra_scale;
// get the delayed ra_sum to match the GPS lag
if (using_gps_corrections) {
GA_b[i] = ra_delayed(i, _ra_sum[i]);
} else {
GA_b[i] = _ra_sum[i];
}
if (GA_b[i].is_zero()) {
// wait for some non-zero acceleration information
continue;
}
GA_b[i].normalize();
if (GA_b[i].is_inf()) {
// wait for some non-zero acceleration information
continue;
}
error[i] = GA_b[i] % GA_e;
float error_length = error[i].length();
if (besti == -1 || error_length < best_error) {
besti = i;
best_error = error_length;
}
}
if (besti == -1) {
// no healthy accelerometers!
_last_failure_ms = hal.scheduler->millis();
return;
}
_active_accel_instance = besti;
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z
float earth_error_Z = error.z;
// equation 10
float tilt = pythagorous2(GA_e.x, GA_e.y);
// equation 11
float theta = atan2f(GA_b[besti].y, GA_b[besti].x);
// equation 12
Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
// step 6
error = GA_b[besti] % GA_e2;
error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION
// to reduce the impact of two competing yaw controllers, we
// reduce the impact of the gps/accelerometers on yaw when we are
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (use_compass()) {
if (have_gps() && gps_gain == 1.0f) {
error[besti].z *= sinf(fabsf(roll));
} else {
error[besti].z = 0;
}
}
// if ins is unhealthy then stop attitude drift correction and
// hope the gyros are OK for a while. Just slowly reduce _omega_P
// to prevent previous bad accels from throwing us off
if (!_ins.healthy()) {
error[besti].zero();
} else {
// convert the error term to body frame
error[besti] = _dcm_matrix.mul_transpose(error[besti]);
}
if (error[besti].is_nan() || error[besti].is_inf()) {
// don't allow bad values
check_matrix();
_last_failure_ms = hal.scheduler->millis();
return;
}
_error_rp_sum += best_error;
_error_rp_count++;
// base the P gain on the spin rate
float spin_rate = _omega.length();
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error[besti] * _P_gain(spin_rate) * _kp;
if (_flags.fast_ground_gains) {
_omega_P *= 8;
}
if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
_gps.ground_speed() < GPS_SPEED_MIN &&
_ins.get_accel().x >= 7 &&
pitch_sensor > -3000 && pitch_sensor < 3000) {
// assume we are in a launch acceleration, and reduce the
// rp gain by 50% to reduce the impact of GPS lag on
// takeoff attitude when using a catapult
_omega_P *= 0.5f;
}
// accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
_omega_I_sum += error[besti] * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
}
if (_omega_I_sum_time >= 5) {
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
// zero our accumulator ready for the next GPS step
memset(&_ra_sum[0], 0, sizeof(_ra_sum));
_ra_deltat = 0;
_ra_sum_start = last_correction_time;
// remember the velocity for next time
_last_velocity = velocity;
}
// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
bool new_value = false;
float yaw_error;
float yaw_deltat;
if (use_compass()) {
/*
we are using compass for yaw
*/
if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6f;
_compass_last_update = _compass->last_update;
// we force an additional compass read()
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_flags.have_initial_yaw && _compass->read()) {
float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass();
// also update the _gps_last_update, so if we later
// disable the compass due to significant yaw error we
// don't suddenly change yaw with a reset
_gps_last_update = _gps.last_fix_time_ms();
}
} else if (_flags.fly_forward && have_gps()) {
/*
we are using GPS for yaw
*/
if (_gps.last_fix_time_ms() != _gps_last_update &&
_gps.ground_speed() >= GPS_SPEED_MIN) {
yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps.last_fix_time_ms();
new_value = true;
float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f);
float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
yaw_error = sinf(yaw_error_rad);
/* reset yaw to match GPS heading under any of the
following 3 conditions:
1) if we have reached GPS_SPEED_MIN and have never had
yaw information before
2) if the last time we got yaw information from the GPS
is more than 20 seconds ago, which means we may have
suffered from considerable gyro drift
3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
and our yaw error is over 60 degrees, which means very
poor yaw. This can happen on bungee launch when the
operator pulls back the plane rapidly enough then on
release the GPS heading changes very rapidly
*/
if (!_flags.have_initial_yaw ||
yaw_deltat > 20 ||
(_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
// reset DCM matrix based on current yaw
_dcm_matrix.from_euler(roll, pitch, gps_course_rad);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
yaw_error = 0;
}
}
}
if (!new_value) {
// we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss
// of our yaw source
_omega_yaw_P *= 0.97f;
return;
}
// convert the error vector to body frame
float error_z = _dcm_matrix.c.z * yaw_error;
// the spin rate changes the P gain, and disables the
// integration at higher rates
float spin_rate = _omega.length();
// update the proportional control to drag the
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani
// We also adjust the gain depending on the rate of change of horizontal velocity which
// is proportional to how observable the heading is from the acceerations and GPS velocity
// The accelration derived heading will be more reliable in turns than compass or GPS
_omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain();
if (_flags.fast_ground_gains) {
_omega_yaw_P.z *= 8;
}
// don't update the drift term if we lost the yaw reference
// for more than 2 seconds
if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
// also add to the I term
_omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
}
_error_yaw_sum += fabsf(yaw_error);
_error_yaw_count++;
}
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
// http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
Matrix3f temp_dcm = _dcm_matrix;
Vector3f velocity;
uint32_t last_correction_time;
// perform yaw drift correction if we have a new yaw reference
// vector
drift_correction_yaw();
// apply trim
temp_dcm.rotateXY(_trim);
// rotate accelerometer values into the earth frame
_accel_ef = temp_dcm * _accel_vector;
// integrate the accel vector in the earth frame between GPS readings
_ra_sum += _accel_ef * deltat;
// keep a sum of the deltat values, so we know how much time
// we have integrated over
_ra_deltat += deltat;
if (!have_gps() ||
_gps->status() < GPS::GPS_OK_FIX_3D ||
_gps->num_sats < _gps_minsats) {
// no GPS, or not a good lock. From experience we need at
// least 6 satellites to get a really reliable velocity number
// from the GPS.
//
// As a fallback we use the fixed wing acceleration correction
// if we have an airspeed estimate (which we only have if
// _fly_forward is set), otherwise no correction
if (_ra_deltat < 0.2f) {
// not enough time has accumulated
return;
}
float airspeed;
if (_airspeed && _airspeed->use()) {
airspeed = _airspeed->get_airspeed();
} else {
airspeed = _last_airspeed;
}
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
// add in wind estimate
velocity += _wind;
last_correction_time = hal.scheduler->millis();
_have_gps_lock = false;
} else {
if (_gps->last_fix_time == _ra_sum_start) {
// we don't have a new GPS fix - nothing more to do
return;
}
velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), _gps->velocity_down());
last_correction_time = _gps->last_fix_time;
if (_have_gps_lock == false) {
// if we didn't have GPS lock in the last drift
// correction interval then set the velocities equal
_last_velocity = velocity;
}
_have_gps_lock = true;
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
}
if (have_gps()) {
// use GPS for positioning with any fix, even a 2D fix
_last_lat = _gps->latitude;
_last_lng = _gps->longitude;
_position_offset_north = 0;
_position_offset_east = 0;
// once we have a single GPS lock, we can update using
// dead-reckoning from then on
_have_position = true;
} else {
// update dead-reckoning position estimate
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
}
// see if this is our first time through - in which case we
// just setup the start times and return
if (_ra_sum_start == 0) {
_ra_sum_start = last_correction_time;
_last_velocity = velocity;
return;
}
// equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f GA_e;
GA_e = Vector3f(0, 0, -1.0f);
if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
// limit vertical acceleration correction to 0.5 gravities. The
// barometer sometimes gives crazy acceleration changes.
vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f);
GA_e += vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
return;
}
}
// calculate the error term in earth frame.
Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS);
float length = GA_b.length();
if (length > 1.0f) {
GA_b /= length;
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
}
}
Vector3f error = GA_b % GA_e;
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z
float earth_error_Z = error.z;
// equation 10
float tilt = pythagorous2(GA_e.x, GA_e.y);
// equation 11
float theta = atan2f(GA_b.y, GA_b.x);
// equation 12
Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
// step 6
error = GA_b % GA_e2;
error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION
// to reduce the impact of two competing yaw controllers, we
// reduce the impact of the gps/accelerometers on yaw when we are
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (use_compass()) {
if (have_gps() && gps_gain == 1.0f) {
error.z *= sinf(fabsf(roll));
} else {
error.z = 0;
}
}
// convert the error term to body frame
error = _dcm_matrix.mul_transpose(error);
if (error.is_nan() || error.is_inf()) {
// don't allow bad values
check_matrix();
return;
}
_error_rp_sum += error.length();
_error_rp_count++;
// base the P gain on the spin rate
float spin_rate = _omega.length();
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _P_gain(spin_rate) * _kp;
if (_flags.fast_ground_gains) {
_omega_P *= 8;
}
if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D &&
_gps->ground_speed_cm < GPS_SPEED_MIN &&
_accel_vector.x >= 7 &&
pitch_sensor > -3000 && pitch_sensor < 3000) {
// assume we are in a launch acceleration, and reduce the
// rp gain by 50% to reduce the impact of GPS lag on
// takeoff attitude when using a catapult
_omega_P *= 0.5f;
}
// accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
_omega_I_sum += error * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
}
if (_omega_I_sum_time >= 5) {
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
// zero our accumulator ready for the next GPS step
_ra_sum.zero();
_ra_deltat = 0;
_ra_sum_start = last_correction_time;
// remember the velocity for next time
_last_velocity = velocity;
if (_have_gps_lock && _flags.fly_forward) {
// update wind estimate
estimate_wind(velocity);
}
}
// check for new magnetometer data and update store measurements if available
void NavEKF3_core::readMagData()
{
if (!_ahrs->get_compass()) {
allMagSensorsFailed = true;
return;
}
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
allMagSensorsFailed = true;
return;
}
// limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && ((_ahrs->get_compass()->last_update_usec() - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
frontend->logging.log_compass = true;
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false;
// request a reset of the magnetic field states
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
}
}
}
// detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
if (changeDetected) {
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
}
lastMagOffsets = nowMagOffsets;
lastMagOffsetsValid = true;
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// save magnetometer measurement to buffer to be fused later
storedMag.push(magDataNew);
}
}
/* Monitor GPS data to see if quality is good enough to initialise the EKF
Monitor magnetometer innovations to to see if the heading is good enough to use GPS
Return true if all criteria pass for 10 seconds
We also record the failure reason so that prearm_failure_reason()
can give a good report to the user on why arming is failing
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
if (inFlight && assume_zero_sideslip() && !use_compass()) {
// this is a special case where a plane has launched without magnetometer
// is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
return true;
}
// User defined multiplier to be applied to check thresholds
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
// This enables us to handle large changes to the external magnetic field environment that occur before arming
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) {
magYawResetTimer_ms = imuSampleTime_ms;
}
if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
// reset heading and field states
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
magYawResetTimer_ms = imuSampleTime_ms;
}
// Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary
const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsesd since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length();
gpsloc_prev = gpsloc;
// Decay distance moved exponentially to zero
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
// Clamp the fiter state to prevent excessive persistence of large transients
gpsDriftNE = MIN(gpsDriftNE,10.0f);
// Fail if more than 3 metres drift after filtering whilst on-ground
// This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT);
// Report check result as a text string and bitmask
if (gpsDriftFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
gpsCheckStatus.bad_horiz_drift = true;
} else {
gpsCheckStatus.bad_horiz_drift = false;
}
// Check that the vertical GPS vertical velocity is reasonable after noise filtering
bool gpsVertVelFail;
if (_ahrs->get_gps().have_vertical_velocity() && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and
// EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
}
} else {
gpsVertVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsVertVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
gpsCheckStatus.bad_vert_vel = true;
} else {
gpsCheckStatus.bad_vert_vel = false;
}
// Check that the horizontal GPS vertical velocity is reasonable after noise filtering
// This check can only be used if the vehicle is stationary
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsHorizVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
gpsCheckStatus.bad_horiz_vel = true;
} else {
gpsCheckStatus.bad_horiz_vel = false;
}
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
}
// Report check result as a text string and bitmask
if (hAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
gpsCheckStatus.bad_hAcc = true;
} else {
gpsCheckStatus.bad_hAcc = false;
}
// fail if reported speed accuracy greater than threshold
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR);
// Report check result as a text string and bitmask
if (gpsSpdAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS speed error %.1f (needs %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
gpsCheckStatus.bad_sAcc = true;
} else {
gpsCheckStatus.bad_sAcc = false;
}
// fail if satellite geometry is poor
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop()));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if not enough sats
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
}
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
}
// Report check result as a text string and bitmask
if (yawFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"Mag yaw error x=%.1f y=%.1f",
(double)magTestRatio.x,
(double)magTestRatio.y);
gpsCheckStatus.bad_yaw = true;
} else {
gpsCheckStatus.bad_yaw = false;
}
// assume failed first time through and notify user checks have started
if (lastGpsVelFail_ms == 0) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
lastGpsVelFail_ms = imuSampleTime_ms;
}
// record time of fail
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
lastGpsVelFail_ms = imuSampleTime_ms;
}
// continuous period without fail required to return a healthy status
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true;
}
return false;
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
if (!inhibitWindStates && setWindInhibit) {
inhibitWindStates = true;
} else if (inhibitWindStates && !setWindInhibit) {
inhibitWindStates = false;
// set states and variances
if (yawAlignComplete && useAirspeed()) {
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
// which assumes the vehicle has launched into the wind
Vector3f tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);
// set the wind sate variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f));
}
} else {
// set the variances using a typical wind speed
for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(5.0f);
}
}
}
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// Determine if learning of magnetic field states has been requested by the user
uint8_t magCal = effective_magCal();
bool magCalRequested =
((magCal == 0) && inFlight) || // when flying
((magCal == 1) && manoeuvring) || // when manoeuvring
((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
(magCal == 4); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied
bool setMagInhibit = !magCalRequested || magCalDenied;
if (!inhibitMagStates && setMagInhibit) {
inhibitMagStates = true;
} else if (inhibitMagStates && !setMagInhibit) {
inhibitMagStates = false;
if (magFieldLearned) {
// if we have already learned the field states, then retain the learned variances
P[16][16] = earthMagFieldVar.x;
P[17][17] = earthMagFieldVar.y;
P[18][18] = earthMagFieldVar.z;
P[19][19] = bodyMagFieldVar.x;
P[20][20] = bodyMagFieldVar.y;
P[21][21] = bodyMagFieldVar.z;
} else {
// set the variances equal to the observation variances
for (uint8_t index=18; index<=21; index++) {
P[index][index] = sq(frontend->_magNoise);
}
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination();
}
// request a reset of the yaw and magnetic field states if not done before
if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
magYawResetRequest = true;
}
}
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because we want it re-done for each takeoff
if (onGround) {
finalInflightYawInit = false;
finalInflightMagInit = false;
}
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
}
// select fusion of velocity, position and height measurements
void NavEKF2_core::SelectVelPosFusion()
{
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
posVelFusionDelayed = true;
return;
} else {
posVelFusionDelayed = false;
}
// Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
// read GPS data from the sensor and check for new data in the buffer
readGpsData();
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// set fusion request flags
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fusePosData = true;
extNavUsedForPos = false;
// correct GPS data for position offset of antenna phase centre relative to the IMU
Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (!posOffsetBody.is_zero()) {
// Don't fuse velocity data if GPS doesn't support it
if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3f velOffsetBody = angRate % posOffsetBody;
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
gpsDataDelayed.vel.x -= velOffsetEarth.x;
gpsDataDelayed.vel.y -= velOffsetEarth.y;
gpsDataDelayed.vel.z -= velOffsetEarth.z;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
gpsDataDelayed.pos.x -= posOffsetEarth.x;
gpsDataDelayed.pos.y -= posOffsetEarth.y;
gpsDataDelayed.hgt += posOffsetEarth.z;
}
// copy corrected GPS data to observation vector
if (fuseVelData) {
velPosObs[0] = gpsDataDelayed.vel.x;
velPosObs[1] = gpsDataDelayed.vel.y;
velPosObs[2] = gpsDataDelayed.vel.z;
}
velPosObs[3] = gpsDataDelayed.pos.x;
velPosObs[4] = gpsDataDelayed.pos.y;
} else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// This is a special case that uses and external nav system for position
extNavUsedForPos = true;
activeHgtSource = HGT_SOURCE_EV;
fuseVelData = false;
fuseHgtData = true;
fusePosData = true;
velPosObs[3] = extNavDataDelayed.pos.x;
velPosObs[4] = extNavDataDelayed.pos.y;
velPosObs[5] = extNavDataDelayed.pos.z;
// if compass is disabled, also use it for yaw
if (!use_compass()) {
extNavUsedForYaw = true;
if (!yawAlignComplete) {
extNavYawResetRequest = true;
magYawResetRequest = false;
gpsYawResetRequest = false;
controlMagYawReset();
finalInflightYawInit = true;
} else {
fuseEulerYaw();
}
} else {
extNavUsedForYaw = false;
}
} else {
fuseVelData = false;
fusePosData = false;
}
// we have GPS data to fuse and a request to align the yaw using the GPS course
if (gpsYawResetRequest) {
realignYawGPS();
}
// Select height data to be fused from the available baro, range finder and GPS sources
selectHeightForFusion();
// if we are using GPS, check for a change in receiver and reset position and height
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) {
// record the ID of the GPS that we are using for the reset
last_gps_idx = gpsDataDelayed.sensor_idx;
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
posResetNE.y = stateStruct.position.y;
// Set the position states to the position from the new GPS
stateStruct.position.x = gpsDataNew.pos.x;
stateStruct.position.y = gpsDataNew.pos.y;
// Calculate the position offset due to the reset
posResetNE.x = stateStruct.position.x - posResetNE.x;
posResetNE.y = stateStruct.position.y - posResetNE.y;
// Add the offset to the output observer states
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.x += posResetNE.x;
storedOutput[i].position.y += posResetNE.y;
}
outputDataNew.position.x += posResetNE.x;
outputDataNew.position.y += posResetNE.y;
outputDataDelayed.position.x += posResetNE.x;
outputDataDelayed.position.y += posResetNE.y;
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
// If we are alseo using GPS as the height reference, reset the height
if (activeHgtSource == HGT_SOURCE_GPS) {
// Store the position before the reset so that we can record the reset delta
posResetD = stateStruct.position.z;
// write to the state vector
stateStruct.position.z = -hgtMea;
// Calculate the position jump due to the reset
posResetD = stateStruct.position.z - posResetD;
// Add the offset to the output observer states
outputDataNew.position.z += posResetD;
outputDataDelayed.position.z += posResetD;
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.z += posResetD;
}
// store the time of the reset
lastPosResetD_ms = imuSampleTime_ms;
}
}
// If we are operating without any aiding, fuse in the last known position
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
// Do this to coincide with the height fusion
if (fuseHgtData && PV_AidingMode == AID_NONE) {
velPosObs[3] = lastKnownPositionNE.x;
velPosObs[4] = lastKnownPositionNE.y;
fusePosData = true;
fuseVelData = false;
}
// perform fusion
if (fuseVelData || fusePosData || fuseHgtData) {
FuseVelPosNED();
// clear the flags to prevent repeated fusion of the same data
fuseVelData = false;
fuseHgtData = false;
fusePosData = false;
}
}
