ekf2: verify parameter configuration for height sources

src/modules/ekf2/EKF/common.h

@@ -128,14 +128,14 @@ enum RngCtrl : uint8_t {
 
 enum SensorFusionMask : uint16_t {
 	// Bit locations for fusion_mode
-	/* USE_GPS          = (1<<0),      ///< set to true to use GPS data */
+	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
 	USE_OPT_FLOW     = (1<<1),      ///< set to true to use optical flow data
 	INHIBIT_ACC_BIAS = (1<<2),      ///< set to true to inhibit estimation of accelerometer delta velocity bias
 	USE_EXT_VIS_POS  = (1<<3),      ///< set to true to use external vision position data
 	USE_EXT_VIS_YAW  = (1<<4),      ///< set to true to use external vision quaternion data for yaw
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
 	ROTATE_EXT_VIS   = (1<<6),      ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
-	/* USE_GPS_YAW      = (1<<7),      ///< set to true to use GPS yaw data if available */
+	DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
 	USE_EXT_VIS_VEL  = (1<<8),      ///< set to true to use external vision velocity data
 };
 

src/modules/ekf2/EKF2.cpp

@@ -31,6 +31,7 @@
  *
  ****************************************************************************/
 
+#include <px4_platform_common/events.h>
 #include "EKF2.hpp"
 
 using namespace time_literals;
@@ -284,6 +285,8 @@ void EKF2::Run()
 		// update parameters from storage
 		updateParams();
 
+		VerifyParams();
+
 		_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
 
 		// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
@@ -643,6 +646,66 @@ void EKF2::Run()
 	ScheduleDelayed(100_ms);
 }
 
+void EKF2::VerifyParams()
+{
+	if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS)
+	    || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) {
+		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS |
+					 SensorFusionMask::DEPRECATED_USE_GPS_YAW));
+		_param_ekf2_aid_mask.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "Use EKF2_GPS_CTRL instead\n");
+		/* EVENT
+		 * @description <param>EKF2_AID_MASK</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_aid_mask_gps"), events::Log::Warning,
+				    "Use EKF2_GPS_CTRL instead", _param_ekf2_aid_mask.get());
+	}
+
+	if ((_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::HPOS)) {
+		_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() & ~GnssCtrl::VPOS);
+		_param_ekf2_gps_ctrl.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "GPS lon/lat is required for altitude fusion\n");
+		/* EVENT
+		 * @description <param>EKF2_GPS_CTRL</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_gps_ctrl_alt"), events::Log::Warning,
+				    "GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get());
+	}
+
+	if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) {
+		_param_ekf2_baro_ctrl.set(1);
+		_param_ekf2_baro_ctrl.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "Baro enabled by EKF2_HGT_REF\n");
+		/* EVENT
+		 * @description <param>EKF2_BARO_CTRL</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_hgt_ref_baro"), events::Log::Warning,
+				    "Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get());
+	}
+
+	if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) {
+		_param_ekf2_rng_ctrl.set(1);
+		_param_ekf2_rng_ctrl.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "Range enabled by EKF2_HGT_REF\n");
+		/* EVENT
+		 * @description <param>EKF2_RNG_CTRL</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_hgt_ref_rng"), events::Log::Warning,
+				    "Range enabled by EKF2_HGT_REF", _param_ekf2_rng_ctrl.get());
+	}
+
+	if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) {
+		_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL));
+		_param_ekf2_gps_ctrl.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "GPS enabled by EKF2_HGT_REF\n");
+		/* EVENT
+		 * @description <param>EKF2_GPS_CTRL</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_hgt_ref_gps"), events::Log::Warning,
+				    "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get());
+	}
+}
+
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 {
 	// airspeed

src/modules/ekf2/EKF2.hpp

@@ -52,6 +52,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
@@ -134,6 +135,8 @@ private:
 
 	void Run() override;
 
+	void VerifyParams();
+
 	void PublishAidSourceStatus(const hrt_abstime &timestamp);
 	void PublishAttitude(const hrt_abstime &timestamp);
 	void PublishBaroBias(const hrt_abstime &timestamp);
@@ -281,6 +284,8 @@ private:
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
 	hrt_abstime _airspeed_validated_timestamp_last{0};
 
+	orb_advert_t _mavlink_log_pub{nullptr};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};