Browse Source

AP_NavEKF3: move global state to be on the stack

zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
commit
2d25149e2e
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
  2. 1
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
  3. 1
      libraries/AP_NavEKF3/AP_NavEKF3_core.h

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@ -557,7 +557,7 @@ void NavEKF3_core::readGpsData() @@ -557,7 +557,7 @@ void NavEKF3_core::readGpsData()
gpsCheckStatus.bad_fix = false;
// store fix time from previous read
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
const uint32_t secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time
lastTimeGpsReceived_ms = gps.last_message_time_ms(selected_gps);

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@ -198,7 +198,6 @@ void NavEKF3_core::InitialiseVariables() @@ -198,7 +198,6 @@ void NavEKF3_core::InitialiseVariables()
lastTasPassTime_ms = 0;
lastSynthYawTime_ms = 0;
lastTimeGpsReceived_ms = 0;
secondLastGpsTime_ms = 0;
lastDecayTime_ms = imuSampleTime_ms;
timeAtLastAuxEKF_ms = imuSampleTime_ms;
flowValidMeaTime_ms = imuSampleTime_ms;

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h

@ -1001,7 +1001,6 @@ private: @@ -1001,7 +1001,6 @@ private:
uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_t lastTimeGpsReceived_ms;// last time we received GPS data
uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements
uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)

Loading…
Cancel
Save