move faultiness check above pop_first_older_than() and check independent of range aid to switch back to original sensor if available

EKF/control.cpp

@@ -79,6 +79,14 @@ void Ekf::controlFusionModes()
 
 	}
 
+	// check faultiness (before pop_first_older_than) to see if we can change back to original height sensor
+	baroSample baro_init = _baro_buffer.get_newest();
+	_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+	gpsSample gps_init = _gps_buffer.get_newest();
+	_gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
+	rangeSample rng_init = _range_buffer.get_newest();
+	_rng_hgt_faulty = !((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
+
 	// check for arrival of new sensor data at the fusion time horizon
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
@@ -479,17 +487,6 @@ void Ekf::controlHeightSensorTimeouts()
 	// check if height has been inertial deadreckoning for too long
 	bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > 5e6);
 
-	// if in range aid mode, check faultiness that otherwise would never change back
-	if (_params.range_aid) {
-		// check if range finder data is available
-		rangeSample rng_init = _range_buffer.get_newest();
-		_rng_hgt_faulty = ((_time_last_imu - rng_init.time_us) > 2 * RNG_MAX_INTERVAL);
-
-		// check if GPS height is available
-		gpsSample gps_init = _gps_buffer.get_newest();
-		_gps_hgt_faulty = ((_time_last_imu - gps_init.time_us) > 2 * GPS_MAX_INTERVAL);
-	}
-
 	// reset the vertical position and velocity states
 	if ((P[9][9] > sq(_params.hgt_reset_lim)) && (hgt_fusion_timeout || continuous_bad_accel_hgt)) {
 		// boolean that indicates we will do a height reset