123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567 |
- #include <AP_HAL/AP_HAL.h>
- #include "AP_NavEKF3.h"
- #include "AP_NavEKF3_core.h"
- #include <AP_AHRS/AP_AHRS.h>
- #include <AP_Vehicle/AP_Vehicle.h>
- #include <GCS_MAVLink/GCS.h>
- #include <AP_GPS/AP_GPS.h>
- extern const AP_HAL::HAL& hal;
- void NavEKF3_core::controlFilterModes()
- {
-
- prevMotorsArmed = motorsArmed;
- motorsArmed = hal.util->get_soft_armed();
- if (motorsArmed && !prevMotorsArmed) {
-
- timeAtArming_ms = imuSampleTime_ms;
- }
-
- detectFlight();
-
-
- setWindMagStateLearningMode();
-
-
- checkAttitudeAlignmentStatus();
-
- setAidingMode();
- }
- uint8_t NavEKF3_core::effective_magCal(void) const
- {
-
- if (frontend->_magMask & core_index) {
- return 2;
- } else {
- return frontend->_magCal;
- }
- }
- void NavEKF3_core::setWindMagStateLearningMode()
- {
-
- bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
- if (!inhibitWindStates && setWindInhibit) {
- inhibitWindStates = true;
- updateStateIndexLim();
- } else if (inhibitWindStates && !setWindInhibit) {
- inhibitWindStates = false;
- updateStateIndexLim();
-
- if (yawAlignComplete && useAirspeed()) {
-
-
- 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);
-
- 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 {
-
- for (uint8_t index=22; index<=23; index++) {
- P[index][index] = sq(5.0f);
- }
- }
- }
-
- if (accNavMagHoriz > 0.5f) {
- manoeuvring = true;
- } else {
- manoeuvring = false;
- }
-
- uint8_t magCal = effective_magCal();
- bool magCalRequested =
- ((magCal == 0) && inFlight) ||
- ((magCal == 1) && manoeuvring) ||
- ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) ||
- (magCal == 4);
-
-
- bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
-
- bool setMagInhibit = !magCalRequested || magCalDenied;
- if (!inhibitMagStates && setMagInhibit) {
- inhibitMagStates = true;
- updateStateIndexLim();
- } else if (inhibitMagStates && !setMagInhibit) {
- inhibitMagStates = false;
- updateStateIndexLim();
- if (magFieldLearned) {
-
- 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 {
-
- for (uint8_t index=18; index<=21; index++) {
- P[index][index] = sq(frontend->_magNoise);
- }
-
-
- alignMagStateDeclination();
- }
-
- if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
- magYawResetRequest = true;
- }
- }
-
- if (tiltAlignComplete && inhibitDelVelBiasStates) {
-
- inhibitDelVelBiasStates = false;
- updateStateIndexLim();
-
- P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
- P[14][14] = P[13][13];
- P[15][15] = P[13][13];
- }
- if (tiltAlignComplete && inhibitDelAngBiasStates) {
-
- inhibitDelAngBiasStates = false;
- updateStateIndexLim();
-
- P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
- P[11][11] = P[10][10];
- P[12][12] = P[10][10];
- }
-
-
- if (onGround) {
- finalInflightYawInit = false;
- finalInflightMagInit = false;
- }
- updateStateIndexLim();
- }
- void NavEKF3_core::updateStateIndexLim()
- {
- if (inhibitWindStates) {
- if (inhibitMagStates) {
- if (inhibitDelVelBiasStates) {
- if (inhibitDelAngBiasStates) {
- stateIndexLim = 9;
- } else {
- stateIndexLim = 12;
- }
- } else {
- stateIndexLim = 15;
- }
- } else {
- stateIndexLim = 21;
- }
- } else {
- stateIndexLim = 23;
- }
- }
- void NavEKF3_core::setAidingMode()
- {
-
- PV_AidingModePrev = PV_AidingMode;
-
- checkGyroCalStatus();
-
- switch (PV_AidingMode) {
- case AID_NONE: {
-
-
-
-
- if(readyToUseGPS() || readyToUseRangeBeacon()) {
- PV_AidingMode = AID_ABSOLUTE;
- } else if ((readyToUseOptFlow() && (frontend->_flowUse == FLOW_USE_NAV)) || readyToUseBodyOdm()) {
- PV_AidingMode = AID_RELATIVE;
- }
- break;
- }
- case AID_RELATIVE: {
-
- bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
-
- bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
-
-
- if(readyToUseGPS() || readyToUseRangeBeacon()) {
- PV_AidingMode = AID_ABSOLUTE;
- } else if (flowFusionTimeout && bodyOdmFusionTimeout) {
- PV_AidingMode = AID_NONE;
- }
- break;
- }
- case AID_ABSOLUTE: {
-
- uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
-
- bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
-
- bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_ms <= minTestTime_ms);
-
- bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
-
- bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
-
- bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
- bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
-
- bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed;
-
- bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed;
-
- bool posAiding = gpsPosUsed || rngBcnUsed;
-
- bool attAidLossCritical = false;
- if (!attAiding) {
- attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
- (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
- (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
- (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
- (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
- }
-
- bool posAidLossCritical = false;
- if (!posAiding ) {
- uint16_t maxLossTime_ms;
- if (!velAiding) {
- maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
- } else {
- maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
- }
- posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
- (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
- }
- if (attAidLossCritical) {
-
- PV_AidingMode = AID_NONE;
- posTimeout = true;
- velTimeout = true;
- rngBcnTimeout = true;
- tasTimeout = true;
- gpsNotAvailable = true;
- } else if (posAidLossCritical) {
-
- posTimeout = true;
- velTimeout = true;
- rngBcnTimeout = true;
- gpsNotAvailable = true;
- }
- break;
- }
- }
-
- if (PV_AidingMode != PV_AidingModePrev) {
-
- switch (PV_AidingMode) {
- case AID_NONE:
-
- gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);
-
- posTimeout = true;
- velTimeout = true;
-
- velTestRatio = 0.0f;
- posTestRatio = 0.0f;
-
- lastKnownPositionNE.x = stateStruct.position.x;
- lastKnownPositionNE.y = stateStruct.position.y;
-
-
- meaHgtAtTakeOff = baroDataDelayed.hgt;
-
- stateStruct.position.z = -meaHgtAtTakeOff;
-
- flowFusionActive = false;
- bodyVelFusionActive = false;
- break;
- case AID_RELATIVE:
-
- gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
- if (readyToUseOptFlow()) {
-
- flowValidMeaTime_ms = imuSampleTime_ms;
- prevFlowFuseTime_ms = imuSampleTime_ms;
- } else if (readyToUseBodyOdm()) {
-
- lastbodyVelPassTime_ms = imuSampleTime_ms;
- prevBodyVelFuseTime_ms = imuSampleTime_ms;
- }
- posTimeout = true;
- velTimeout = true;
- break;
- case AID_ABSOLUTE:
- if (readyToUseGPS()) {
-
- posResetSource = GPS;
- velResetSource = GPS;
- gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
- } else if (readyToUseRangeBeacon()) {
-
- posResetSource = RNGBCN;
- velResetSource = DEFAULT;
- gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
- gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
- gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
- }
-
- posTimeout = false;
- velTimeout = false;
-
- lastPosPassTime_ms = imuSampleTime_ms;
- lastVelPassTime_ms = imuSampleTime_ms;
- lastRngBcnPassTime_ms = imuSampleTime_ms;
- break;
- }
-
- ResetVelocity();
- ResetPosition();
- }
- }
- void NavEKF3_core::checkAttitudeAlignmentStatus()
- {
-
-
-
- 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",(unsigned)imu_index);
- }
- }
-
- if (!yawAlignComplete && tiltAlignComplete && use_compass()) {
- magYawResetRequest = true;
- }
- }
- bool NavEKF3_core::useAirspeed(void) const
- {
- return _ahrs->airspeed_sensor_enabled();
- }
- bool NavEKF3_core::useRngFinder(void) const
- {
-
- return true;
- }
- bool NavEKF3_core::readyToUseOptFlow(void) const
- {
-
- return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
- }
- bool NavEKF3_core::readyToUseBodyOdm(void) const
- {
-
- bool visoDataGood = (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) && (bodyOdmDataNew.velErr < 1.0f);
-
- bool wencDataGood = (imuSampleTime_ms - wheelOdmMeasTime_ms < 200);
-
-
- return (visoDataGood || wencDataGood)
- && tiltAlignComplete
- && delAngBiasLearned;
- }
- bool NavEKF3_core::readyToUseGPS(void) const
- {
- return validOrigin && tiltAlignComplete && yawAlignComplete && delAngBiasLearned && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit;
- }
- bool NavEKF3_core::readyToUseRangeBeacon(void) const
- {
- return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse;
- }
- bool NavEKF3_core::use_compass(void) const
- {
- return (frontend->_magCal != 5) && _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
- }
- bool NavEKF3_core::assume_zero_sideslip(void) const
- {
-
-
-
- return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
- }
- bool NavEKF3_core::setOriginLLH(const Location &loc)
- {
- if (PV_AidingMode == AID_ABSOLUTE) {
- return false;
- }
- EKF_origin = loc;
- ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
-
- calcEarthRateNED(earthRateNED, loc.lat);
- validOrigin = true;
- return true;
- }
- void NavEKF3_core::setOrigin(const Location &loc)
- {
- EKF_origin = loc;
-
- if (inFlight) {
- EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z);
- }
- ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
-
- calcEarthRateNED(earthRateNED, EKF_origin.lat);
- validOrigin = true;
- gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index);
-
- frontend->common_EKF_origin = EKF_origin;
- frontend->common_origin_valid = true;
- }
- void NavEKF3_core::recordYawReset()
- {
- yawAlignComplete = true;
- if (inFlight) {
- finalInflightYawInit = true;
- }
- }
- void NavEKF3_core::checkGyroCalStatus(void)
- {
-
- const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
- delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
- (P[11][11] <= delAngBiasVarMax) &&
- (P[12][12] <= delAngBiasVarMax);
- }
- uint8_t NavEKF3_core::setInhibitGPS(void)
- {
- if((PV_AidingMode == AID_ABSOLUTE) || motorsArmed) {
- return 0;
- } else {
- gpsInhibit = true;
- return 1;
- }
- }
- void NavEKF3_core::updateFilterStatus(void)
- {
-
- filterStatus.value = 0;
- bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000);
- bool doingFlowNav = (PV_AidingMode != AID_NONE) && 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 || doingBodyVelNav;
- bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
-
- bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
-
- filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy;
- filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy;
- filterStatus.flags.vert_vel = someVertRefData && filterHealthy;
- filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy;
- filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy;
- filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate;
- filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy;
- filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy;
- filterStatus.flags.pred_horiz_pos_rel = filterStatus.flags.horiz_pos_rel;
- filterStatus.flags.pred_horiz_pos_abs = filterStatus.flags.horiz_pos_abs;
- filterStatus.flags.takeoff_detected = takeOffDetected;
- filterStatus.flags.takeoff = expectGndEffectTakeoff;
- filterStatus.flags.touchdown = expectGndEffectTouchdown;
- filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
- filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3);
- filterStatus.flags.gps_quality_good = gpsGoodToAlign;
- }
|