fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 03:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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.
This commit is contained in:
Jacob Dahl
2026-03-31 17:10:20 -08:00
parent 5568c66959
commit dc4cf031f9
12 changed files with 148 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/cannode/init.d/rcS
-2
View File
@@ -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
+15
View File
@@ -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
+22 -2
View File
@@ -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
+4
View File
@@ -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
+13
View File
@@ -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
+13
View File
@@ -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
-12
View File
@@ -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
+45 -52
View File
@@ -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<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) {
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);
}
// 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
-1
View File
@@ -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
-11
View File
@@ -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
+36 -40
View File
@@ -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;
}
}
}
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
-1
View File
@@ -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