AP_NavEKF3: make reset source variable local

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;
+                posResetSource = resetDataSource::GPS;
-                velResetSource = 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;
+                posResetSource = resetDataSource::RNGBCN;
-                velResetSource = DEFAULT;
                 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();
+        ResetVelocity(velResetSource);
-        ResetPosition();
+        ResetPosition(posResetSource);
 
     }
 

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(resetDataSource::GPS);
-            ResetVelocity();
+            ResetPosition(resetDataSource::GPS);
-            posResetSource = GPS;
-            ResetPosition();
 
             // 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();
+        ResetVelocity(resetDataSource::DEFAULT);
-        ResetPosition();
+        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;

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
+                    // reset the position to the current external sensor position
-                    ResetPosition();
+                    ResetPosition(resetDataSource::DEFAULT);
-                    // reset the velocity to the GPS velocity
+                    // reset the velocity to the external sensor velocity
-                    ResetVelocity();
+                    ResetVelocity(resetDataSource::DEFAULT);
-                    // don't fuse GPS data on this time step
+                    // 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
+                    // reset the velocity to the external sensor velocity
-                    ResetVelocity();
+                    ResetVelocity(resetDataSource::DEFAULT);
-                    // don't fuse GPS velocity data on this time step
+                    // 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;

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();
+    ResetVelocity(resetDataSource::DEFAULT);
-    ResetPosition();
+    ResetPosition(resetDataSource::DEFAULT);
     ResetHeight();
 
     // define Earth rotation vector in the NED navigation frame

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;