ekf2: enable multiple height sources fusion

Instead of having a single height source fused into the EKF and the
other ones "waiting" for a failure or the primary sensor, fuse all
sources in EKF2 at the same time. To prevent the sources from fighting against each
other, the "primary" source is set as reference and the other ones are
running a bias estimator in order to make all the secondary height
sources converge to the primary one.

If the reference isn't available, another one is automatically selected
from a priority list. This secondary reference keeps its current bias
estimate but stops updating it in order to be the new reference as close
as possible to the primary one.

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -10,9 +10,19 @@ EkfWrapper::~EkfWrapper()
 {
 }
 
-void EkfWrapper::setBaroHeight()
+void EkfWrapper::setBaroHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::BARO;
+}
+
+void EkfWrapper::enableBaroHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_BARO_HGT;
+}
+
+void EkfWrapper::disableBaroHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_BARO_HGT;
 }
 
 bool EkfWrapper::isIntendingBaroHeightFusion() const
@@ -20,9 +30,19 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
 	return _ekf->control_status_flags().baro_hgt;
 }
 
-void EkfWrapper::setGpsHeight()
+void EkfWrapper::setGpsHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::GPS;
+}
+
+void EkfWrapper::enableGpsHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_HGT;
+}
+
+void EkfWrapper::disableGpsHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_HGT;
 }
 
 bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -30,9 +50,19 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
 	return _ekf->control_status_flags().gps_hgt;
 }
 
-void EkfWrapper::setRangeHeight()
+void EkfWrapper::setRangeHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::RANGE;
+}
+
+void EkfWrapper::enableRangeHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_RNG_HGT;
+}
+
+void EkfWrapper::disableRangeHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_RNG_HGT;
 }
 
 bool EkfWrapper::isIntendingRangeHeightFusion() const
@@ -40,9 +70,19 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
 	return _ekf->control_status_flags().rng_hgt;
 }
 
-void EkfWrapper::setVisionHeight()
+void EkfWrapper::setVisionHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::EV;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::EV;
+}
+
+void EkfWrapper::enableVisionHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_HGT;
+}
+
+void EkfWrapper::disableVisionHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_HGT;
 }
 
 bool EkfWrapper::isIntendingVisionHeightFusion() const

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -49,16 +49,24 @@ public:
 	~EkfWrapper();
 
 
-	void setBaroHeight();
+	void setBaroHeightRef();
+	void enableBaroHeightFusion();
+	void disableBaroHeightFusion();
 	bool isIntendingBaroHeightFusion() const;
 
-	void setGpsHeight();
+	void setGpsHeightRef();
+	void enableGpsHeightFusion();
+	void disableGpsHeightFusion();
 	bool isIntendingGpsHeightFusion() const;
 
-	void setRangeHeight();
+	void setRangeHeightRef();
+	void enableRangeHeightFusion();
+	void disableRangeHeightFusion();
 	bool isIntendingRangeHeightFusion() const;
 
-	void setVisionHeight();
+	void setVisionHeightRef();
+	void enableVisionHeightFusion();
+	void disableVisionHeightFusion();
 	bool isIntendingVisionHeightFusion() const;
 
 	void enableGpsFusion();