Browse Source

AP_NavEKF2: update function name

The primary purpose of this function is re-alignment
mission-4.1.18
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
commit
64a8153b68
  1. 6
      libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
  2. 2
      libraries/AP_NavEKF2/AP_NavEKF2_core.h

6
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@ -60,7 +60,7 @@ void NavEKF2_core::controlMagYawReset()
// In-Flight reset for vehicles that can use a zero sideslip assumption (Planes) // 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 // this is done to protect against unrecoverable heading alignment errors due to compass faults
if (!firstMagYawInit && assume_zero_sideslip() && inFlight) { if (!firstMagYawInit && assume_zero_sideslip() && inFlight) {
alignYawGPS(); realignYawGPS();
firstMagYawInit = true; firstMagYawInit = true;
} }
@ -74,9 +74,9 @@ void NavEKF2_core::controlMagYawReset()
} }
// this function is used to do a forced alignment of the yaw angle to align with the horizontal velocity // this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity
// vector from GPS. It is used to align the yaw angle after launch or takeoff. // vector from GPS. It is used to align the yaw angle after launch or takeoff.
void NavEKF2_core::alignYawGPS() void NavEKF2_core::realignYawGPS()
{ {
// get quaternion from existing filter states and calculate roll, pitch and yaw angles // get quaternion from existing filter states and calculate roll, pitch and yaw angles
Vector3f eulerAngles; Vector3f eulerAngles;

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h

@ -502,7 +502,7 @@ private:
void SelectBetaFusion(); void SelectBetaFusion();
// force alignment of the yaw angle using GPS velocity data // force alignment of the yaw angle using GPS velocity data
void alignYawGPS(); void realignYawGPS();
// initialise the earth magnetic field states using declination and current attitude and magnetometer meaasurements // initialise the earth magnetic field states using declination and current attitude and magnetometer meaasurements
// and return attitude quaternion // and return attitude quaternion

Loading…
Cancel
Save