From dc4cf031f9641eefc382ec3b24b25623ea189367 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 31 Mar 2026 17:10:20 -0800 Subject: [PATCH] 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 | 2 - src/lib/parameters/param_translation.cpp | 15 +++ src/modules/ekf2/EKF2.cpp | 24 ++++- src/modules/ekf2/EKF2.hpp | 4 + src/modules/ekf2/params_barometer.yaml | 13 +++ src/modules/ekf2/params_magnetometer.yaml | 13 +++ src/modules/sensors/sensor_params_mag.yaml | 12 --- .../vehicle_air_data/VehicleAirData.cpp | 97 +++++++++---------- .../vehicle_air_data/VehicleAirData.hpp | 1 - .../sensors/vehicle_air_data/params.yaml | 11 --- .../VehicleMagnetometer.cpp | 76 +++++++-------- .../VehicleMagnetometer.hpp | 1 - 12 files changed, 148 insertions(+), 121 deletions(-) diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index 0f88c7a70a..575464ea1e 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/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 diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index e88faf8579..92d6f8b473 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/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; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 84227fab6a..6e9c9bb540 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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(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(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); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 36d7763995..18de839293 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/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) _param_ekf2_baro_ctrl,///< barometer control selection + (ParamFloat) _param_ekf2_baro_rate, (ParamExtFloat) _param_ekf2_baro_delay, (ParamExtFloat) _param_ekf2_baro_noise, (ParamExtFloat) _param_ekf2_baro_gate, @@ -575,6 +578,7 @@ private: #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_MAGNETOMETER) + (ParamFloat) _param_ekf2_mag_rate, (ParamExtFloat) _param_ekf2_mag_delay, (ParamExtFloat) _param_ekf2_mag_e_noise, (ParamExtFloat) _param_ekf2_mag_b_noise, diff --git a/src/modules/ekf2/params_barometer.yaml b/src/modules/ekf2/params_barometer.yaml index cefa503039..a0f6f1955e 100644 --- a/src/modules/ekf2/params_barometer.yaml +++ b/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 diff --git a/src/modules/ekf2/params_magnetometer.yaml b/src/modules/ekf2/params_magnetometer.yaml index 1790d113db..df30adaebc 100644 --- a/src/modules/ekf2/params_magnetometer.yaml +++ b/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 diff --git a/src/modules/sensors/sensor_params_mag.yaml b/src/modules/sensors/sensor_params_mag.yaml index e5bbdf8d2d..475827328b 100644 --- a/src/modules/sensors/sensor_params_mag.yaml +++ b/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 diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 9c4fe75ece..6732f85934 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -265,62 +265,55 @@ void VehicleAirData::Run() } } - // Publish - if (_param_sens_baro_rate.get() > 0) { - int interval_us = 1e6f / _param_sens_baro_rate.get(); + // Publish at full sensor rate — Run() is triggered per sensor_baro publication via + // SubscriptionCallbackWorkItem, so _data_sum_count is normally 1. Rate limiting + // 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++) { - if (updated[instance] && (_data_sum_count[instance] > 0)) { + const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance]; - 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) { - - bool publish = (time_now_us <= timestamp_sample + 1_s); - - 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(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) { + 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(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; } } diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index d7620d48d0..23a7390d71 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -147,7 +147,6 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, - (ParamFloat) _param_sens_baro_rate, (ParamBool) _param_sens_baro_autocal ) }; diff --git a/src/modules/sensors/vehicle_air_data/params.yaml b/src/modules/sensors/vehicle_air_data/params.yaml index bfdf106ada..4ffbdf2e90 100644 --- a/src/modules/sensors/vehicle_air_data/params.yaml +++ b/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 diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 08afb1d57d..12517f204a 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -555,9 +555,10 @@ void VehicleMagnetometer::Run() } } - // Publish - if (_param_sens_mag_rate.get() > 0) { - int interval_us = 1e6f / _param_sens_mag_rate.get(); + // Publish at full sensor rate — Run() is triggered per sensor_mag publication via + // SubscriptionCallbackWorkItem, so _data_sum_count is normally 1. Rate limiting + // 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) { - 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 Vector3f magnetometer_data = _data_sum[instance] / _data_sum_count[instance]; - if (publish) { - const Vector3f magnetometer_data = _data_sum[instance] / _data_sum_count[instance]; + // populate vehicle_magnetometer and publish + 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 - 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 (multi_mode) { - - if (!_vehicle_magnetometer_pub[instance].advertised()) { - // prefer to maintain vehicle_magneometer instance numbering in sensor order - for (int mag_instance = 0; mag_instance < instance; mag_instance++) { - if (_calibration[instance].enabled()) { - _vehicle_magnetometer_pub[mag_instance].advertise(); - } + if (!_vehicle_magnetometer_pub[instance].advertised()) { + // prefer to maintain vehicle_magneometer instance numbering in sensor order + for (int mag_instance = 0; mag_instance < instance; mag_instance++) { + 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; } } } diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index c90398fe1f..c8747424c3 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -178,7 +178,6 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_mag_comp_typ, (ParamBool) _param_sens_mag_mode, - (ParamFloat) _param_sens_mag_rate, (ParamBool) _param_sens_mag_autocal, (ParamInt) _param_cal_mag_sides, (ParamInt) _param_sens_mag_sides