yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

sensors: filter sample rate calculate with simple interval average instead of perf count

Browse Source 
- the perf counter intervals aren't numerically stable over extended periods (https://github.com/PX4/Firmware/pull/14046)
This commit is contained in:
Daniel Agar
2020-02-07 13:04:02 -05:00
committed by GitHub
parent a3710fcdef
commit 8df22541ef
4 changed files with 56 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -52,8 +52,6 @@ VehicleAcceleration::VehicleAcceleration() :
VehicleAcceleration::~VehicleAcceleration()
{
Stop();
perf_free(_interval_perf);
}
bool VehicleAcceleration::Start()
@@ -85,40 +83,40 @@ void VehicleAcceleration::Stop()
void VehicleAcceleration::CheckFilters()
{
if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
_filter_check_last = hrt_absolute_time();
// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration)
if (_interval_count > 2500) {
bool sample_rate_changed = false;
// calculate sensor update rate
const float sample_interval_avg = perf_mean(_interval_perf);
const float sample_interval_avg = _interval_sum / _interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
const float update_rate_hz = 1.0f / sample_interval_avg;
const float update_rate_hz = 1.e6f / sample_interval_avg;
if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
_update_rate_hz = update_rate_hz;
// check if sample rate error is greater than 1%
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
++_sample_rate_incorrect_count;
sample_rate_changed = true;
}
}
}
const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
const bool lp_updated = (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f);
if (sample_rate_updated || lp_updated) {
PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
if (sample_rate_changed || lp_updated) {
PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
_filter_sample_rate = _update_rate_hz;
// update software low pass filters
_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
_lp_filter.reset(_acceleration_prev);
// reset state
_sample_rate_incorrect_count = 0;
}
// reset sample interval accumulator
_timestamp_sample_last = 0;
}
}
@@ -167,8 +165,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
_corrections.set_device_id(report.device_id);
// reset sample rate monitor
_sample_rate_incorrect_count = 0;
// reset sample interval accumulator on sensor change
_timestamp_sample_last = 0;
return true;
}
@@ -218,7 +216,17 @@ void VehicleAcceleration::Run()
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
if (sensor_updated) {
perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
// collect sample interval average for filters
if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
_interval_count++;
} else {
_interval_sum = 0.f;
_interval_count = 0.f;
}
_timestamp_sample_last = sensor_data.timestamp_sample;
}
CheckFilters();

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

@@ -90,25 +90,25 @@ private:
SensorCorrections _corrections;
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
hrt_abstime _last_publish{0};
hrt_abstime _filter_check_last{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] */
math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f};
float _filter_sample_rate{kInitialRateHz};
int _sample_rate_incorrect_count{0};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
hrt_abstime _timestamp_sample_last{0};
float _interval_sum{0.f};
float _interval_count{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
)

41
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@@ -55,8 +55,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
VehicleAngularVelocity::~VehicleAngularVelocity()
{
Stop();
perf_free(_interval_perf);
}
bool VehicleAngularVelocity::Start()
@@ -88,28 +86,27 @@ void VehicleAngularVelocity::Stop()
void VehicleAngularVelocity::CheckFilters()
{
if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
_filter_check_last = hrt_absolute_time();
// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration)
if (_interval_count > 2500) {
bool sample_rate_changed = false;
// calculate sensor update rate
const float sample_interval_avg = perf_mean(_interval_perf);
const float sample_interval_avg = _interval_sum / _interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
const float update_rate_hz = 1.0f / sample_interval_avg;
const float update_rate_hz = 1.e6f / sample_interval_avg;
if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
_update_rate_hz = update_rate_hz;
// check if sample rate error is greater than 1%
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
++_sample_rate_incorrect_count;
sample_rate_changed = true;
}
}
}
const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f);
const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
|| (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f));
@@ -117,8 +114,8 @@ void VehicleAngularVelocity::CheckFilters()
const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) >
0.01f);
if (sample_rate_updated || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
if (sample_rate_changed || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
_filter_sample_rate = _update_rate_hz;
// update software low pass filters
@@ -130,10 +127,10 @@ void VehicleAngularVelocity::CheckFilters()
_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
_lp_filter_acceleration.reset(_angular_acceleration_prev);
// reset state
_sample_rate_incorrect_count = 0;
}
// reset sample interval accumulator
_timestamp_sample_last = 0;
}
}
@@ -182,8 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
_corrections.set_device_id(report.device_id);
// reset sample rate monitor
_sample_rate_incorrect_count = 0;
// reset sample interval accumulator on sensor change
_timestamp_sample_last = 0;
return true;
}
@@ -233,7 +230,17 @@ void VehicleAngularVelocity::Run()
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
if (sensor_updated) {
perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
// collect sample interval average for filters
if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
_interval_count++;
} else {
_interval_sum = 0.f;
_interval_count = 0.f;
}
_timestamp_sample_last = sensor_data.timestamp_sample;
}
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.

8
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@@ -93,8 +93,6 @@ private:
SensorCorrections _corrections;
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f};
@@ -102,7 +100,6 @@ private:
hrt_abstime _timestamp_sample_prev{0};
hrt_abstime _last_publish{0};
hrt_abstime _filter_check_last{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] */
@@ -114,11 +111,14 @@ private:
math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 10.0f};
float _filter_sample_rate{kInitialRateHz};
int _sample_rate_incorrect_count{0};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
hrt_abstime _timestamp_sample_last{0};
float _interval_sum{0.f};
float _interval_count{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,