refactor(sensors): move baro/mag rate limiting from publishers to EKF2

Remove SENS_BARO_RATE and SENS_MAG_RATE parameters from the sensor
publisher modules (VehicleAirData, VehicleMagnetometer). Sensors now
publish at full sensor rate.

Add EKF2_BARO_RATE and EKF2_MAG_RATE parameters in the EKF2 module
to gate fusion rate on the consumer side. This fixes a rate-aliasing
bug where the publish-side rate limiter created irregular sample
intervals when the sensor rate was close to the configured limit
(e.g. 23Hz baro with 20Hz SENS_BARO_RATE). It also provides full-rate
data to the logger and other consumers.

Parameter migration translates SENS_BARO_RATE -> EKF2_BARO_RATE and
SENS_MAG_RATE -> EKF2_MAG_RATE on import.

ROMFS/cannode/init.d/rcS

@@ -110,8 +110,6 @@ then
 	mag_bias_estimator start
 fi
 
-param set-default SENS_MAG_RATE 100
-
 sensors start
 
 uavcannode start

src/lib/parameters/param_translation.cpp

@@ -244,5 +244,20 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2026-03-30: move sensor rate limiting from publishers to EKF2
+	{
+		if (strcmp("SENS_BARO_RATE", node->name) == 0) {
+			strcpy(node->name, "EKF2_BARO_RATE");
+			PX4_INFO("migrating %s -> %s", "SENS_BARO_RATE", "EKF2_BARO_RATE");
+			return param_modify_on_import_ret::PARAM_MODIFIED;
+		}
+
+		if (strcmp("SENS_MAG_RATE", node->name) == 0) {
+			strcpy(node->name, "EKF2_MAG_RATE");
+			PX4_INFO("migrating %s -> %s", "SENS_MAG_RATE", "EKF2_MAG_RATE");
+			return param_modify_on_import_ret::PARAM_MODIFIED;
+		}
+	}
+
 	return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
 }

src/modules/ekf2/EKF2.cpp

@@ -2179,7 +2179,17 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 
 		_ekf.set_air_density(airdata.rho);
 
-		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset});
+		// Rate-limit baro data to reduce EKF fusion CPU load
+		const float baro_rate_hz = _param_ekf2_baro_rate.get();
+		const hrt_abstime baro_interval_us = (baro_rate_hz > 0.f)
+						     ? static_cast<hrt_abstime>(1e6f / baro_rate_hz)
+						     : 0;
+
+		if (reset || (baro_interval_us == 0)
+		    || (airdata.timestamp_sample >= _last_baro_ekf_timestamp + baro_interval_us)) {
+			_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset});
+			_last_baro_ekf_timestamp = airdata.timestamp_sample;
+		}
 
 		ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
@@ -2525,7 +2535,17 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 			_mag_cal = {};
 		}
 
-		_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset});
+		// Rate-limit mag data to reduce EKF fusion CPU load (3-axis fusion is expensive)
+		const float mag_rate_hz = _param_ekf2_mag_rate.get();
+		const hrt_abstime mag_interval_us = (mag_rate_hz > 0.f)
+						    ? static_cast<hrt_abstime>(1e6f / mag_rate_hz)
+						    : 0;
+
+		if (reset || (mag_interval_us == 0)
+		    || (magnetometer.timestamp_sample >= _last_mag_ekf_timestamp + mag_interval_us)) {
+			_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset});
+			_last_mag_ekf_timestamp = magnetometer.timestamp_sample;
+		}
 
 		ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);

src/modules/ekf2/EKF2.hpp

@@ -309,6 +309,7 @@ private:
 	Vector3f _last_mag_bias_published{};
 
 	hrt_abstime _status_mag_pub_last{0};
+	hrt_abstime _last_mag_ekf_timestamp{0};
 
 	uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
 
@@ -352,6 +353,7 @@ private:
 	uint8_t _baro_calibration_count {0};
 	uint32_t _device_id_baro{0};
 	hrt_abstime _status_baro_hgt_pub_last{0};
+	hrt_abstime _last_baro_ekf_timestamp{0};
 
 	float _last_baro_bias_published{};
 
