mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
AP_NavEKF3: make external navigation optional
This commit is contained in:
parent
8da511f039
commit
1ccda938cb
@ -373,6 +373,7 @@ void NavEKF3_core::setAidingMode()
|
|||||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
|
||||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
|
||||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if (readyToUseExtNav()) {
|
} else if (readyToUseExtNav()) {
|
||||||
// we are commencing aiding using external nav
|
// we are commencing aiding using external nav
|
||||||
posResetSource = resetDataSource::EXTNAV;
|
posResetSource = resetDataSource::EXTNAV;
|
||||||
@ -386,6 +387,7 @@ void NavEKF3_core::setAidingMode()
|
|||||||
hgtMea = -extNavDataDelayed.pos.z;
|
hgtMea = -extNavDataDelayed.pos.z;
|
||||||
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
|
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
|
||||||
ResetHeight();
|
ResetHeight();
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
}
|
}
|
||||||
|
|
||||||
// clear timeout flags as a precaution to avoid triggering any additional transitions
|
// clear timeout flags as a precaution to avoid triggering any additional transitions
|
||||||
@ -505,11 +507,15 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const
|
|||||||
// return true if the filter is ready to use external nav data
|
// return true if the filter is ready to use external nav data
|
||||||
bool NavEKF3_core::readyToUseExtNav(void) const
|
bool NavEKF3_core::readyToUseExtNav(void) const
|
||||||
{
|
{
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) {
|
if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
return tiltAlignComplete && extNavDataToFuse;
|
return tiltAlignComplete && extNavDataToFuse;
|
||||||
|
#else
|
||||||
|
return false;
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
}
|
}
|
||||||
|
|
||||||
// return true if we should use the compass
|
// return true if we should use the compass
|
||||||
@ -531,6 +537,7 @@ bool NavEKF3_core::use_compass(void) const
|
|||||||
// are we using a yaw source other than the magnetomer?
|
// are we using a yaw source other than the magnetomer?
|
||||||
bool NavEKF3_core::using_external_yaw(void) const
|
bool NavEKF3_core::using_external_yaw(void) const
|
||||||
{
|
{
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
|
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
|
||||||
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
|
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
|
||||||
yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
|
yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
|
||||||
@ -539,6 +546,7 @@ bool NavEKF3_core::using_external_yaw(void) const
|
|||||||
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
|
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
|
||||||
return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
|
return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -326,6 +326,7 @@ void NavEKF3_core::SelectMagFusion()
|
|||||||
// fall through to magnetometer fusion
|
// fall through to magnetometer fusion
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// Handle case where we are using an external nav for yaw
|
// Handle case where we are using an external nav for yaw
|
||||||
const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms);
|
const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms);
|
||||||
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
|
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
|
||||||
@ -353,6 +354,7 @@ void NavEKF3_core::SelectMagFusion()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
|
|
||||||
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
|
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
|
||||||
if (magHealth) {
|
if (magHealth) {
|
||||||
@ -905,9 +907,11 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||||||
R_YAW = sq(frontend->_yawNoise);
|
R_YAW = sq(frontend->_yawNoise);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
case yawFusionMethod::EXTNAV:
|
case yawFusionMethod::EXTNAV:
|
||||||
R_YAW = sq(MAX(extNavYawAngDataDelayed.yawAngErr, 0.05f));
|
R_YAW = sq(MAX(extNavYawAngDataDelayed.yawAngErr, 0.05f));
|
||||||
break;
|
break;
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// determine if a 321 or 312 Euler sequence is best
|
// determine if a 321 or 312 Euler sequence is best
|
||||||
@ -929,9 +933,11 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||||||
order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
|
order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
case yawFusionMethod::EXTNAV:
|
case yawFusionMethod::EXTNAV:
|
||||||
order = extNavYawAngDataDelayed.order;
|
order = extNavYawAngDataDelayed.order;
|
||||||
break;
|
break;
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
|
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
|
||||||
@ -1091,9 +1097,11 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||||||
innovYaw = 0.0f;
|
innovYaw = 0.0f;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
case yawFusionMethod::EXTNAV:
|
case yawFusionMethod::EXTNAV:
|
||||||
innovYaw = wrap_PI(yawAngPredicted - extNavYawAngDataDelayed.yawAng);
|
innovYaw = wrap_PI(yawAngPredicted - extNavYawAngDataDelayed.yawAng);
|
||||||
break;
|
break;
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
|
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
|
||||||
|
@ -990,6 +990,7 @@ void NavEKF3_core::writeDefaultAirSpeed(float airspeed)
|
|||||||
|
|
||||||
void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
|
void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
|
||||||
{
|
{
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// protect against NaN
|
// protect against NaN
|
||||||
if (pos.is_nan() || isnan(posErr)) {
|
if (pos.is_nan() || isnan(posErr)) {
|
||||||
return;
|
return;
|
||||||
@ -1038,10 +1039,12 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
|
|||||||
extNavYawAngDataNew.time_ms = timeStamp_ms;
|
extNavYawAngDataNew.time_ms = timeStamp_ms;
|
||||||
storedExtNavYawAng.push(extNavYawAngDataNew);
|
storedExtNavYawAng.push(extNavYawAngDataNew);
|
||||||
}
|
}
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
}
|
}
|
||||||
|
|
||||||
void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
|
void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
|
||||||
{
|
{
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// sanity check for NaNs
|
// sanity check for NaNs
|
||||||
if (vel.is_nan() || isnan(err)) {
|
if (vel.is_nan() || isnan(err)) {
|
||||||
return;
|
return;
|
||||||
@ -1067,6 +1070,7 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
|
|||||||
extNavVelNew.corrected = false;
|
extNavVelNew.corrected = false;
|
||||||
|
|
||||||
storedExtNavVel.push(extNavVelNew);
|
storedExtNavVel.push(extNavVelNew);
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -474,6 +474,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
|
|||||||
innovations = gpsVelInnov;
|
innovations = gpsVelInnov;
|
||||||
variances = gpsVelVarInnov;
|
variances = gpsVelVarInnov;
|
||||||
return true;
|
return true;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
case AP_NavEKF_Source::SourceXY::EXTNAV:
|
case AP_NavEKF_Source::SourceXY::EXTNAV:
|
||||||
// check for timeouts
|
// check for timeouts
|
||||||
if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
|
if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
|
||||||
@ -482,6 +483,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
|
|||||||
innovations = extNavVelInnov;
|
innovations = extNavVelInnov;
|
||||||
variances = extNavVelVarInnov;
|
variances = extNavVelVarInnov;
|
||||||
return true;
|
return true;
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
default:
|
default:
|
||||||
// variances are not available for this source
|
// variances are not available for this source
|
||||||
return false;
|
return false;
|
||||||
|
@ -35,12 +35,14 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
|
|||||||
stateStruct.velocity.y = gps_corrected.vel.y;
|
stateStruct.velocity.y = gps_corrected.vel.y;
|
||||||
// set the variances using the reported GPS speed accuracy
|
// set the variances using the reported GPS speed accuracy
|
||||||
P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
|
P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) {
|
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) {
|
||||||
// use external nav data as the 2nd preference
|
// use external nav data as the 2nd preference
|
||||||
// already corrected for sensor position
|
// already corrected for sensor position
|
||||||
stateStruct.velocity.x = extNavVelDelayed.vel.x;
|
stateStruct.velocity.x = extNavVelDelayed.vel.x;
|
||||||
stateStruct.velocity.y = extNavVelDelayed.vel.y;
|
stateStruct.velocity.y = extNavVelDelayed.vel.y;
|
||||||
P[5][5] = P[4][4] = sq(extNavVelDelayed.err);
|
P[5][5] = P[4][4] = sq(extNavVelDelayed.err);
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else {
|
} else {
|
||||||
stateStruct.velocity.x = 0.0f;
|
stateStruct.velocity.x = 0.0f;
|
||||||
stateStruct.velocity.y = 0.0f;
|
stateStruct.velocity.y = 0.0f;
|
||||||
@ -111,6 +113,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
|
|||||||
// clear the timeout flags and counters
|
// clear the timeout flags and counters
|
||||||
rngBcnTimeout = false;
|
rngBcnTimeout = false;
|
||||||
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) {
|
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) {
|
||||||
// use external nav data as the third preference
|
// use external nav data as the third preference
|
||||||
stateStruct.position.x = extNavDataDelayed.pos.x;
|
stateStruct.position.x = extNavDataDelayed.pos.x;
|
||||||
@ -120,6 +123,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
|
|||||||
// clear the timeout flags and counters
|
// clear the timeout flags and counters
|
||||||
posTimeout = false;
|
posTimeout = false;
|
||||||
lastPosPassTime_ms = imuSampleTime_ms;
|
lastPosPassTime_ms = imuSampleTime_ms;
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
||||||
@ -243,8 +247,10 @@ void NavEKF3_core::ResetHeight(void)
|
|||||||
if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
|
if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
|
||||||
gpsDataNew.have_vz) {
|
gpsDataNew.have_vz) {
|
||||||
stateStruct.velocity.z = gpsDataNew.vel.z;
|
stateStruct.velocity.z = gpsDataNew.vel.z;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
|
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
|
||||||
stateStruct.velocity.z = extNavVelDelayed.vel.z;
|
stateStruct.velocity.z = extNavVelDelayed.vel.z;
|
||||||
|
#endif
|
||||||
} else if (onGround) {
|
} else if (onGround) {
|
||||||
stateStruct.velocity.z = 0.0f;
|
stateStruct.velocity.z = 0.0f;
|
||||||
}
|
}
|
||||||
@ -260,9 +266,12 @@ void NavEKF3_core::ResetHeight(void)
|
|||||||
zeroCols(P,6,6);
|
zeroCols(P,6,6);
|
||||||
|
|
||||||
// set the variances to the measurement variance
|
// set the variances to the measurement variance
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
if (useExtNavVel) {
|
if (useExtNavVel) {
|
||||||
P[6][6] = sq(extNavVelDelayed.err);
|
P[6][6] = sq(extNavVelDelayed.err);
|
||||||
} else {
|
} else
|
||||||
|
#endif
|
||||||
|
{
|
||||||
P[6][6] = sq(frontend->_gpsVertVelNoise);
|
P[6][6] = sq(frontend->_gpsVertVelNoise);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -414,6 +423,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|||||||
posVelFusionDelayed = false;
|
posVelFusionDelayed = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// Check for data at the fusion time horizon
|
// Check for data at the fusion time horizon
|
||||||
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
|
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
|
||||||
if (extNavDataToFuse) {
|
if (extNavDataToFuse) {
|
||||||
@ -429,6 +439,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|||||||
// record time innovations were calculated (for timeout checks)
|
// record time innovations were calculated (for timeout checks)
|
||||||
extNavVelInnovTime_ms = AP_HAL::millis();
|
extNavVelInnovTime_ms = AP_HAL::millis();
|
||||||
}
|
}
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
|
|
||||||
// Read GPS data from the sensor
|
// Read GPS data from the sensor
|
||||||
readGpsData();
|
readGpsData();
|
||||||
@ -460,8 +471,9 @@ void NavEKF3_core::SelectVelPosFusion()
|
|||||||
// Don't fuse velocity data if GPS doesn't support it
|
// Don't fuse velocity data if GPS doesn't support it
|
||||||
fuseVelData = frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS);
|
fuseVelData = frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS);
|
||||||
fusePosData = true;
|
fusePosData = true;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
extNavUsedForPos = false;
|
extNavUsedForPos = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
// copy corrected GPS data to observation vector
|
// copy corrected GPS data to observation vector
|
||||||
if (fuseVelData) {
|
if (fuseVelData) {
|
||||||
@ -471,14 +483,17 @@ void NavEKF3_core::SelectVelPosFusion()
|
|||||||
}
|
}
|
||||||
velPosObs[3] = gpsDataDelayed.pos.x;
|
velPosObs[3] = gpsDataDelayed.pos.x;
|
||||||
velPosObs[4] = gpsDataDelayed.pos.y;
|
velPosObs[4] = gpsDataDelayed.pos.y;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) {
|
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) {
|
||||||
// use external nav system for horizontal position
|
// use external nav system for horizontal position
|
||||||
extNavUsedForPos = true;
|
extNavUsedForPos = true;
|
||||||
fusePosData = true;
|
fusePosData = true;
|
||||||
velPosObs[3] = extNavDataDelayed.pos.x;
|
velPosObs[3] = extNavDataDelayed.pos.x;
|
||||||
velPosObs[4] = extNavDataDelayed.pos.y;
|
velPosObs[4] = extNavDataDelayed.pos.y;
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// fuse external navigation velocity data if available
|
// fuse external navigation velocity data if available
|
||||||
// extNavVelDelayed is already corrected for sensor position
|
// extNavVelDelayed is already corrected for sensor position
|
||||||
if (extNavVelToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) {
|
if (extNavVelToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) {
|
||||||
@ -487,6 +502,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|||||||
velPosObs[1] = extNavVelDelayed.vel.y;
|
velPosObs[1] = extNavVelDelayed.vel.y;
|
||||||
velPosObs[2] = extNavVelDelayed.vel.z;
|
velPosObs[2] = extNavVelDelayed.vel.z;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// we have GPS data to fuse and a request to align the yaw using the GPS course
|
// we have GPS data to fuse and a request to align the yaw using the GPS course
|
||||||
if (gpsYawResetRequest) {
|
if (gpsYawResetRequest) {
|
||||||
@ -513,6 +529,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// check for external nav position reset
|
// check for external nav position reset
|
||||||
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || posxy_source_reset)) {
|
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || posxy_source_reset)) {
|
||||||
// mark a source reset as consumed
|
// mark a source reset as consumed
|
||||||
@ -522,6 +539,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|||||||
ResetPositionD(-hgtMea);
|
ResetPositionD(-hgtMea);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
|
|
||||||
// If we are operating without any aiding, fuse in constant position of constant
|
// If we are operating without any aiding, fuse in constant position of constant
|
||||||
// velocity measurements to constrain tilt drift. This assumes a non-manoeuvring
|
// velocity measurements to constrain tilt drift. This assumes a non-manoeuvring
|
||||||
@ -620,8 +638,10 @@ void NavEKF3_core::FuseVelPosNED()
|
|||||||
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
|
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
|
||||||
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
|
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
|
||||||
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
|
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if (extNavVelToFuse) {
|
} else if (extNavVelToFuse) {
|
||||||
R_OBS[2] = R_OBS[0] = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f));
|
R_OBS[2] = R_OBS[0] = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f));
|
||||||
|
#endif
|
||||||
} else {
|
} else {
|
||||||
// calculate additional error in GPS velocity caused by manoeuvring
|
// calculate additional error in GPS velocity caused by manoeuvring
|
||||||
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
||||||
@ -631,8 +651,10 @@ void NavEKF3_core::FuseVelPosNED()
|
|||||||
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
|
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
|
||||||
if (gpsPosAccuracy > 0.0f) {
|
if (gpsPosAccuracy > 0.0f) {
|
||||||
R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
|
R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if (extNavUsedForPos) {
|
} else if (extNavUsedForPos) {
|
||||||
R_OBS[3] = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f));
|
R_OBS[3] = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f));
|
||||||
|
#endif
|
||||||
} else {
|
} else {
|
||||||
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
||||||
}
|
}
|
||||||
@ -641,9 +663,12 @@ void NavEKF3_core::FuseVelPosNED()
|
|||||||
// For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS performance
|
// For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS performance
|
||||||
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
|
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
|
||||||
float obs_data_chk;
|
float obs_data_chk;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
if (extNavVelToFuse) {
|
if (extNavVelToFuse) {
|
||||||
obs_data_chk = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
|
obs_data_chk = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
|
||||||
} else {
|
} else
|
||||||
|
#endif
|
||||||
|
{
|
||||||
obs_data_chk = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
obs_data_chk = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
||||||
}
|
}
|
||||||
R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
|
R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
|
||||||
@ -984,7 +1009,9 @@ void NavEKF3_core::selectHeightForFusion()
|
|||||||
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
||||||
|
|
||||||
bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500);
|
bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500);
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500);
|
const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500);
|
||||||
|
#endif
|
||||||
// select height source
|
// select height source
|
||||||
if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) {
|
if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) {
|
||||||
// user has specified the range finder as a primary height source
|
// user has specified the range finder as a primary height source
|
||||||
@ -1033,16 +1060,22 @@ void NavEKF3_core::selectHeightForFusion()
|
|||||||
activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
|
activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
|
||||||
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcnGoodToAlign) {
|
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcnGoodToAlign) {
|
||||||
activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON;
|
activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) {
|
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) {
|
||||||
activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV;
|
activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV;
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
|
// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
|
||||||
bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh);
|
bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh);
|
||||||
bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
|
bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
|
||||||
bool lostExtNavHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) && !extNavDataIsFresh);
|
|
||||||
bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
|
bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
|
||||||
if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) {
|
bool fallback_to_baro = lostRngHgt || lostGpsHgt || lostRngBcnHgt;
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
|
bool lostExtNavHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) && !extNavDataIsFresh);
|
||||||
|
fallback_to_baro |= lostExtNavHgt;
|
||||||
|
#endif
|
||||||
|
if (fallback_to_baro) {
|
||||||
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
|
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1073,12 +1106,15 @@ void NavEKF3_core::selectHeightForFusion()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Select the height measurement source
|
// Select the height measurement source
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
if (extNavDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
|
if (extNavDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
|
||||||
hgtMea = -extNavDataDelayed.pos.z;
|
hgtMea = -extNavDataDelayed.pos.z;
|
||||||
velPosObs[5] = -hgtMea;
|
velPosObs[5] = -hgtMea;
|
||||||
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
|
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
|
||||||
fuseHgtData = true;
|
fuseHgtData = true;
|
||||||
} else if (rangeDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
|
} else
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
|
if (rangeDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
|
||||||
// using range finder data
|
// using range finder data
|
||||||
// correct for tilt using a flat earth model
|
// correct for tilt using a flat earth model
|
||||||
if (prevTnb.c.z >= 0.7) {
|
if (prevTnb.c.z >= 0.7) {
|
||||||
|
@ -136,6 +136,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
|||||||
if(dal.beacon() && !storedRangeBeacon.init(imu_buffer_length+1)) {
|
if(dal.beacon() && !storedRangeBeacon.init(imu_buffer_length+1)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
if (frontend->sources.ext_nav_enabled() && !storedExtNav.init(extnav_buffer_length)) {
|
if (frontend->sources.ext_nav_enabled() && !storedExtNav.init(extnav_buffer_length)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -145,6 +146,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
|||||||
if(frontend->sources.ext_nav_enabled() && !storedExtNavYawAng.init(extnav_buffer_length)) {
|
if(frontend->sources.ext_nav_enabled() && !storedExtNavYawAng.init(extnav_buffer_length)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
if(!storedIMU.init(imu_buffer_length)) {
|
if(!storedIMU.init(imu_buffer_length)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -393,6 +395,7 @@ void NavEKF3_core::InitialiseVariables()
|
|||||||
memset(&yawAngDataNew, 0, sizeof(yawAngDataNew));
|
memset(&yawAngDataNew, 0, sizeof(yawAngDataNew));
|
||||||
memset(&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed));
|
memset(&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed));
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// external nav data fusion
|
// external nav data fusion
|
||||||
extNavDataDelayed = {};
|
extNavDataDelayed = {};
|
||||||
extNavMeasTime_ms = 0;
|
extNavMeasTime_ms = 0;
|
||||||
@ -403,6 +406,7 @@ void NavEKF3_core::InitialiseVariables()
|
|||||||
extNavVelToFuse = false;
|
extNavVelToFuse = false;
|
||||||
useExtNavVel = false;
|
useExtNavVel = false;
|
||||||
extNavVelMeasTime_ms = 0;
|
extNavVelMeasTime_ms = 0;
|
||||||
|
#endif
|
||||||
|
|
||||||
// zero data buffers
|
// zero data buffers
|
||||||
storedIMU.reset();
|
storedIMU.reset();
|
||||||
@ -416,8 +420,10 @@ void NavEKF3_core::InitialiseVariables()
|
|||||||
storedBodyOdm.reset();
|
storedBodyOdm.reset();
|
||||||
storedWheelOdm.reset();
|
storedWheelOdm.reset();
|
||||||
#endif
|
#endif
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
storedExtNav.reset();
|
storedExtNav.reset();
|
||||||
storedExtNavVel.reset();
|
storedExtNavVel.reset();
|
||||||
|
#endif
|
||||||
|
|
||||||
// initialise pre-arm message
|
// initialise pre-arm message
|
||||||
dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising");
|
dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising");
|
||||||
@ -458,7 +464,9 @@ void NavEKF3_core::InitialiseVariablesMag()
|
|||||||
magFieldLearned = false;
|
magFieldLearned = false;
|
||||||
storedMag.reset();
|
storedMag.reset();
|
||||||
storedYawAng.reset();
|
storedYawAng.reset();
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
storedExtNavYawAng.reset();
|
storedExtNavYawAng.reset();
|
||||||
|
#endif
|
||||||
needMagBodyVarReset = false;
|
needMagBodyVarReset = false;
|
||||||
needEarthBodyVarReset = false;
|
needEarthBodyVarReset = false;
|
||||||
}
|
}
|
||||||
|
@ -1316,6 +1316,7 @@ private:
|
|||||||
bool onGroundNotMoving; // true when on the ground and not moving
|
bool onGroundNotMoving; // true when on the ground and not moving
|
||||||
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
|
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
|
||||||
|
|
||||||
|
#if EK3_FEATURE_EXTERNAL_NAV
|
||||||
// external navigation fusion
|
// external navigation fusion
|
||||||
EKF_obs_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer
|
EKF_obs_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer
|
||||||
ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
|
ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
|
||||||
@ -1327,13 +1328,14 @@ private:
|
|||||||
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
|
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
|
||||||
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
|
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
|
||||||
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
|
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
|
||||||
bool useExtNavVel; // true if external nav velocity should be used
|
|
||||||
Vector3f extNavVelInnov; // external nav velocity innovations
|
Vector3f extNavVelInnov; // external nav velocity innovations
|
||||||
Vector3f extNavVelVarInnov; // external nav velocity innovation variances
|
Vector3f extNavVelVarInnov; // external nav velocity innovation variances
|
||||||
uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts)
|
uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts)
|
||||||
EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng; // external navigation yaw angle buffer
|
EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng; // external navigation yaw angle buffer
|
||||||
yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon
|
yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon
|
||||||
uint32_t last_extnav_yaw_fusion_ms; // system time that external nav yaw was last fused
|
uint32_t last_extnav_yaw_fusion_ms; // system time that external nav yaw was last fused
|
||||||
|
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||||
|
bool useExtNavVel; // true if external nav velocity should be used
|
||||||
|
|
||||||
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
|
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
|
||||||
struct {
|
struct {
|
||||||
|
@ -10,4 +10,12 @@
|
|||||||
#define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay)
|
#define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay)
|
||||||
|
|
||||||
// body odomotry (which includes wheel encoding) on rover or 2M boards
|
// body odomotry (which includes wheel encoding) on rover or 2M boards
|
||||||
|
#ifndef EK3_FEATURE_BODY_ODOM
|
||||||
#define EK3_FEATURE_BODY_ODOM EK3_FEATURE_ALL || APM_BUILD_TYPE(APM_BUILD_Rover) || BOARD_FLASH_SIZE > 1024
|
#define EK3_FEATURE_BODY_ODOM EK3_FEATURE_ALL || APM_BUILD_TYPE(APM_BUILD_Rover) || BOARD_FLASH_SIZE > 1024
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// external navigation on 2M boards
|
||||||
|
#ifndef EK3_FEATURE_EXTERNAL_NAV
|
||||||
|
#define EK3_FEATURE_EXTERNAL_NAV EK3_FEATURE_ALL || BOARD_FLASH_SIZE > 1024
|
||||||
|
#endif
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user