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

sensors/vehicle_acceleration: limit scheduling to IMU_INTEG_RATE

Browse Source
This commit is contained in:
Daniel Agar 2020-06-01 10:14:21 -04:00
parent b36845330e
commit 96f043cd62
2 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -100,6 +100,22 @@ void VehicleAcceleration::CheckFilters()
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
reset_filters = true;
}
if (reset_filters || (_required_sample_updates == 0)) {
if (_param_imu_integ_rate.get() > 0) {
// determine number of sensor samples that will get closest to the desired rate
const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
(float)sensor_accel_s::ORB_QUEUE_LENGTH);
_sensor_sub[_selected_sensor_sub_index].set_required_updates(samples);
_required_sample_updates = samples;
} else {
_sensor_sub[_selected_sensor_sub_index].set_required_updates(1);
_required_sample_updates = 1;
}
}
}
if (!reset_filters) {
@ -170,6 +186,7 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
// reset sample interval accumulator on sensor change
_timestamp_sample_last = 0;
_required_sample_updates = 0;
return true;
}
@ -256,7 +273,6 @@ void VehicleAcceleration::Run()
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration);
_last_publish = v_acceleration.timestamp_sample;
return;
}
}

6
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View File

@ -94,10 +94,11 @@ private:
matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
hrt_abstime _last_publish{0};
static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
uint8_t _required_sample_updates{0}; /**< number or sensor publications required for configured rate */
math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f};
float _filter_sample_rate{kInitialRateHz};
@ -110,7 +111,8 @@ private:
float _interval_count{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
)
};