@@ -538,6 +540,7 @@ private:
 
 #if defined(CONFIG_EKF2_BAROMETER)
 		(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
+		(ParamFloat<px4::params::EKF2_BARO_RATE>) _param_ekf2_baro_rate,
 		(ParamExtFloat<px4::params::EKF2_BARO_DELAY>) _param_ekf2_baro_delay,
 		(ParamExtFloat<px4::params::EKF2_BARO_NOISE>) _param_ekf2_baro_noise,
 		(ParamExtFloat<px4::params::EKF2_BARO_GATE>) _param_ekf2_baro_gate,
@@ -575,6 +578,7 @@ private:
 #endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
+		(ParamFloat<px4::params::EKF2_MAG_RATE>) _param_ekf2_mag_rate,
 		(ParamExtFloat<px4::params::EKF2_MAG_DELAY>) _param_ekf2_mag_delay,
 		(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>) _param_ekf2_mag_e_noise,
 		(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>) _param_ekf2_mag_b_noise,

src/modules/ekf2/params_barometer.yaml

@@ -10,6 +10,19 @@ parameters:
           height sources (if activated).
       type: boolean
       default: 1
+    EKF2_BARO_RATE:
+      description:
+        short: Maximum barometer fusion rate
+        long: |-
+          Maximum rate at which barometer data is fused by the estimator.
+          Limiting this reduces EKF CPU load at the cost of slightly
+          delayed baro height tracking.
+      type: float
+      default: 20.0
+      min: 1
+      max: 200
+      unit: Hz
+      decimal: 0
     EKF2_BARO_DELAY:
       description:
         short: Barometer measurement delay relative to IMU measurements

src/modules/ekf2/params_magnetometer.yaml

@@ -2,6 +2,19 @@ module_name: ekf2
 parameters:
 - group: EKF2
   definitions:
+    EKF2_MAG_RATE:
+      description:
+        short: Maximum magnetometer fusion rate
+        long: |-
+          Maximum rate at which magnetometer data is fused by the estimator.
+          Limiting this reduces EKF CPU load (3-axis mag fusion is expensive)
+          at the cost of slightly delayed heading tracking.
+      type: float
+      default: 15.0
+      min: 1
+      max: 200
+      unit: Hz
+      decimal: 0
     EKF2_MAG_TYPE:
       description:
         short: Type of magnetometer fusion

src/modules/sensors/sensor_params_mag.yaml

@@ -52,18 +52,6 @@ parameters:
           external magnetometers.
       type: boolean
       default: 1
-    SENS_MAG_RATE:
-      description:
-        short: Magnetometer max rate
-        long: |-
-          Magnetometer data maximum publication rate. This is an upper bound,
-          actual magnetometer data rate is still dependent on the sensor.
-      type: float
-      default: 15.0
-      min: 1
-      max: 200
-      unit: Hz
-      reboot_required: true
     SENS_MAG_MODE:
       description:
         short: Sensors hub mag mode

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -265,62 +265,55 @@ void VehicleAirData::Run()
 		}
 	}
 
