mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 15:33:57 -04:00
AP_NavEKF2: rework yaw and magnetic heading reset logic
Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags. Reduce the number of places where decisions to perform a yaw and field reset are made. Don't perform a reset unless there is is data in the buffer Don't use 3-axis fusion if the field states still need to be reset. When starting 3-axis fusion request a reset if not previously performed. Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
a6cbc5d4a5
commit
24d8cc62e2
@ -98,12 +98,12 @@ void NavEKF2_core::setWindMagStateLearningMode()
|
||||
bool magCalRequested =
|
||||
((magCal == 0) && inFlight) || // when flying
|
||||
((magCal == 1) && manoeuvring) || // when manoeuvring
|
||||
((magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
|
||||
((magCal == 3) && firstInflightYawInit && firstInflightMagInit) || // 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);
|
||||
bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
|
||||
|
||||
// Inhibit the magnetic field calibration if not requested or denied
|
||||
bool setMagInhibit = !magCalRequested || magCalDenied;
|
||||
@ -115,12 +115,17 @@ void NavEKF2_core::setWindMagStateLearningMode()
|
||||
for (uint8_t index=16; index<=21; index++) {
|
||||
P[index][index] = sq(frontend->_magNoise);
|
||||
}
|
||||
// request a reset of the yaw and magnetic field states if not done before
|
||||
if (!magStateInitComplete || (!firstInflightMagInit && 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) {
|
||||
firstMagYawInit = false;
|
||||
firstInflightYawInit = false;
|
||||
firstInflightMagInit = false;
|
||||
}
|
||||
|
||||
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
|
||||
@ -205,7 +210,7 @@ void NavEKF2_core::setAidingMode()
|
||||
}
|
||||
}
|
||||
|
||||
// Check the alignmnent status of the tilt and yaw attitude
|
||||
// Check the tilt and yaw alignmnent status
|
||||
// Used during initial bootstrap alignment of the filter
|
||||
void NavEKF2_core::checkAttitudeAlignmentStatus()
|
||||
{
|
||||
@ -218,14 +223,15 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
|
||||
hal.console->printf("EKF2 IMU%u tilt alignment complete\n",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// Once tilt has converged, align yaw using magnetic field measurements
|
||||
if (tiltAlignComplete && !yawAlignComplete && use_compass()) {
|
||||
Vector3f eulerAngles;
|
||||
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
|
||||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
StoreQuatReset();
|
||||
yawAlignComplete = true;
|
||||
hal.console->printf("EKF2 IMU%u yaw 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -293,6 +299,15 @@ void NavEKF2_core::setOrigin()
|
||||
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) {
|
||||
firstInflightYawInit = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
@ -23,17 +23,17 @@ void NavEKF2_core::controlMagYawReset()
|
||||
// Use a quaternion division to calcualte the delta quaternion between the rotation at the current and last time
|
||||
Quaternion deltaQuat = stateStruct.quat / prevQuatMagReset;
|
||||
prevQuatMagReset = stateStruct.quat;
|
||||
|
||||
// convert the quaternion to a rotation vector and find its length
|
||||
Vector3f deltaRotVec;
|
||||
deltaQuat.to_axis_angle(deltaRotVec);
|
||||
float deltaRot = deltaRotVec.length();
|
||||
|
||||
// In-Flight reset for vehicle that cannot use a zero sideslip assumption
|
||||
// Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
|
||||
// Perform the reset earlier if high yaw and velocity innovations indicate that 'toilet bowling' is occurring
|
||||
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
|
||||
// Delay if rotated too far since the last check as rapid rotations will produce errors in the magnetic field states
|
||||
if (!firstMagYawInit && !assume_zero_sideslip() && inFlight && deltaRot < 0.1745f) {
|
||||
// check if the spin rate is OK - high spin rates can cause angular alignment errors
|
||||
bool angRateOK = deltaRotVec.length() < 0.1745f;
|
||||
|
||||
// a flight reset is allowed if we are not spinning too fast and are in flight
|
||||
bool flightResetAllowed = angRateOK && inFlight;
|
||||
|
||||
// check that we have reached a height where ground magnetic interference effects are insignificant
|
||||
bool hgtCheckPassed = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
|
||||
|
||||
@ -41,25 +41,63 @@ void NavEKF2_core::controlMagYawReset()
|
||||
// this can occur if there is severe magnetic interference on the ground
|
||||
bool toiletBowling = (yawTestRatio > 1.0f) && (velTestRatio > 1.0f);
|
||||
|
||||
if (hgtCheckPassed || toiletBowling) {
|
||||
firstMagYawInit = true;
|
||||
// Update the yaw angle and earth field states using the magnetic field measurements
|
||||
Quaternion tempQuat;
|
||||
// calculate if an in flight reset is required
|
||||
bool flightResetRequired = (!firstInflightYawInit && (hgtCheckPassed || toiletBowling));
|
||||
|
||||
// an initial reset is required if we have not yet aligned the yaw angle
|
||||
bool initialResetRequired = !yawAlignComplete;
|
||||
|
||||
// a combined yaw angle and magnetic field reset can be initiated by:
|
||||
magYawResetRequest = magYawResetRequest || // an external request
|
||||
(initialResetRequired && angRateOK) || // we need to do an initial alignment and aren't rotating too fast
|
||||
(flightResetRequired && flightResetAllowed && !assume_zero_sideslip()); // conditions are right to do the in-flight re-alignment
|
||||
|
||||
// Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
|
||||
if (magYawResetRequest || magStateResetRequest) {
|
||||
|
||||
// gett he eeruler angles from the current state estimate
|
||||
Vector3f eulerAngles;
|
||||
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
|
||||
tempQuat = stateStruct.quat;
|
||||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
|
||||
// Use the Euler angles and magnetometer measurement to update the magnetic field states
|
||||
// and get an updated quaternion
|
||||
Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
|
||||
// if a yaw reset has been requested, apply the updated quaterniont the current state
|
||||
if (magYawResetRequest) {
|
||||
// previous value used to calculate a reset delta
|
||||
Quaternion prevQuat = stateStruct.quat;
|
||||
|
||||
// update the quaternion states using the new yaw angle
|
||||
stateStruct.quat = newQuat;
|
||||
|
||||
// calculate the change in the quaternion state and apply it to the ouput history buffer
|
||||
tempQuat = stateStruct.quat/tempQuat;
|
||||
StoreQuatRotate(tempQuat);
|
||||
}
|
||||
prevQuat = stateStruct.quat/prevQuat;
|
||||
StoreQuatRotate(prevQuat);
|
||||
|
||||
// send initial alignment status to console
|
||||
if (!yawAlignComplete) {
|
||||
hal.console->printf("EKF2 IMU%u initial yaw alignment complete\n",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// In-Flight reset for vehicles that can use a zero sideslip assumption (Planes)
|
||||
// this is done to protect against unrecoverable heading alignment errors due to compass faults
|
||||
if (!firstMagYawInit && assume_zero_sideslip() && inFlight) {
|
||||
realignYawGPS();
|
||||
firstMagYawInit = true;
|
||||
// send initial in-flight yaw alignment status to console
|
||||
if (!firstInflightYawInit && inFlight) {
|
||||
hal.console->printf("EKF2 IMU%u in-flight yaw alignment complete\n",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// update the yaw reset completed status
|
||||
recordYawReset();
|
||||
|
||||
// clear the yaw reset request flag
|
||||
magYawResetRequest = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Request an in-flight check of heading against GPS and reset if necessary
|
||||
// this can only be used by vehicles that can use a zero sideslip assumption (Planes)
|
||||
// this allows recovery for heading alignment errors due to compass faults
|
||||
if (!firstInflightYawInit && inFlight && assume_zero_sideslip()) {
|
||||
gpsYawResetRequest = true;
|
||||
}
|
||||
|
||||
}
|
||||
@ -78,23 +116,13 @@ void NavEKF2_core::realignYawGPS()
|
||||
float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
|
||||
|
||||
// calculate course yaw angle from GPS velocity
|
||||
float gpsYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x);
|
||||
|
||||
// check if this is our first alignment and we are not using the compass
|
||||
if (!yawAlignComplete && !use_compass()) {
|
||||
// calculate new filter quaternion states from Euler angles
|
||||
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
|
||||
yawAlignComplete = true;
|
||||
|
||||
// zero the attitude covariances becasue the corelations will now be invalid
|
||||
zeroAttCovOnly();
|
||||
}
|
||||
float gpsYaw = atan2f(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
|
||||
|
||||
// Check the yaw angles for consistency
|
||||
float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),MAX(fabsf(wrap_PI(gpsYaw - eulerAngles.z)),fabsf(wrap_PI(velYaw - eulerAngles.z))));
|
||||
|
||||
// If the angles disagree by more than 45 degrees and GPS innovations are large or no compass, we declare the magnetic yaw as bad
|
||||
badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && !(PV_AidingMode == AID_NONE)) || !use_compass();
|
||||
// If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
|
||||
badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
|
||||
|
||||
// correct yaw angle using GPS ground course if compass yaw bad
|
||||
if (badMagYaw) {
|
||||
@ -102,18 +130,27 @@ void NavEKF2_core::realignYawGPS()
|
||||
// calculate new filter quaternion states from Euler angles
|
||||
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
|
||||
|
||||
// send yaw alignment information to console
|
||||
hal.console->printf("EKF2 IMU%u yaw aligned to GPS velocity\n",(unsigned)imu_index);
|
||||
|
||||
// zero the attitude covariances becasue the corelations will now be invalid
|
||||
zeroAttCovOnly();
|
||||
|
||||
// reset tposition fusion timer to cause the states to be reset to the GPS on the next GPS fusion cycle
|
||||
lastPosPassTime_ms = 0;
|
||||
}
|
||||
|
||||
// record the yaw reset event
|
||||
recordYawReset();
|
||||
|
||||
// clear any GPS yaw requests
|
||||
gpsYawResetRequest = false;
|
||||
}
|
||||
|
||||
// fix magnetic field states and clear any compass fault conditions
|
||||
if (use_compass()) {
|
||||
// reset the magnetometer field states - we could have got bad external interference when initialising on-ground
|
||||
calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
// submit a request to reset the magnetic field states
|
||||
magStateResetRequest = true;
|
||||
|
||||
// We shoud retry the primary magnetometer if previously switched or failed
|
||||
magSelectIndex = 0;
|
||||
@ -149,8 +186,8 @@ void NavEKF2_core::SelectMagFusion()
|
||||
// check for availability of magnetometer data to fuse
|
||||
magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
|
||||
|
||||
if (magDataToFuse) {
|
||||
// Control reset of yaw and magnetic field states
|
||||
// Control reset of yaw and magnetic field states if we are using compass data
|
||||
if (magDataToFuse && use_compass()) {
|
||||
controlMagYawReset();
|
||||
}
|
||||
|
||||
@ -158,8 +195,8 @@ void NavEKF2_core::SelectMagFusion()
|
||||
// wait until the EKF time horizon catches up with the measurement
|
||||
bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete);
|
||||
if (dataReady) {
|
||||
// If we haven't performed the first airborne magnetic field update or have inhibited magnetic field learning, then we use the simple method of declination to maintain heading
|
||||
if(inhibitMagStates) {
|
||||
// use the simple method of declination to maintain heading if we cannot use the magnetic field states
|
||||
if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) {
|
||||
fuseEulerYaw();
|
||||
// zero the test ratio output from the inactive 3-axis magneteometer fusion
|
||||
magTestRatio.zero();
|
||||
@ -193,12 +230,6 @@ void NavEKF2_core::SelectMagFusion()
|
||||
yawTestRatio = 0.0f;
|
||||
lastSynthYawTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
// In-Flight yaw alignment for vehicles that can use a zero sideslip assumption (Planes)
|
||||
// and are not using a compass
|
||||
if (!yawAlignComplete && assume_zero_sideslip() && inFlight) {
|
||||
realignYawGPS();
|
||||
firstMagYawInit = yawAlignComplete;
|
||||
}
|
||||
}
|
||||
|
||||
// stop performance timer
|
||||
@ -739,8 +770,9 @@ void NavEKF2_core::fuseEulerYaw()
|
||||
|
||||
// Use the difference between the horizontal projection and declination to give the measured yaw
|
||||
// If we can't use compass data, set the meaurement to the predicted
|
||||
// to prevent uncontrolled variance growth whilst on ground without magnetometer
|
||||
float measured_yaw;
|
||||
if (use_compass()) {
|
||||
if (use_compass() && yawAlignComplete && magStateInitComplete) {
|
||||
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
|
||||
} else {
|
||||
measured_yaw = predicted_yaw;
|
||||
@ -1007,5 +1039,14 @@ void NavEKF2_core::alignMagStateDeclination()
|
||||
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
|
||||
}
|
||||
|
||||
// record a magentic field state reset event
|
||||
void NavEKF2_core::recordMagReset()
|
||||
{
|
||||
magStateInitComplete = true;
|
||||
if (inFlight) {
|
||||
firstInflightMagInit = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
|
||||
@ -350,7 +350,7 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
|
||||
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
||||
{
|
||||
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
|
||||
if (mag_idx == magSelectIndex && firstMagYawInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
|
||||
if (mag_idx == magSelectIndex && firstInflightMagInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
|
||||
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
|
||||
return true;
|
||||
} else {
|
||||
|
||||
@ -196,6 +196,11 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
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();
|
||||
|
||||
|
||||
@ -39,10 +39,8 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
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);
|
||||
// request reset of heading and magnetic field states
|
||||
magYawResetRequest = true;
|
||||
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
|
||||
magYawResetTimer_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
@ -153,7 +153,8 @@ void NavEKF2_core::InitialiseVariables()
|
||||
tasTimeout = true;
|
||||
badMagYaw = false;
|
||||
badIMUdata = false;
|
||||
firstMagYawInit = false;
|
||||
firstInflightYawInit = false;
|
||||
firstInflightMagInit = false;
|
||||
dtIMUavg = 0.0025f;
|
||||
dtEkfAvg = 0.01f;
|
||||
dt = 0;
|
||||
@ -253,6 +254,10 @@ void NavEKF2_core::InitialiseVariables()
|
||||
runUpdates = false;
|
||||
framesSincePredict = 0;
|
||||
lastMagOffsetsValid = false;
|
||||
magStateResetRequest = false;
|
||||
magStateInitComplete = false;
|
||||
magYawResetRequest = false;
|
||||
gpsYawResetRequest = false;
|
||||
|
||||
// zero data buffers
|
||||
storedIMU.reset();
|
||||
@ -1317,7 +1322,7 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
|
||||
|
||||
// calculate yaw angle rel to true north
|
||||
yaw = magDecAng - magHeading;
|
||||
yawAlignComplete = true;
|
||||
|
||||
// calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
|
||||
// otherwise use existing heading
|
||||
if (!badMagYaw) {
|
||||
@ -1360,8 +1365,15 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
|
||||
|
||||
// clear bad magnetic yaw status
|
||||
badMagYaw = false;
|
||||
|
||||
// clear mag state reset request
|
||||
magStateResetRequest = false;
|
||||
|
||||
// record the fact we have initialised the magnetic field states
|
||||
recordMagReset();
|
||||
} else {
|
||||
// no magnetoemter data so there is nothing we can do
|
||||
// this function should not be called if there is no compass data but if is is, return the
|
||||
// current attitude
|
||||
initQuat = stateStruct.quat;
|
||||
}
|
||||
|
||||
|
||||
@ -560,7 +560,7 @@ private:
|
||||
// avoid unnecessary operations
|
||||
void setWindMagStateLearningMode();
|
||||
|
||||
// Check the alignmnent status of the tilt and yaw attitude
|
||||
// Check the alignmnent status of the tilt attitude
|
||||
// Used during initial bootstrap alignment of the filter
|
||||
void checkAttitudeAlignmentStatus();
|
||||
|
||||
@ -611,6 +611,12 @@ private:
|
||||
// zero attitude state covariances, but preserve variances
|
||||
void zeroAttCovOnly();
|
||||
|
||||
// record a yaw reset event
|
||||
void recordYawReset();
|
||||
|
||||
// record a magnetic field state reset event
|
||||
void recordMagReset();
|
||||
|
||||
// effective value of MAG_CAL
|
||||
uint8_t effective_magCal(void) const;
|
||||
|
||||
@ -708,7 +714,8 @@ private:
|
||||
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
bool firstMagYawInit; // true when the first post takeoff initialisation of earth field and yaw angle has been performed
|
||||
bool firstInflightYawInit; // true when the first post takeoff initialisation of yaw angle has been performed
|
||||
bool firstInflightMagInit; // true when the first post takeoff initialisation of magnetic field states been performed
|
||||
bool gpsNotAvailable; // bool true when valid GPS data is not available
|
||||
bool isAiding; // true when the filter is fusing position, velocity or flow measurements
|
||||
bool prevIsAiding; // isAiding from previous frame
|
||||
@ -726,6 +733,7 @@ private:
|
||||
float tiltErrFilt; // Filtered tilt error metric
|
||||
bool tiltAlignComplete; // true when tilt alignment is complete
|
||||
bool yawAlignComplete; // true when yaw alignment is complete
|
||||
bool magStateInitComplete; // true when the magnetic field sttes have been initialised
|
||||
uint8_t stateIndexLim; // Max state index used during matrix and array operations
|
||||
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
|
||||
imu_elements imuDataNew; // IMU data at the current time horizon
|
||||
@ -780,6 +788,9 @@ private:
|
||||
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele
|
||||
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
|
||||
float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
|
||||
bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter measurements
|
||||
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
|
||||
bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
|
||||
|
||||
// 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.
|
||||
|
||||
Loading…
Reference in New Issue
Block a user