diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 6d4ddd68e3..640bcf7c5c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -585,7 +585,7 @@ void NavEKF2::UpdateFilter(void) for (uint8_t i=0; i 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h b/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h index 4f566fd69e..5cae2d1e9f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h @@ -113,7 +113,7 @@ private: }; -// Folowing buffer model is for IMU data, +// Following buffer model is for IMU data, // it achieves a distance of sample size // between youngest and oldest template diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 74039f6c25..4cff357212 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -94,7 +94,7 @@ void NavEKF2_core::setAidingMode() prevIsAiding = isAiding; // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete bool filterIsStable = tiltAlignComplete && yawAlignComplete; - // If GPS useage has been prohiited then we use flow aiding provided optical flow data is present + // 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 @@ -105,7 +105,7 @@ void NavEKF2_core::setAidingMode() // We have transitioned either into or out of aiding // zero stored velocities used to do dead-reckoning heldVelNE.zero(); - // set various useage modes based on the condition when we start aiding. These are then held until aiding is stopped. + // 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 @@ -121,7 +121,7 @@ void NavEKF2_core::setAidingMode() // 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 useage has been prohibited so use optical flow only + // 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; @@ -131,13 +131,13 @@ void NavEKF2_core::setAidingMode() // Reset the last valid flow fusion time prevFlowFuseTime_ms = imuSampleTime_ms; } else { - // We have commenced aiding and GPS useage is allowed + // 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 becasue the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks + // 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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 5a05f54475..e29a4fca2f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -102,8 +102,8 @@ void NavEKF2_core::alignYawGPS() // calculate new filter quaternion states from Euler angles stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw); - // The correlations between attitude errors and positon and velocity errors in the covariance matrix - // are invalid becasue og the changed yaw angle, so reset the corresponding row and columns + // The correlations between attitude errors and position and velocity errors in the covariance matrix + // are invalid because og the changed yaw angle, so reset the corresponding row and columns zeroCols(P,0,2); zeroRows(P,0,2); @@ -111,7 +111,7 @@ void NavEKF2_core::alignYawGPS() P[1][1] = P[0][0] = sq(radians(5.0f)); P[2][2] = sq(radians(45.0f)); - // reset tposition fusion timer to casue the states to be reset to the GPS on the next GPS fusion cycle + // reset tposition fusion timer to cause the states to be reset to the GPS on the next GPS fusion cycle lastPosPassTime_ms = 0; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index dd65d30ad6..932e6cc695 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -280,7 +280,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const location_offset(loc, outputDataNew.position.x, outputDataNew.position.y); return true; } else { - // we could be in constant position mode becasue the vehicle has taken off without GPS, or has lost GPS + // we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS // in this mode we cannot use the EKF states to estimate position so will return the best available data if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { // we have a GPS position fix to return @@ -461,7 +461,7 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3); bool gpsNavPossible = !gpsNotAvailable && (PV_AidingMode == AID_ABSOLUTE) && gpsGoodToAlign; bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete; - // If GPS height useage is specified, height is considered to be inaccurate until the GPS passes all checks + // 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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index f99e5bed56..ad970480f0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -274,7 +274,7 @@ void NavEKF2_core::FuseVelPosNED() R_OBS[5] = posDownObsNoise; for (uint8_t i=3; 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 + // if vertical GPS velocity data and an independent 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)) { @@ -589,7 +589,7 @@ void NavEKF2_core::selectHeightForFusion() hgtMea = gpsDataDelayed.hgt; // enable fusion fuseHgtData = true; - // set the observation noise to the horizontal GPS noise plus a scaler becasue GPS vertical position is usually less accurate + // set the observation noise to the horizontal GPS noise plus a scaler because GPS vertical position is usually less accurate // TODO use VDOP/HDOP, reported accuracy or a separate parameter posDownObsNoise = sq(constrain_float(frontend->_gpsHorizPosNoise * 1.5f, 0.1f, 10.0f)); } else if (baroDataToFuse && !usingRangeForHgt && !usingGpsForHgt) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 47ca6b2778..4369556ec7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -508,7 +508,7 @@ void NavEKF2_core::UpdateStrapdownEquationsNED() accNavMag = velDotNEDfilt.length(); accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y); - // save velocity for use in trapezoidal intergration for position calcuation + // save velocity for use in trapezoidal integration for position calcuation Vector3f lastVelocity = stateStruct.velocity; // sum delta velocities to get velocity @@ -563,7 +563,7 @@ void NavEKF2_core::calcOutputStatesFast() { Vector3f delVelNav = Tbn_temp*imuDataNew.delVel + delVelCorrection; delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT; - // save velocity for use in trapezoidal intergration for position calcuation + // save velocity for use in trapezoidal integration for position calcuation Vector3f lastVelocity = outputDataNew.velocity; // sum delta velocities to get velocity @@ -619,7 +619,7 @@ void NavEKF2_core::calcOutputStatesFast() { /* * Calculate the predicted state covariance matrix using algebraic equations generated with Matlab symbolic toolbox. - * The script file used to generate these and otehr equations in this filter can be found here: + * The script file used to generate these and other equations in this filter can be found here: * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m */ void NavEKF2_core::CovariancePrediction() diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index e051885711..74f52daa21 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -680,8 +680,8 @@ private: uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec) uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec) uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec) - uint32_t lastTimeGpsReceived_ms;// last time we recieved GPS data - uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements + uint32_t lastTimeGpsReceived_ms;// last time we received GPS data + uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data @@ -776,7 +776,7 @@ private: uint8_t localFilterTimeStep_ms; // average number of msec between filter updates float posDownObsNoise; // observationn noise on the vertical position used by the state and covariance update step (m) - // variables used to calulate a vertical velocity that is kinematically consistent with the verical position + // variables used to calculate a vertical velocity that is kinematically consistent with the verical position float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD. float posDown; // Down position state used in calculation of posDownRate