From 487bbc3ed21a6ad309afc7f70229d106236ab2fc Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 9 Jul 2016 12:27:49 +1000 Subject: [PATCH] AP_NavEKF: Fix reporting of position status Split publishing of local position into horiz and vert components with separate validity checks Split status reporting into separate update and get functions and only update once after each loop update. This removes unnecessary re-calculation of the status logic and ensures all consumers get the same data (avoid possible race conditions). --- libraries/AP_NavEKF/AP_NavEKF.cpp | 17 +++- libraries/AP_NavEKF/AP_NavEKF.h | 9 +- libraries/AP_NavEKF/AP_NavEKF_core.cpp | 122 ++++++++++++------------- libraries/AP_NavEKF/AP_NavEKF_core.h | 17 +++- 4 files changed, 92 insertions(+), 73 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 8f73ae3480..51e49f697f 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -470,15 +470,26 @@ bool NavEKF::healthy(void) const return core->healthy(); } -// Return the last calculated NED position relative to the reference point (m). +// Return the last calculated North East position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF::getPosNED(Vector3f &pos) const +bool NavEKF::getPosNE(Vector2f &posNE) const { if (!core) { return false; } - return core->getPosNED(pos); + return core->getPosNE(posNE); +} + +// Return the last calculated Down position relative to the reference point (m). +// If a calculated solution is not available, use the best available data and return false +// If false returned, do not use for flight control +bool NavEKF::getPosD(float &posD) const +{ + if (!core) { + return false; + } + return core->getPosD(posD); } // return NED velocity in m/s diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 543746003e..21f82a448f 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -71,10 +71,15 @@ public: // Check basic filter health metrics and return a consolidated health status bool healthy(void) const; - // Return the last calculated NED position relative to the reference point (m). + // Return the last calculated North East position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNED(Vector3f &pos) const; + bool getPosNE(Vector2f &posNE) const; + + // Return the last calculated Down position relative to the reference point (m). + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosD(float &posD) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.cpp b/libraries/AP_NavEKF/AP_NavEKF_core.cpp index 36e6754371..d9e1636541 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_core.cpp @@ -445,6 +445,9 @@ void NavEKF_core::UpdateFilter() SelectTasFusion(); SelectBetaFusion(); + // Update the filter status + updateFilterStatus(); + end: // stop the timer used for load measurement hal.util->perf_end(_perf_UpdateFilter); @@ -3355,19 +3358,15 @@ float NavEKF_core::getPosDownDerivative(void) const return posDownDerivative; } -// Return the last calculated NED position relative to the reference point (m). +// Return the last calculated NE position relative to the reference point (m). // if a calculated solution is not available, use the best available data and return false -bool NavEKF_core::getPosNED(Vector3f &pos) const +bool NavEKF_core::getPosNE(Vector2f &posNE) const { - // The EKF always has a height estimate regardless of mode of operation - pos.z = state.position.z; // 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 (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) { + if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) { // This is the normal mode of operation where we can use the EKF position states - pos.x = state.position.x; - pos.y = state.position.y; + posNE.x = state.position.x; + posNE.y = state.position.y; return true; } else { // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate @@ -3376,25 +3375,34 @@ bool NavEKF_core::getPosNED(Vector3f &pos) const // If the origin has been set and we have GPS, then return the GPS position relative to the origin const struct Location &gpsloc = _ahrs->get_gps().location(); Vector2f tempPosNE = location_diff(EKF_origin, gpsloc); - pos.x = tempPosNE.x; - pos.y = tempPosNE.y; + posNE.x = tempPosNE.x; + posNE.y = tempPosNE.y; return false; } else { // If no GPS fix is available, all we can do is provide the last known position - pos.x = state.position.x + lastKnownPositionNE.x; - pos.y = state.position.y + lastKnownPositionNE.y; + posNE.x = state.position.x + lastKnownPositionNE.x; + posNE.y = state.position.y + lastKnownPositionNE.y; return false; } } else { // If the origin has not been set, then we have no means of providing a relative position - pos.x = 0.0f; - pos.y = 0.0f; + posNE.x = 0.0f; + posNE.y = 0.0f; return false; } } return false; } +// Return the last calculated D position relative to the reference point (m). +// if a calculated solution is not available, use the best available data and return false +bool NavEKF_core::getPosD(float &posD) const +{ + // The EKF always has a height estimate regardless of mode of operation + posD = state.position.z; + return filterStatus.flags.vert_pos; +} + // return body axis gyro bias estimates in rad/sec void NavEKF_core::getGyroBias(Vector3f &gyroBias) const { @@ -3543,9 +3551,7 @@ bool NavEKF_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, state.position.x, state.position.y); @@ -4515,6 +4521,7 @@ void NavEKF_core::InitialiseVariables() posResetNE.zero(); velResetNE.zero(); hgtInnovFiltState = 0.0f; + memset(&filterStatus, 0, sizeof(filterStatus)); } // return true if we should use the airspeed sensor @@ -4637,22 +4644,9 @@ void NavEKF_core::getFilterGpsStatus(nav_gps_status &faults) const faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static) } -/* -return filter function status as a bitmasked integer - 0 = attitude estimate valid - 1 = horizontal velocity estimate valid - 2 = vertical velocity estimate valid - 3 = relative horizontal position estimate valid - 4 = absolute horizontal position estimate valid - 5 = vertical position estimate valid - 6 = terrain height estimate valid - 7 = constant position mode -*/ -void NavEKF_core::getFilterStatus(nav_filter_status &status) const +// Update the naigation filter status message +void NavEKF_core::updateFilterStatus(void) { - // 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); @@ -4665,42 +4659,44 @@ void NavEKF_core::getFilterStatus(nav_filter_status &status) const bool gyroHealthy = checkGyroHealthPreFlight(); // set individual flags - status.flags.attitude = !state.quat.is_nan() && filterHealthy && gyroHealthy; // attitude valid (we need a better check) - status.flags.horiz_vel = someHorizRefData && notDeadReckoning && filterHealthy; // horizontal velocity estimate valid - status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid - status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid - status.flags.horiz_pos_abs = !gpsAidingBad && doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid - status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid - status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid - status.flags.const_pos_mode = constPosMode && filterHealthy; // constant position mode - status.flags.pred_horiz_pos_rel = (optFlowNavPossible || gpsNavPossible) && filterHealthy && gyroHealthy; // we should be able to estimate a relative position when we enter flight mode - status.flags.pred_horiz_pos_abs = gpsNavPossible && filterHealthy && gyroHealthy; // 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) < 4000; - status.flags.gps_glitching = !gpsAccuracyGood; // The GPS is glitching + filterStatus.flags.attitude = !state.quat.is_nan() && filterHealthy && gyroHealthy; // attitude valid (we need a better check) + filterStatus.flags.horiz_vel = someHorizRefData && notDeadReckoning && filterHealthy; // horizontal velocity estimate valid + filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid + filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid + filterStatus.flags.horiz_pos_abs = !gpsAidingBad && doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid + filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid + filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid + filterStatus.flags.const_pos_mode = constPosMode && filterHealthy; // constant position mode + filterStatus.flags.pred_horiz_pos_rel = (optFlowNavPossible || gpsNavPossible) && filterHealthy && gyroHealthy; // we should be able to estimate a relative position when we enter flight mode + filterStatus.flags.pred_horiz_pos_abs = gpsNavPossible && filterHealthy && gyroHealthy; // 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) < 4000; + filterStatus.flags.gps_glitching = !gpsAccuracyGood; // The GPS is glitching } -// send an EKF_STATUS message to GCS -void NavEKF_core::send_status_report(mavlink_channel_t chan) +// Return the navigation filter status message +void NavEKF_core::getFilterStatus(nav_filter_status &status) const { - // get filter status - nav_filter_status filt_state; - getFilterStatus(filt_state); + status = filterStatus; +} + // send an EKF_STATUS message to GCS +void NavEKF_core::send_status_report(mavlink_channel_t chan) +{ // prepare flags uint16_t flags = 0; - if (filt_state.flags.attitude) { flags |= EKF_ATTITUDE; } - if (filt_state.flags.horiz_vel) { flags |= EKF_VELOCITY_HORIZ; } - if (filt_state.flags.vert_vel) { flags |= EKF_VELOCITY_VERT; } - if (filt_state.flags.horiz_pos_rel) { flags |= EKF_POS_HORIZ_REL; } - if (filt_state.flags.horiz_pos_abs) { flags |= EKF_POS_HORIZ_ABS; } - if (filt_state.flags.vert_pos) { flags |= EKF_POS_VERT_ABS; } - if (filt_state.flags.terrain_alt) { flags |= EKF_POS_VERT_AGL; } - if (filt_state.flags.const_pos_mode) { flags |= EKF_CONST_POS_MODE; } - if (filt_state.flags.pred_horiz_pos_rel) { flags |= EKF_PRED_POS_HORIZ_REL; } - if (filt_state.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } + if (filterStatus.flags.attitude) { flags |= EKF_ATTITUDE; } + if (filterStatus.flags.horiz_vel) { flags |= EKF_VELOCITY_HORIZ; } + if (filterStatus.flags.vert_vel) { flags |= EKF_VELOCITY_VERT; } + if (filterStatus.flags.horiz_pos_rel) { flags |= EKF_POS_HORIZ_REL; } + if (filterStatus.flags.horiz_pos_abs) { flags |= EKF_POS_HORIZ_ABS; } + if (filterStatus.flags.vert_pos) { flags |= EKF_POS_VERT_ABS; } + if (filterStatus.flags.terrain_alt) { flags |= EKF_POS_VERT_AGL; } + if (filterStatus.flags.const_pos_mode) { flags |= EKF_CONST_POS_MODE; } + if (filterStatus.flags.pred_horiz_pos_rel) { flags |= EKF_PRED_POS_HORIZ_REL; } + if (filterStatus.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } // get variances float velVar, posVar, hgtVar, tasVar; diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.h b/libraries/AP_NavEKF/AP_NavEKF_core.h index bb3a716f73..83a3bcd712 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core.h +++ b/libraries/AP_NavEKF/AP_NavEKF_core.h @@ -114,10 +114,15 @@ public: // Check basic filter health metrics and return a consolidated health status bool healthy(void) const; - // Return the last calculated NED position relative to the reference point (m). + // Return the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNED(Vector3f &pos) const; + bool getPosNE(Vector2f &posNE) const; + + // Return the last calculated D position relative to the reference point (m). + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosD(float &posD) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; @@ -259,9 +264,7 @@ public: */ void getFilterTimeouts(uint8_t &timeouts) const; - /* - return filter status flags - */ + // return filter status flags void getFilterStatus(nav_filter_status &status) const; /* @@ -324,6 +327,9 @@ private: Vector3f omega; // 31 .. 33 } &state; + // update the navigation filter status + void updateFilterStatus(void); + // update the quaternion, velocity and position states using IMU measurements void UpdateStrapdownEquationsNED(); @@ -697,6 +703,7 @@ private: Vector3f delAngBiasAtArming; // value of the gyro delta angle bias at arming float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks Quaternion prevQuatMagReset; // Quaternion from the previous frame that the magnetic field state reset condition test was performed + nav_filter_status filterStatus; // contains the status of various filter outputs // Used by smoothing of state corrections Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement