From 7ad9e3548f13929b2ed71782ae98e49047e83b42 Mon Sep 17 00:00:00 2001 From: chobits Date: Fri, 17 Jul 2020 16:47:43 +0800 Subject: [PATCH] AP_NavEKF3: make reset source variable local --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 20 ++++----- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 10 ++--- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 42 ++++++++----------- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 6 +-- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 32 +++++++------- 5 files changed, 48 insertions(+), 62 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 00fb3677c0..af0d14e853 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -195,6 +195,9 @@ void NavEKF3_core::updateStateIndexLim() // Set inertial navigation aiding mode void NavEKF3_core::setAidingMode() { + resetDataSource posResetSource = resetDataSource::DEFAULT; + resetDataSource velResetSource = resetDataSource::DEFAULT; + // Save the previous status so we can detect when it has changed PV_AidingModePrev = PV_AidingMode; @@ -355,26 +358,23 @@ void NavEKF3_core::setAidingMode() case AID_ABSOLUTE: if (readyToUseGPS()) { // We are commencing aiding using GPS - this is the preferred method - posResetSource = GPS; - velResetSource = GPS; + posResetSource = resetDataSource::GPS; + velResetSource = resetDataSource::GPS; gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index); } else if (readyToUseRangeBeacon()) { // We are commencing aiding using range beacons - posResetSource = RNGBCN; - velResetSource = DEFAULT; + posResetSource = resetDataSource::RNGBCN; 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 beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z); } else if (readyToUseExtNav()) { // we are commencing aiding using external nav - posResetSource = EXTNAV; + posResetSource = resetDataSource::EXTNAV; gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using external nav data",(unsigned)imu_index); gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z); if (useExtNavVel) { - velResetSource = EXTNAV; + velResetSource = resetDataSource::EXTNAV; gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial vel NED = %3.1f,%3.1f,%3.1f (m/s)",(unsigned)imu_index,(double)extNavVelDelayed.vel.x,(double)extNavVelDelayed.vel.y,(double)extNavVelDelayed.vel.z); - } else { - velResetSource = DEFAULT; } // handle height reset as special case hgtMea = -extNavDataDelayed.pos.z; @@ -394,8 +394,8 @@ void NavEKF3_core::setAidingMode() } // Always reset the position and velocity when changing mode - ResetVelocity(); - ResetPosition(); + ResetVelocity(velResetSource); + ResetPosition(posResetSource); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 4a1b9124e3..f1d3963083 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -158,10 +158,8 @@ void NavEKF3_core::realignYawGPS() resetQuatStateYawOnly(gpsYaw, sq(radians(45.0f))); // reset the velocity and position states as they will be inaccurate due to bad yaw - velResetSource = GPS; - ResetVelocity(); - posResetSource = GPS; - ResetPosition(); + ResetVelocity(resetDataSource::GPS); + ResetPosition(resetDataSource::GPS); // send yaw alignment information to console gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index); @@ -1478,8 +1476,8 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw() recordYawReset(); // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly - ResetVelocity(); - ResetPosition(); + ResetVelocity(resetDataSource::DEFAULT); + ResetPosition(resetDataSource::DEFAULT); // reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset velTestRatio = 0.0f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 5f08b33c77..0fba1da29a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -16,7 +16,7 @@ extern const AP_HAL::HAL& hal; // Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift -void NavEKF3_core::ResetVelocity(void) +void NavEKF3_core::ResetVelocity(resetDataSource velResetSource) { // Store the velocity before the reset so that we can record the reset delta velResetNE.x = stateStruct.velocity.x; @@ -32,7 +32,7 @@ void NavEKF3_core::ResetVelocity(void) P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise); } else { // reset horizontal velocity states to the GPS velocity if available - if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) { + if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::GPS) { // correct for antenna position gps_elements gps_corrected = gpsDataNew; CorrectGPSForAntennaOffset(gps_corrected); @@ -43,7 +43,7 @@ void NavEKF3_core::ResetVelocity(void) // clear the timeout flags and counters velTimeout = false; lastVelPassTime_ms = imuSampleTime_ms; - } else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == DEFAULT) || velResetSource == EXTNAV) { + } else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) { // use external nav data as the 2nd preference // already corrected for sensor position stateStruct.velocity.x = extNavVelDelayed.vel.x; @@ -76,14 +76,10 @@ void NavEKF3_core::ResetVelocity(void) // store the time of the reset lastVelReset_ms = imuSampleTime_ms; - - // clear reset data source preference - velResetSource = DEFAULT; - } // resets position states to last GPS measurement or to zero if in constant position mode -void NavEKF3_core::ResetPosition(void) +void NavEKF3_core::ResetPosition(resetDataSource posResetSource) { // Store the position before the reset so that we can record the reset delta posResetNE.x = stateStruct.position.x; @@ -101,7 +97,7 @@ void NavEKF3_core::ResetPosition(void) P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise); } else { // Use GPS data as first preference if fresh data is available - if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) { + if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::GPS) { // correct for antenna position gps_elements gps_corrected = gpsDataNew; CorrectGPSForAntennaOffset(gps_corrected); @@ -115,7 +111,7 @@ void NavEKF3_core::ResetPosition(void) // clear the timeout flags and counters posTimeout = false; lastPosPassTime_ms = imuSampleTime_ms; - } else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == DEFAULT) || posResetSource == RNGBCN) { + } else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) { // use the range beacon data as a second preference stateStruct.position.x = receiverPos.x; stateStruct.position.y = receiverPos.y; @@ -125,7 +121,7 @@ void NavEKF3_core::ResetPosition(void) // clear the timeout flags and counters rngBcnTimeout = false; lastRngBcnPassTime_ms = imuSampleTime_ms; - } else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == DEFAULT) || posResetSource == EXTNAV) { + } else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) { // use external nav data as the third preference ext_nav_elements extNavCorrected = extNavDataDelayed; CorrectExtNavForSensorOffset(extNavCorrected.pos); @@ -153,10 +149,6 @@ void NavEKF3_core::ResetPosition(void) // store the time of the reset lastPosReset_ms = imuSampleTime_ms; - - // clear reset source preference - posResetSource = DEFAULT; - } // reset the stateStruct's NE position to the specified position @@ -700,13 +692,13 @@ void NavEKF3_core::FuseVelPosNED() } else if (posHealth || posTimeout) { posHealth = true; lastPosPassTime_ms = imuSampleTime_ms; - // if timed out or outside the specified uncertainty radius, reset to the GPS + // if timed out or outside the specified uncertainty radius, reset to the external sensor if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) { - // reset the position to the current GPS position - ResetPosition(); - // reset the velocity to the GPS velocity - ResetVelocity(); - // don't fuse GPS data on this time step + // reset the position to the current external sensor position + ResetPosition(resetDataSource::DEFAULT); + // reset the velocity to the external sensor velocity + ResetVelocity(resetDataSource::DEFAULT); + // don't fuse external sensor data on this time step fusePosData = false; fuseVelData = false; // Reset the position variances and corresponding covariances to a value that will pass the checks @@ -754,11 +746,11 @@ void NavEKF3_core::FuseVelPosNED() velHealth = true; // restart the timeout count lastVelPassTime_ms = imuSampleTime_ms; - // If we are doing full aiding and velocity fusion times out, reset to the GPS velocity + // If we are doing full aiding and velocity fusion times out, reset to the external sensor velocity if (PV_AidingMode == AID_ABSOLUTE && velTimeout) { - // reset the velocity to the GPS velocity - ResetVelocity(); - // don't fuse GPS velocity data on this time step + // reset the velocity to the external sensor velocity + ResetVelocity(resetDataSource::DEFAULT); + // don't fuse external sensor velocity data on this time step fuseVelData = false; // Reset the normalised innovation to avoid failing the bad fusion tests velTestRatio = 0.0f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 187c8176d4..286b77ac18 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -351,8 +351,6 @@ void NavEKF3_core::InitialiseVariables() velOffsetNED.zero(); posOffsetNED.zero(); memset(&velPosObs, 0, sizeof(velPosObs)); - posResetSource = DEFAULT; - velResetSource = DEFAULT; // range beacon fusion variables memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew)); @@ -555,8 +553,8 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) stateStruct.body_magfield.zero(); // set the position, velocity and height - ResetVelocity(); - ResetPosition(); + ResetVelocity(resetDataSource::DEFAULT); + ResetPosition(resetDataSource::DEFAULT); ResetHeight(); // define Earth rotation vector in the NED navigation frame diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 916ec8c168..82911f1752 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -629,6 +629,19 @@ private: Vector3f accel_bias; } inactiveBias[INS_MAX_INSTANCES]; + // Specify source of data to be used for a partial state reset + // Checking the availability and quality of the data source specified is the responsibility of the caller + enum class resetDataSource { + DEFAULT=0, // Use data source selected by reset function internal rules + GPS=1, // Use GPS + RNGBCN=2, // Use beacon range data + FLOW=3, // Use optical flow rates + BARO=4, // Use Baro height + MAG=5, // Use magnetometer data + RNGFND=6, // Use rangefinder data + EXTNAV=7 // Use external nav data + }; + // update the navigation filter status void updateFilterStatus(void); @@ -798,7 +811,7 @@ private: void InitialiseVariablesMag(); // reset the horizontal position states uing the last GPS measurement - void ResetPosition(void); + void ResetPosition(resetDataSource posResetSource); // reset the stateStruct's NE position to the specified position void ResetPositionNE(float posN, float posE); @@ -807,7 +820,7 @@ private: void ResetPositionD(float posD); // reset velocity states using the last GPS measurement - void ResetVelocity(void); + void ResetVelocity(resetDataSource velResetSource); // reset the vertical position state using the last height measurement void ResetHeight(void); @@ -1149,21 +1162,6 @@ private: uint32_t firstInitTime_ms; // First time the initialise function was called (msec) uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec) - // Specify source of data to be used for a partial state reset - // Checking the availability and quality of the data source specified is the responsibility of the caller - enum resetDataSource { - DEFAULT=0, // Use data source selected by reset function internal rules - GPS=1, // Use GPS - RNGBCN=2, // Use beacon range data - FLOW=3, // Use optical flow rates - BARO=4, // Use Baro height - MAG=5, // Use magnetometer data - RNGFND=6, // Use rangefinder data - EXTNAV=7 // Use external nav data - }; - resetDataSource posResetSource; // preferred source of data for position reset - resetDataSource velResetSource; // preferred source of data for a velocity reset - // variables used to calculate a vertical velocity that is kinematically consistent with the vertical position struct { float pos;