aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Separate filter status update and get functions

Browse Source 
The filter status logic calculations were being repeated every time the get function was called.
The logic is now updated once per filter update step and a separate get function added
This commit is contained in:
priseborough 2016-07-10 23:15:46 +10:00 committed by Andrew Tridgell
parent b1717649b1
commit 341d070db8
4 changed files with 56 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -358,4 +358,38 @@ uint8_t NavEKF2_core::setInhibitGPS(void)
}
}
// Update the filter status
void NavEKF2_core::updateFilterStatus(void)
{
// init return value
filterStatus.value = 0;
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3) && delAngBiasLearned;
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
// set individual flags
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
}
#endif // HAL_CPU_CLASS

72
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -214,8 +214,6 @@ void NavEKF2_core::getAccelZBias(float &zbias) const {
bool NavEKF2_core::getPosNE(Vector2f &posNE) const
{
// There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
nav_filter_status status;
getFilterStatus(status);
if (PV_AidingMode != AID_NONE) {
// This is the normal mode of operation where we can use the EKF position states
posNE.x = outputDataNew.position.x;
@ -255,9 +253,7 @@ bool NavEKF2_core::getPosD(float &posD) const
posD = outputDataNew.position.z;
// Return the current height solution status
nav_filter_status status;
getFilterStatus(status);
return status.flags.vert_pos;
return filterStatus.flags.vert_pos;
}
// return the estimated height above ground level
@ -282,9 +278,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
loc.flags.terrain_alt = 0;
// there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding)
nav_filter_status status;
getFilterStatus(status);
if (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) {
if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
loc.lat = EKF_origin.lat;
loc.lng = EKF_origin.lng;
location_offset(loc, outputDataNew.position.x, outputDataNew.position.y);
@ -458,44 +452,10 @@ void NavEKF2_core::getFilterTimeouts(uint8_t &timeouts) const
tasTimeout<<4);
}
/*
Return a filter function status that indicates:
Which outputs are valid
If the filter has detected takeoff
If the filter has activated the mode that mitigates against ground effect static pressure errors
If GPS data is being used
*/
// Return the navigation filter status message
void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
{
// init return value
status.value = 0;
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3) && delAngBiasLearned;
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
// set individual flags
status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
status.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
status.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
status.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || status.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
status.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || status.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
status.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
status.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
status.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
status = filterStatus;
}
/*
@ -521,40 +481,36 @@ void NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const
// send an EKF_STATUS message to GCS
void NavEKF2_core::send_status_report(mavlink_channel_t chan)
{
// get filter status
nav_filter_status filt_state;
getFilterStatus(filt_state);
// prepare flags
uint16_t flags = 0;
if (filt_state.flags.attitude) {
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filt_state.flags.horiz_vel) {
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filt_state.flags.vert_vel) {
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filt_state.flags.horiz_pos_rel) {
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filt_state.flags.horiz_pos_abs) {
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filt_state.flags.vert_pos) {
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filt_state.flags.terrain_alt) {
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filt_state.flags.const_pos_mode) {
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filt_state.flags.pred_horiz_pos_rel) {
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filt_state.flags.pred_horiz_pos_abs) {
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -263,6 +263,7 @@ void NavEKF2_core::InitialiseVariables()
quatAtLastMagReset = stateStruct.quat;
magFieldLearned = false;
delAngBiasLearned = false;
memset(&filterStatus, 0, sizeof(filterStatus));
// zero data buffers
storedIMU.reset();
@ -460,6 +461,9 @@ void NavEKF2_core::UpdateFilter(bool predict)
// Update states using sideslip constraint assumption for fly-forward vehicles
SelectBetaFusion();
// Update the filter status
updateFilterStatus();
}
// Wind output forward from the fusion to output time horizon

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -389,6 +389,9 @@ private:
uint32_t time_ms; // 4
};
// update the navigation filter status
void updateFilterStatus(void);
// update the quaternion, velocity and position states using IMU measurements
void UpdateStrapdownEquationsNED();
@ -807,6 +810,7 @@ private:
Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
bool delAngBiasLearned; // true when the gyro bias has been learned
nav_filter_status filterStatus; // contains the status of various filter outputs
Vector3f outputTrackError;