// fuse selected position, velocity and height measurements void NavEKF2_core::FuseVelPosNED() { // start performance timer hal.util->perf_begin(_perf_FuseVelPosNED); // health is set bad until test passed velHealth = false; posHealth = false; hgtHealth = false; // declare variables used to check measurement errors Vector3f velInnov; // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; // declare variables used by state and covariance update calculations float posErr; Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only Vector6 observation; float SK; // perform sequential fusion of GPS measurements. This assumes that the // errors in the different velocity and position components are // uncorrelated which is not true, however in the absence of covariance // data from the GPS receiver it is the only assumption we can make // so we might as well take advantage of the computational efficiencies // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { // set the GPS data timeout depending on whether airspeed data is present uint32_t gpsRetryTime; if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms; else gpsRetryTime = frontend->gpsRetryTimeNoTAS_ms; // form the observation vector observation[0] = gpsDataDelayed.vel.x; observation[1] = gpsDataDelayed.vel.y; observation[2] = gpsDataDelayed.vel.z; observation[3] = gpsDataDelayed.pos.x; observation[4] = gpsDataDelayed.pos.y; observation[5] = -hgtMea; // calculate additional error in GPS position caused by manoeuvring posErr = frontend->gpsPosVarAccScale * accNavMag; // estimate the GPS Velocity, GPS horiz position and height measurement variances. // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity // otherwise we scale it using manoeuvre acceleration // Use different errors if flying without GPS using synthetic position and velocity data if (PV_AidingMode == AID_NONE && inFlight) { // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use) // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres R_OBS[0] = sq(10.0f); R_OBS[1] = R_OBS[0]; R_OBS[2] = R_OBS[0]; // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres R_OBS[3] = sq(25.0f); R_OBS[4] = R_OBS[3]; } else { if (gpsSpdAccuracy > 0.0f) { // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f)); R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); } else { // calculate additional error in GPS velocity caused by manoeuvring R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag); } R_OBS[1] = R_OBS[0]; R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; } R_OBS[5] = posDownObsNoise; // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; // if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting // the accelerometers and we should disable the GPS and barometer innovation consistency checks. if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) { // calculate innovations for height and vertical GPS vel measurements float hgtErr = stateStruct.position.z - observation[5]; float velDErr = stateStruct.velocity.z - observation[2]; // check if they are the same sign and both more than 3-sigma out of bounds if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) { badIMUdata = true; } else { badIMUdata = false; } } // calculate innovations and check GPS data validity using an innovation consistency check // test position measurements if (fusePosData) { // test horizontal position measurements innovVelPos[3] = stateStruct.position.x - observation[3]; innovVelPos[4] = stateStruct.position.y - observation[4]; varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4]; // apply an innovation consistency threshold test, but don't fail if bad IMU data float maxPosInnov2 = sq(max(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); // declare a timeout condition if we have been too long without data or not aiding posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE); // use position data if healthy, timed out, or in constant position mode if (posHealth || posTimeout || (PV_AidingMode == AID_NONE)) { posHealth = true; // only reset the failed time and do glitch timeout checks if we are doing full aiding if (PV_AidingMode == AID_ABSOLUTE) { lastPosPassTime_ms = imuSampleTime_ms; // if timed out or outside the specified uncertainty radius, reset to the GPS if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) { // reset the position to the current GPS position ResetPosition(); // reset the velocity to the GPS velocity ResetVelocity(); // don't fuse GPS data on this time step fusePosData = false; fuseVelData = false; // Reset the position variances and corresponding covariances to a value that will pass the checks zeroRows(P,6,7); zeroCols(P,6,7); P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax)); P[7][7] = P[6][6]; // Reset the normalised innovation to avoid failing the bad fusion tests posTestRatio = 0.0f; velTestRatio = 0.0f; } } } else { posHealth = false; } } // test velocity measurements if (fuseVelData) { // test velocity measurements uint8_t imax = 2; // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) { imax = 1; } float innovVelSumSq = 0; // sum of squares of velocity innovations float varVelSum = 0; // sum of velocity innovation variances for (uint8_t i = 0; i<=imax; i++) { // velocity states start at index 3 stateIndex = i + 3; // calculate innovations using blended and single IMU predicted states velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended // calculate innovation variance varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; // sum the innovation and innovation variances innovVelSumSq += sq(velInnov[i]); varVelSum += varInnovVelPos[i]; } // apply an innovation consistency threshold test, but don't fail if bad IMU data // calculate the test ratio velTestRatio = innovVelSumSq / (varVelSum * sq(max(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); // fail if the ratio is greater than 1 velHealth = ((velTestRatio < 1.0f) || badIMUdata); // declare a timeout if we have not fused velocity data for too long or not aiding velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE); // use velocity data if healthy, timed out, or in constant position mode if (velHealth || velTimeout) { velHealth = true; // restart the timeout count lastVelPassTime_ms = imuSampleTime_ms; // If we are doing full aiding and velocity fusion times out, reset to the GPS velocity if (PV_AidingMode == AID_ABSOLUTE && velTimeout) { // reset the velocity to the GPS velocity ResetVelocity(); // don't fuse GPS velocity data on this time step fuseVelData = false; // Reset the normalised innovation to avoid failing the bad fusion tests velTestRatio = 0.0f; } } else { velHealth = false; } } // test height measurements if (fuseHgtData) { // calculate height innovations innovVelPos[5] = stateStruct.position.z - observation[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio hgtTestRatio = sq(innovVelPos[5]) / (sq(max(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); // fail if the ratio is > 1, but don't fail if bad IMU data hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); // Fuse height data if healthy or timed out or in constant position mode if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) { // Calculate a filtered value to be used by pre-flight health checks // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution if (onGround) { float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f; const float hgtInnovFiltTC = 2.0f; float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f); hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha; } else { hgtInnovFiltState = 0.0f; } // if timed out, reset the height if (hgtTimeout) { ResetHeight(); hgtTimeout = false; } // If we have got this far then declare the height data as healthy and reset the timeout counter hgtHealth = true; lastHgtPassTime_ms = imuSampleTime_ms; } } // set range for sequential fusion of velocity and position measurements depending on which data is available and its health if (fuseVelData && velHealth) { fuseData[0] = true; fuseData[1] = true; if (useGpsVertVel) { fuseData[2] = true; } tiltErrVec.zero(); } if (fusePosData && posHealth) { fuseData[3] = true; fuseData[4] = true; tiltErrVec.zero(); } if (fuseHgtData && hgtHealth) { fuseData[5] = true; } // fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) { if (fuseData[obsIndex]) { stateIndex = 3 + obsIndex; // calculate the measurement innovation, using states from a different time coordinate if fusing height data // adjust scaling on GPS measurement noise variances if not enough satellites if (obsIndex <= 2) { innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 3 || obsIndex == 4) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 5) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; const float gndMaxBaroErr = 4.0f; const float gndBaroInnovFloor = -0.5f; if(getTouchdownExpected()) { // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr // this function looks like this: // |/ //---------|--------- // ____/| // / | // / | innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); } } // calculate the Kalman gain and calculate innovation variances varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; SK = 1.0f/varInnovVelPos[obsIndex]; for (uint8_t i= 0; i<=15; i++) { Kfusion[i] = P[i][stateIndex]*SK; } // inhibit magnetic field state estimation by setting Kalman gains to zero if (!inhibitMagStates) { for (uint8_t i = 16; i<=21; i++) { Kfusion[i] = P[i][stateIndex]*SK; } } else { for (uint8_t i = 16; i<=21; i++) { Kfusion[i] = 0.0f; } } // inhibit wind state estimation by setting Kalman gains to zero if (!inhibitWindStates) { Kfusion[22] = P[22][stateIndex]*SK; Kfusion[23] = P[23][stateIndex]*SK; } else { Kfusion[22] = 0.0f; Kfusion[23] = 0.0f; } // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion stateStruct.angErr.zero(); // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data for (uint8_t i = 0; i<=stateIndexLim; i++) { statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex]; } // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion stateStruct.quat.rotate(stateStruct.angErr); // sum the attitude error from velocity and position fusion only // used as a metric for convergence monitoring if (obsIndex != 5) { tiltErrVec += stateStruct.angErr; } // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations // this is a numerically optimised implementation of standard equation P = (I - K*H)*P; for (uint8_t i= 0; i<=stateIndexLim; i++) { for (uint8_t j= 0; j<=stateIndexLim; j++) { KHP[i][j] = Kfusion[i] * P[stateIndex][j]; } } for (uint8_t i= 0; i<=stateIndexLim; i++) { for (uint8_t j= 0; j<=stateIndexLim; j++) { P[i][j] = P[i][j] - KHP[i][j]; } } } } } // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. ForceSymmetry(); ConstrainVariances(); // stop performance timer hal.util->perf_end(_perf_FuseVelPosNED); }
/* Event handler for the details form */ Boolean DetailsFormHandleEvent ( EventType* event /* pointer to an EventType structure */ ) { Boolean handled; Boolean update; handled = false; update = false; switch ( event->eType ) { case ctlSelectEvent: if ( event->data.ctlEnter.controlID == frmDetailsOK ) { UInt16 reference; Boolean oldStatus; Boolean newStatus; reference = GetHistoryCurrent(); oldStatus = ShowImages( reference ); newStatus = CtlGetValue( GetObjectPtr( frmDetailsShowImages ) ); update = oldStatus ^ newStatus; if ( newStatus ) ShowImagesOn( reference ); else ShowImagesOff( reference ); if ( CtlGetValue( GetObjectPtr( frmDetailsStatusRead ) ) ) SetVisitedLink( reference ); else UnsetVisitedLink( reference ); } else if ( event->data.ctlEnter.controlID == frmDetailsCopy ) { FieldType* field; field = GetObjectPtr( frmDetailsLink ); WriteTextFieldToMemo( field ); } else if ( event->data.ctlEnter.controlID != frmDetailsCancel ) { break; } FrmReturnToForm( PREVIOUS_FORM ); if ( update ) { ResetHeight(); FrmUpdateForm( GetMainFormId(), frmRedrawUpdateCode ); } handled = true; break; case winEnterEvent: handled = ResizeHandleWinEnterEvent(); break; case winDisplayChangedEvent: handled = ResizeHandleWinDisplayChangedEvent(); break; case winExitEvent: handled = ResizeHandleWinExitEvent(); break; case frmOpenEvent: #ifdef HAVE_SILKSCREEN ResizeHandleFrmOpenEvent(); #endif DetailsFormInit(); handled = true; break; case frmCloseEvent: #ifdef HAVE_SILKSCREEN ResizeHandleFrmCloseEvent(); #endif handled = false; break; default: handled = false; } return handled; }
// Set inertial navigation aiding mode void NavEKF2_core::setAidingMode() { // Save the previous status so we can detect when it has changed PV_AidingModePrev = PV_AidingMode; // Determine if we should change aiding mode switch (PV_AidingMode) { case AID_NONE: { // 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 && checkGyroCalStatus(); // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present // GPS aiding is the preferred option unless excluded by the user bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit); bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable; bool canUseExtNav = readyToUseExtNav(); if(canUseGPS || canUseRangeBeacon || canUseExtNav) { PV_AidingMode = AID_ABSOLUTE; } else if (optFlowDataPresent() && filterIsStable) { PV_AidingMode = AID_RELATIVE; } } break; case AID_RELATIVE: { // Check if the optical flow sensor has timed out bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000); // Check if the fusion has timed out (flow measurements have been rejected for too long) bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); // Enable switch to absolute position mode if GPS is available // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) { PV_AidingMode = AID_ABSOLUTE; } else if (flowSensorTimeout || flowFusionTimeout) { PV_AidingMode = AID_NONE; } } break; case AID_ABSOLUTE: { // Find the minimum time without data required to trigger any check uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms)); // Check if optical flow data is being used bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms); // Check if airspeed data is being used bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms); // Check if range beacon data is being used bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms); // Check if GPS is being used bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed; // check if velocity drift has been constrained by a measurement source bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed; // check if position drift has been constrained by a measurement source bool posAiding = posUsed || rngBcnUsed; // Check if the loss of attitude aiding has become critical 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); } // Check if the loss of position accuracy has become critical 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) { // if the loss of attitude data is critical, then put the filter into a constant position mode PV_AidingMode = AID_NONE; posTimeout = true; velTimeout = true; rngBcnTimeout = true; tasTimeout = true; gpsNotAvailable = true; } else if (posAidLossCritical) { // if the loss of position is critical, declare all sources of position aiding as being timed out posTimeout = true; velTimeout = true; rngBcnTimeout = true; gpsNotAvailable = true; } break; } } // check to see if we are starting or stopping aiding and set states and modes as required if (PV_AidingMode != PV_AidingModePrev) { // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped. switch (PV_AidingMode) { case AID_NONE: // We have ceased aiding gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index); // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors posTimeout = true; velTimeout = true; // Reset the normalised innovation to avoid false failing bad fusion tests velTestRatio = 0.0f; posTestRatio = 0.0f; // 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; break; case AID_RELATIVE: // We have commenced aiding, but GPS usage has been prohibited so use optical flow only gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index); 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; break; case AID_ABSOLUTE: { bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit); bool canUseRangeBeacon = readyToUseRangeBeacon(); bool canUseExtNav = readyToUseExtNav(); // We have commenced aiding and GPS usage is allowed if (canUseGPS) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index); } posTimeout = false; velTimeout = false; // We have commenced aiding and range beacon usage is allowed if (canUseRangeBeacon) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index); gcs().send_text(MAV_SEVERITY_INFO, "EKF2 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, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset); } // We have commenced aiding and external nav usage is allowed if (canUseExtNav) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index); gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z); // handle yaw reset as special case extNavYawResetRequest = true; controlMagYawReset(); // handle height reset as special case hgtMea = -extNavDataDelayed.pos.z; posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); ResetHeight(); } // reset the last fusion accepted times to prevent unwanted activation of timeout logic lastPosPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms; } break; } // Always reset the position and velocity when changing mode ResetVelocity(); ResetPosition(); } }