-	// Publish
+	// Publish at full sensor rate — Run() is triggered per sensor_baro publication via
-	if (_param_sens_baro_rate.get() > 0) {
+	// SubscriptionCallbackWorkItem, so _data_sum_count is normally 1. Rate limiting
-		int interval_us = 1e6f / _param_sens_baro_rate.get();
+	// is handled downstream by EKF2 (EKF2_BARO_RATE).
+	for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+		if (updated[instance] && (_data_sum_count[instance] > 0)) {
 
-		for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+			const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance];
-			if (updated[instance] && (_data_sum_count[instance] > 0)) {
 
-				const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance];
+			bool publish = (time_now_us <= timestamp_sample + 1_s);
 
-				if (time_now_us >= _last_publication_timestamp[instance] + interval_us) {
+			if (publish) {
-
+				publish = (_selected_sensor_sub_index >= 0)
-					bool publish = (time_now_us <= timestamp_sample + 1_s);
+					  && (instance == _selected_sensor_sub_index)
-
+					  && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR);
-					if (publish) {
-						publish = (_selected_sensor_sub_index >= 0)
-							  && (instance == _selected_sensor_sub_index)
-							  && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR);
-					}
-
-					if (publish) {
-						const float pressure_pa = _data_sum[instance] / _data_sum_count[instance];
-						const float temperature_baro = _temperature_sum[instance] / _data_sum_count[instance];
-						TemperatureSource temperature_source = _calibration[instance].external() ? TemperatureSource::EXTERNAL_BARO :
-										       TemperatureSource::DEFAULT_TEMP;
-						const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us);
-
-						const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f;
-						const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa);
-
-						// calculate air density
-						const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature);
-
-						// populate vehicle_air_data with and publish
-						vehicle_air_data_s out{};
-						out.timestamp_sample = timestamp_sample;
-						out.baro_device_id = _calibration[instance].device_id();
-						out.baro_alt_meter = altitude;
-						out.ambient_temperature = ambient_temperature;
-						out.temperature_source = static_cast<uint8_t>(temperature_source);
-						out.baro_pressure_pa = pressure_pa;
-						out.rho = air_density;
-						out.calibration_count = _calibration[instance].calibration_count();
-						out.timestamp = hrt_absolute_time();
-
-						_vehicle_air_data_pub.publish(out);
-					}
-
-					_last_publication_timestamp[instance] = time_now_us;
-
-					// reset
-					_timestamp_sample_sum[instance] = 0;
-					_data_sum[instance] = 0;
-					_temperature_sum[instance] = 0;
-					_data_sum_count[instance] = 0;
-				}
 			}
+
+			if (publish) {
+				const float pressure_pa = _data_sum[instance] / _data_sum_count[instance];
+				const float temperature_baro = _temperature_sum[instance] / _data_sum_count[instance];
+				TemperatureSource temperature_source = _calibration[instance].external() ? TemperatureSource::EXTERNAL_BARO :
+								       TemperatureSource::DEFAULT_TEMP;
+				const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us);
+
+				const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f;
+				const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa);
+
+				// calculate air density
+				const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature);
+
+				// populate vehicle_air_data with and publish
+				vehicle_air_data_s out{};
+				out.timestamp_sample = timestamp_sample;
+				out.baro_device_id = _calibration[instance].device_id();
+				out.baro_alt_meter = altitude;
+				out.ambient_temperature = ambient_temperature;
+				out.temperature_source = static_cast<uint8_t>(temperature_source);
+				out.baro_pressure_pa = pressure_pa;
+				out.rho = air_density;
+				out.calibration_count = _calibration[instance].calibration_count();
+				out.timestamp = hrt_absolute_time();
+
+				_vehicle_air_data_pub.publish(out);
+			}
+
+			// reset
+			_timestamp_sample_sum[instance] = 0;
+			_data_sum[instance] = 0;
+			_temperature_sum[instance] = 0;
+			_data_sum_count[instance] = 0;
 		}
 	}
 

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -147,7 +147,6 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
-		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate,
 		(ParamBool<px4::params::SENS_BAR_AUTOCAL>) _param_sens_baro_autocal
 	)
 };

src/modules/sensors/vehicle_air_data/params.yaml

@@ -10,17 +10,6 @@ parameters:
       min: 500
       max: 1500
       unit: hPa
-    SENS_BARO_RATE:
-      description:
-        short: Baro max rate
-        long: |-
-          Barometric air data maximum publication rate. This is an upper bound,
-          actual barometric data rate is still dependent on the sensor.
-      type: float
-      default: 20.0
-      min: 1
-      max: 200
-      unit: Hz
     SENS_BAR_AUTOCAL:
       description:
         short: Barometer auto calibration

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -555,9 +555,10 @@ void VehicleMagnetometer::Run()
 		}
 	}
 
