ekf2: conditional range aid change height ref

- we should never directly use an "unhealthy" range finder sample for
   state corrections or resets, but we also shouldn't immediately abort
   active rng_hgt until the timeout has passed

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -81,6 +81,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			// If we are supposed to be using range finder data but have bad range measurements
 			// and are on the ground, then synthesise a measurement at the expected on ground value
 			if (!_control_status.flags.in_air
+			    && _control_status.flags.vehicle_at_rest
 			    && _range_sensor.isRegularlySendingData()
 			    && _range_sensor.isDataReady()) {
 
@@ -105,14 +106,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
 				|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
 				&& _control_status.flags.tilt_align
-				&& measurement_valid
-				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isKinematicallyConsistent();
+				&& measurement_valid;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
-				&& _range_sensor.isRegularlySendingData();
-
+				&& _range_sensor.isRegularlySendingData()
+				&& _range_sensor.isDataHealthy();
 
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 						      && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
@@ -129,7 +128,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 		} else {
 			if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
-				if (do_conditional_range_aid) {
+				if (do_conditional_range_aid && starting_conditions_passing) {
 					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
 					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
@@ -142,7 +141,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
 
-				} else if (do_range_aid) {
+				} else if (do_range_aid && starting_conditions_passing) {
 					// Range finder is the primary height source, the ground is now the datum used
 					// to compute the local vertical position
 					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
@@ -159,11 +158,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				}
 
 			} else {
-				if (do_conditional_range_aid || do_range_aid) {
+				if ((do_conditional_range_aid || do_range_aid) && starting_conditions_passing) {
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
 					_control_status.flags.rng_hgt = true;
 
 					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+						ECL_INFO("starting %s height fusion, resetting terrain", HGT_SRC_NAME);
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -174,11 +174,26 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
 			if (continuing_conditions_passing) {
 
-				fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
+				if (do_conditional_range_aid) {
+					_height_sensor_ref = HeightSensor::RANGE;
+
+				} else if (_height_sensor_ref == HeightSensor::RANGE) {
+					_height_sensor_ref = HeightSensor::UNKNOWN;
+				}
+
+				if (_range_sensor.isDataHealthy()
+				    && _control_status.flags.rng_kin_consistent
+				   ) {
+					fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
+				}
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
 
-				if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
+				if (isHeightResetRequired()
+				    && _control_status.flags.rng_hgt
+				    && (_height_sensor_ref == HeightSensor::RANGE)
+				    && starting_conditions_passing
+				   ) {
 					// All height sources are failing
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
@@ -200,7 +215,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 						stopRngHgtFusion();
 						stopRngTerrFusion();
 
-					} else {
+					} else if (starting_conditions_passing) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}

src/modules/ekf2/EKF/estimator_interface.h

@@ -44,9 +44,9 @@
 
 #if defined(MODULE_NAME)
 #include <px4_platform_common/log.h>
-# define ECL_INFO PX4_DEBUG
-# define ECL_WARN PX4_DEBUG
-# define ECL_ERR  PX4_DEBUG
+# define ECL_INFO PX4_INFO
+# define ECL_WARN PX4_WARN
+# define ECL_ERR  PX4_ERR
 # define ECL_DEBUG PX4_DEBUG
 #else
 # define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__)

src/modules/ekf2/EKF/height_control.cpp

@@ -112,8 +112,11 @@ void Ekf::checkHeightSensorRefFallback()
 		    || ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt)
 		    || ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt)
 		    || ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) {
-			ECL_INFO("fallback to secondary height reference");
+
+
 			_height_sensor_ref = fallback_list[i];
+
+			ECL_WARN("fallback to secondary height reference %d", (int)_height_sensor_ref);
 			break;
 		}
 	}

src/modules/ekf2/EKF2.cpp

@@ -1059,12 +1059,12 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
 	if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) {
 		const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
 
-		if (fabsf(status.bias - _last_baro_bias_published) > 1e-6f) {
-			_estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_baro_hgt().timestamp_sample, timestamp,
-							 _device_id_baro));
+		//if (fabsf(status.bias - _last_baro_bias_published) > 1e-6f) {
+		_estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_baro_hgt().timestamp_sample, timestamp,
+						 _device_id_baro));
 
-			_last_baro_bias_published = status.bias;
-		}
+		_last_baro_bias_published = status.bias;
+		//}
 	}
 }
 #endif // CONFIG_EKF2_BAROMETER

src/modules/logger/logged_topics.cpp

@@ -171,12 +171,12 @@ void LoggedTopics::add_default_topics()
 	// EKF multi topics
 	{
 		// optionally log all estimator* topics at minimal rate
-		const uint16_t kEKFVerboseIntervalMilliseconds = 500; // 2 Hz
+		//const uint16_t kEKFVerboseIntervalMilliseconds = 500; // 2 Hz
 		const struct orb_metadata *const *topic_list = orb_get_topics();
 
 		for (size_t i = 0; i < orb_topics_count(); i++) {
 			if (strncmp(topic_list[i]->o_name, "estimator", 9) == 0) {
-				add_optional_topic_multi(topic_list[i]->o_name, kEKFVerboseIntervalMilliseconds);
+				add_topic_multi(topic_list[i]->o_name, 0, 1, false);
 			}
 		}
 	}