|
|
|
@ -342,7 +342,7 @@ void NavEKF3_core::SelectVelPosFusion()
@@ -342,7 +342,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|
|
|
|
// store the time of the reset
|
|
|
|
|
lastPosReset_ms = imuSampleTime_ms; |
|
|
|
|
|
|
|
|
|
// If we are alseo using GPS as the height reference, reset the height
|
|
|
|
|
// If we are also using GPS as the height reference, reset the height
|
|
|
|
|
if (activeHgtSource == HGT_SOURCE_GPS) { |
|
|
|
|
// Store the position before the reset so that we can record the reset delta
|
|
|
|
|
posResetD = stateStruct.position.z; |
|
|
|
@ -435,7 +435,7 @@ void NavEKF3_core::FuseVelPosNED()
@@ -435,7 +435,7 @@ void NavEKF3_core::FuseVelPosNED()
|
|
|
|
|
// Use different errors if operating without external aiding using an assumed position or velocity of zero
|
|
|
|
|
if (PV_AidingMode == AID_NONE) { |
|
|
|
|
if (tiltAlignComplete && motorsArmed) { |
|
|
|
|
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
|
|
|
|
|
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
|
|
|
|
|
R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f)); |
|
|
|
|
} else { |
|
|
|
|
// Use a smaller value to give faster initial alignment
|
|
|
|
|