mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
aaab250f13
If we start GPS aiding before the gyro bias variances have reduced, glitches on the GPS can cause attitude disturbances that degrade flight accuracy during early flight.
364 lines
14 KiB
C++
364 lines
14 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
|
|
#include "AP_NavEKF2.h"
|
|
#include "AP_NavEKF2_core.h"
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
// Control filter mode transitions
|
|
void NavEKF2_core::controlFilterModes()
|
|
{
|
|
// Determine motor arm status
|
|
prevMotorsArmed = motorsArmed;
|
|
motorsArmed = hal.util->get_soft_armed();
|
|
if (motorsArmed && !prevMotorsArmed) {
|
|
// set the time at which we arm to assist with checks
|
|
timeAtArming_ms = imuSampleTime_ms;
|
|
}
|
|
|
|
// Detect if we are in flight on or ground
|
|
detectFlight();
|
|
|
|
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
|
|
// avoid unnecessary operations
|
|
setWindMagStateLearningMode();
|
|
|
|
// Check the alignmnent status of the tilt and yaw attitude
|
|
// Used during initial bootstrap alignment of the filter
|
|
checkAttitudeAlignmentStatus();
|
|
|
|
// Set the type of inertial navigation aiding used
|
|
setAidingMode();
|
|
|
|
}
|
|
|
|
/*
|
|
return effective value for _magCal for this core
|
|
*/
|
|
uint8_t NavEKF2_core::effective_magCal(void) const
|
|
{
|
|
// if we are on the 2nd core and _magCal is 3 then treat it as
|
|
// 2. This is a workaround for a mag fusion problem
|
|
if (frontend->_magCal ==3 && imu_index == 1) {
|
|
return 2;
|
|
}
|
|
return frontend->_magCal;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
// Set inertial navigation aiding mode
|
|
void NavEKF2_core::setAidingMode()
|
|
{
|
|
// Determine when to commence aiding for inertial navigation
|
|
// Save the previous status so we can detect when it has changed
|
|
prevIsAiding = isAiding;
|
|
// perform aiding checks if not aiding
|
|
if (!isAiding) {
|
|
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
|
// and IMU gyro bias estimates have stabilised
|
|
bool filterIsStable = tiltAlignComplete && yawAlignComplete && imuCalCompleted();
|
|
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
|
|
bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
|
|
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
|
|
// Latch to on
|
|
isAiding = (readyToUseGPS() || useFlowAiding) && filterIsStable;
|
|
}
|
|
|
|
// check to see if we are starting or stopping aiding and set states and modes as required
|
|
if (isAiding != prevIsAiding) {
|
|
// We have transitioned either into or out of aiding
|
|
// zero stored velocities used to do dead-reckoning
|
|
heldVelNE.zero();
|
|
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
|
|
if (!isAiding) {
|
|
// We have ceased aiding
|
|
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
|
|
PV_AidingMode = AID_NONE;
|
|
posTimeout = true;
|
|
velTimeout = true;
|
|
// store the current position to be used to keep reporting the last known position
|
|
lastKnownPositionNE.x = stateStruct.position.x;
|
|
lastKnownPositionNE.y = stateStruct.position.y;
|
|
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
|
|
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
|
|
meaHgtAtTakeOff = baroDataDelayed.hgt;
|
|
// reset the vertical position state to faster recover from baro errors experienced during touchdown
|
|
stateStruct.position.z = -meaHgtAtTakeOff;
|
|
} else if (frontend->_fusionModeGPS == 3) {
|
|
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
|
|
hal.console->printf("EKF2 IMU%u is using optical flow\n",(unsigned)imu_index);
|
|
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
|
|
posTimeout = true;
|
|
velTimeout = true;
|
|
// Reset the last valid flow measurement time
|
|
flowValidMeaTime_ms = imuSampleTime_ms;
|
|
// Reset the last valid flow fusion time
|
|
prevFlowFuseTime_ms = imuSampleTime_ms;
|
|
} else {
|
|
// We have commenced aiding and GPS usage is allowed
|
|
hal.console->printf("EKF2 IMU%u is using GPS\n",(unsigned)imu_index);
|
|
PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
|
|
posTimeout = false;
|
|
velTimeout = false;
|
|
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
|
|
// this is because the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks
|
|
lastTimeGpsReceived_ms = imuSampleTime_ms;
|
|
secondLastGpsTime_ms = imuSampleTime_ms;
|
|
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
|
|
lastPosPassTime_ms = imuSampleTime_ms;
|
|
}
|
|
// Reset the position and velocity
|
|
ResetVelocity();
|
|
ResetPosition();
|
|
|
|
}
|
|
|
|
// Always turn aiding off when the vehicle is disarmed
|
|
if (!isAiding) {
|
|
PV_AidingMode = AID_NONE;
|
|
posTimeout = true;
|
|
velTimeout = true;
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
hal.console->printf("EKF2 IMU%u tilt alignment complete\n",(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;
|
|
}
|
|
}
|
|
}
|
|
|
|
// return true if we should use the airspeed sensor
|
|
bool NavEKF2_core::useAirspeed(void) const
|
|
{
|
|
return _ahrs->airspeed_sensor_enabled();
|
|
}
|
|
|
|
// return true if we should use the range finder sensor
|
|
bool NavEKF2_core::useRngFinder(void) const
|
|
{
|
|
// TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor
|
|
return true;
|
|
}
|
|
|
|
// return true if optical flow data is available
|
|
bool NavEKF2_core::optFlowDataPresent(void) const
|
|
{
|
|
return (imuSampleTime_ms - flowMeaTime_ms < 200);
|
|
}
|
|
|
|
// return true if the filter to be ready to use gps
|
|
bool NavEKF2_core::readyToUseGPS(void) const
|
|
{
|
|
return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3);
|
|
}
|
|
|
|
// return true if we should use the compass
|
|
bool NavEKF2_core::use_compass(void) const
|
|
{
|
|
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
|
|
}
|
|
|
|
/*
|
|
should we assume zero sideslip?
|
|
*/
|
|
bool NavEKF2_core::assume_zero_sideslip(void) const
|
|
{
|
|
// we don't assume zero sideslip for ground vehicles as EKF could
|
|
// be quite sensitive to a rapid spin of the ground vehicle if
|
|
// traction is lost
|
|
return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
|
|
}
|
|
|
|
// set the LLH location of the filters NED origin
|
|
bool NavEKF2_core::setOriginLLH(struct Location &loc)
|
|
{
|
|
if (isAiding) {
|
|
return false;
|
|
}
|
|
EKF_origin = loc;
|
|
validOrigin = true;
|
|
return true;
|
|
}
|
|
|
|
// Set the NED origin to be used until the next filter reset
|
|
void NavEKF2_core::setOrigin()
|
|
{
|
|
// assume origin at current GPS location (no averaging)
|
|
EKF_origin = _ahrs->get_gps().location();
|
|
// define Earth rotation vector in the NED navigation frame at the origin
|
|
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
|
|
validOrigin = true;
|
|
hal.console->printf("EKF2 IMU%u Origin Set\n",(unsigned)imu_index);
|
|
}
|
|
|
|
// record a yaw reset event
|
|
void NavEKF2_core::recordYawReset()
|
|
{
|
|
yawAlignComplete = true;
|
|
if (inFlight) {
|
|
finalInflightYawInit = true;
|
|
}
|
|
}
|
|
|
|
// return true when IMU calibration completed
|
|
bool NavEKF2_core::imuCalCompleted(void)
|
|
{
|
|
// check delta angle bias variances
|
|
const float delAngBiasVarMax = sq(radians(0.1f * dtEkfAvg));
|
|
bool gyroCalComplete = (P[9][9] <= delAngBiasVarMax) &&
|
|
(P[10][10] <= delAngBiasVarMax) &&
|
|
(P[11][11] <= delAngBiasVarMax);
|
|
|
|
return gyroCalComplete;
|
|
|
|
}
|
|
|
|
// Commands the EKF to not use GPS.
|
|
// This command must be sent prior to arming
|
|
// This command is forgotten by the EKF each time the vehicle disarms
|
|
// Returns 0 if command rejected
|
|
// Returns 1 if attitude, vertical velocity and vertical position will be provided
|
|
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
|
|
uint8_t NavEKF2_core::setInhibitGPS(void)
|
|
{
|
|
if(!isAiding) {
|
|
return 0;
|
|
}
|
|
if (optFlowDataPresent()) {
|
|
frontend->_fusionModeGPS = 3;
|
|
//#error writing to a tuning parameter
|
|
return 2;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
#endif // HAL_CPU_CLASS
|