|
|
@ -203,7 +203,8 @@ void NavEKF3_core::setAidingMode() |
|
|
|
checkGyroCalStatus(); |
|
|
|
checkGyroCalStatus(); |
|
|
|
|
|
|
|
|
|
|
|
// Determine if we should change aiding mode
|
|
|
|
// Determine if we should change aiding mode
|
|
|
|
if (PV_AidingMode == AID_NONE) { |
|
|
|
switch (PV_AidingMode) { |
|
|
|
|
|
|
|
case AID_NONE: { |
|
|
|
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
|
|
|
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
|
|
|
// and IMU gyro bias estimates have stabilised
|
|
|
|
// and IMU gyro bias estimates have stabilised
|
|
|
|
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
|
|
|
|
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
|
|
|
@ -213,7 +214,9 @@ void NavEKF3_core::setAidingMode() |
|
|
|
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) { |
|
|
|
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) { |
|
|
|
PV_AidingMode = AID_RELATIVE; |
|
|
|
PV_AidingMode = AID_RELATIVE; |
|
|
|
} |
|
|
|
} |
|
|
|
} else if (PV_AidingMode == AID_RELATIVE) { |
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
case AID_RELATIVE: { |
|
|
|
// Check if the fusion has timed out (flow measurements have been rejected for too long)
|
|
|
|
// Check if the fusion has timed out (flow measurements have been rejected for too long)
|
|
|
|
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); |
|
|
|
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); |
|
|
|
// Check if the fusion has timed out (body odometry measurements have been rejected for too long)
|
|
|
|
// Check if the fusion has timed out (body odometry measurements have been rejected for too long)
|
|
|
@ -225,7 +228,9 @@ void NavEKF3_core::setAidingMode() |
|
|
|
} else if (flowFusionTimeout && bodyOdmFusionTimeout) { |
|
|
|
} else if (flowFusionTimeout && bodyOdmFusionTimeout) { |
|
|
|
PV_AidingMode = AID_NONE; |
|
|
|
PV_AidingMode = AID_NONE; |
|
|
|
} |
|
|
|
} |
|
|
|
} else if (PV_AidingMode == AID_ABSOLUTE) { |
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
case AID_ABSOLUTE: { |
|
|
|
// Find the minimum time without data required to trigger any check
|
|
|
|
// Find the minimum time without data required to trigger any check
|
|
|
|
uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms)); |
|
|
|
uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms)); |
|
|
|
|
|
|
|
|
|
|
@ -292,7 +297,8 @@ void NavEKF3_core::setAidingMode() |
|
|
|
rngBcnTimeout = true; |
|
|
|
rngBcnTimeout = true; |
|
|
|
gpsNotAvailable = true; |
|
|
|
gpsNotAvailable = true; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// check to see if we are starting or stopping aiding and set states and modes as required
|
|
|
|
// check to see if we are starting or stopping aiding and set states and modes as required
|
|
|
|