|
|
|
@ -239,7 +239,7 @@ void NavEKF2_core::setAidingMode()
@@ -239,7 +239,7 @@ void NavEKF2_core::setAidingMode()
|
|
|
|
|
stateStruct.position.z = -meaHgtAtTakeOff; |
|
|
|
|
} else if (PV_AidingMode == AID_RELATIVE) { |
|
|
|
|
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
|
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u is using optical flow",(unsigned)imu_index); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index); |
|
|
|
|
posTimeout = true; |
|
|
|
|
velTimeout = true; |
|
|
|
|
// Reset the last valid flow measurement time
|
|
|
|
@ -248,7 +248,7 @@ void NavEKF2_core::setAidingMode()
@@ -248,7 +248,7 @@ void NavEKF2_core::setAidingMode()
|
|
|
|
|
prevFlowFuseTime_ms = imuSampleTime_ms; |
|
|
|
|
} else if (PV_AidingMode == AID_ABSOLUTE) { |
|
|
|
|
// We have commenced aiding and GPS usage is allowed
|
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u is using GPS",(unsigned)imu_index); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index); |
|
|
|
|
posTimeout = false; |
|
|
|
|
velTimeout = false; |
|
|
|
|
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
|
|
|
|
@ -277,7 +277,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
@@ -277,7 +277,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
|
|
|
|
|
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; |
|
|
|
|
if (tiltErrFilt < 0.005f && !tiltAlignComplete) { |
|
|
|
|
tiltAlignComplete = true; |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// submit yaw and magnetic field reset requests depending on whether we have compass data
|
|
|
|
@ -353,7 +353,7 @@ void NavEKF2_core::setOrigin()
@@ -353,7 +353,7 @@ void NavEKF2_core::setOrigin()
|
|
|
|
|
// define Earth rotation vector in the NED navigation frame at the origin
|
|
|
|
|
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); |
|
|
|
|
validOrigin = true; |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u Origin Set",(unsigned)imu_index); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u Origin Set",(unsigned)imu_index); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// record a yaw reset event
|
|
|
|
|