-	// Publish
+	// Publish at full sensor rate — Run() is triggered per sensor_mag publication via
-	if (_param_sens_mag_rate.get() > 0) {
+	// SubscriptionCallbackWorkItem, so _data_sum_count is normally 1. Rate limiting
-		int interval_us = 1e6f / _param_sens_mag_rate.get();
+	// is handled downstream by EKF2 (EKF2_MAG_RATE).
+	{
 		const bool multi_mode = (_param_sens_mag_mode.get() == 0);
 
 		for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
@@ -565,53 +566,48 @@ void VehicleMagnetometer::Run()
 
 				const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance];
 
-				if (timestamp_sample >= _last_publication_timestamp[instance] + interval_us) {
+				bool publish = (time_now_us <= timestamp_sample + 1_s);
 
-					bool publish = (time_now_us <= timestamp_sample + 1_s);
+				if (!multi_mode && publish) {
+					publish = (_selected_sensor_sub_index >= 0)
+						  && (instance == _selected_sensor_sub_index)
+						  && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR);
+				}
 
-					if (!multi_mode && publish) {
+				if (publish) {
-						publish = (_selected_sensor_sub_index >= 0)
+					const Vector3f magnetometer_data = _data_sum[instance] / _data_sum_count[instance];
-							  && (instance == _selected_sensor_sub_index)
-							  && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR);
-					}
 
-					if (publish) {
+					// populate vehicle_magnetometer and publish
-						const Vector3f magnetometer_data = _data_sum[instance] / _data_sum_count[instance];
+					vehicle_magnetometer_s out{};
+					out.timestamp_sample = timestamp_sample;
+					out.device_id = _calibration[instance].device_id();
+					magnetometer_data.copyTo(out.magnetometer_ga);
+					out.calibration_count = _calibration[instance].calibration_count();
+					out.timestamp = hrt_absolute_time();
 
-						// populate vehicle_magnetometer and publish
+					if (multi_mode) {
-						vehicle_magnetometer_s out{};
-						out.timestamp_sample = timestamp_sample;
-						out.device_id = _calibration[instance].device_id();
-						magnetometer_data.copyTo(out.magnetometer_ga);
-						out.calibration_count = _calibration[instance].calibration_count();
-						out.timestamp = hrt_absolute_time();
 
-						if (multi_mode) {
+						if (!_vehicle_magnetometer_pub[instance].advertised()) {
-
+							// prefer to maintain vehicle_magneometer instance numbering in sensor order
-							if (!_vehicle_magnetometer_pub[instance].advertised()) {
+							for (int mag_instance = 0; mag_instance < instance; mag_instance++) {
-								// prefer to maintain vehicle_magneometer instance numbering in sensor order
+								if (_calibration[instance].enabled()) {
-								for (int mag_instance = 0; mag_instance < instance; mag_instance++) {
+									_vehicle_magnetometer_pub[mag_instance].advertise();
-									if (_calibration[instance].enabled()) {
-										_vehicle_magnetometer_pub[mag_instance].advertise();
-									}
 								}
 							}
-
-							_vehicle_magnetometer_pub[instance].publish(out);
-
-						} else {
-							// otherwise only ever publish the first instance
-							_vehicle_magnetometer_pub[0].publish(out);
 						}
+
+						_vehicle_magnetometer_pub[instance].publish(out);
+
+					} else {
+						// otherwise only ever publish the first instance
+						_vehicle_magnetometer_pub[0].publish(out);
 					}
-
-					_last_publication_timestamp[instance] = timestamp_sample;
-
-					// reset
-					_timestamp_sample_sum[instance] = 0;
-					_data_sum[instance].zero();
-					_data_sum_count[instance] = 0;
 				}
+
+				// reset
+				_timestamp_sample_sum[instance] = 0;
+				_data_sum[instance].zero();
+				_data_sum_count[instance] = 0;
 			}
 		}
 	}

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -178,7 +178,6 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
 		(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,
-		(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate,
 		(ParamBool<px4::params::SENS_MAG_AUTOCAL>) _param_sens_mag_autocal,
 		(ParamInt<px4::params::CAL_MAG_SIDES>) _param_cal_mag_sides,
 		(ParamInt<px4::params::SENS_MAG_SIDES>) _param_sens_mag_sides