diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 2eaaa535bd..de9bb75884 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -470,7 +470,6 @@ void NavEKF3_core::setTouchdownExpected(bool val) // enabled by the combination of EK3_RNG_USE_HGT and EK3_RNG_USE_SPD parameters. void NavEKF3_core::setTerrainHgtStable(bool val) { - terrainHgtStableSet_ms = imuSampleTime_ms; terrainHgtStable = val; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index b401a340cc..dacb866561 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -225,7 +225,6 @@ void NavEKF3_core::InitialiseVariables() lastVelReset_ms = 0; lastPosResetD_ms = 0; lastRngMeasTime_ms = 0; - terrainHgtStableSet_ms = 0; // initialise other variables gpsNoiseScaler = 1.0f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 4c6e554fed..31a07a433b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1238,7 +1238,6 @@ private: uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference - uint32_t terrainHgtStableSet_ms; // system time at which terrainHgtStable was set // body frame odometry fusion obs_ring_buffer_t storedBodyOdm; // body velocity data buffer