From 341d070db8c3ac4f4a5148e6287cfde24c6aeec2 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 10 Jul 2016 23:15:46 +1000 Subject: [PATCH] AP_NavEKF2: Separate filter status update and get functions 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 --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 34 ++++++++++ libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 72 ++++----------------- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 4 ++ libraries/AP_NavEKF2/AP_NavEKF2_core.h | 4 ++ 4 files changed, 56 insertions(+), 58 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 0ea83bc292..d55b6dd30f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 9b49dfc99a..93d913c874 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -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; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 7ae9da8c20..22a2453a85 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index e64070b2ac..23d999b1d2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -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;