Skip to content

Commit 8df2254

Browse files
dagardagar
authored
sensors: filter sample rate calculate with simple interval average instead of perf count
 - the perf counter intervals aren't numerically stable over extended periods (#14046)
1 parent a3710fc commit 8df2254

File tree

4 files changed

+56
-41
lines changed

4 files changed

+56
-41
lines changed

‎src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

+24-16
Original file line numberDiff line numberDiff line change
@@ -52,8 +52,6 @@ VehicleAcceleration::VehicleAcceleration() :
5252
VehicleAcceleration::~VehicleAcceleration()
5353
{
5454
Stop();
55-
56-
perf_free(_interval_perf);
5755
}
5856

5957
bool VehicleAcceleration::Start()
@@ -85,40 +83,40 @@ void VehicleAcceleration::Stop()
8583

8684
void VehicleAcceleration::CheckFilters()
8785
{
88-
if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
89-
_filter_check_last = hrt_absolute_time();
86+
// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration)
87+
if (_interval_count > 2500) {
88+
bool sample_rate_changed = false;
9089

9190
// calculate sensor update rate
92-
const float sample_interval_avg = perf_mean(_interval_perf);
91+
const float sample_interval_avg = _interval_sum / _interval_count;
9392

9493
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
9594

96-
const float update_rate_hz = 1.0f / sample_interval_avg;
95+
const float update_rate_hz = 1.e6f / sample_interval_avg;
9796

9897
if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
9998
_update_rate_hz = update_rate_hz;
10099

101100
// check if sample rate error is greater than 1%
102101
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
103-
++_sample_rate_incorrect_count;
102+
sample_rate_changed = true;
104103
}
105104
}
106105
}
107106

108-
const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
109107
const bool lp_updated = (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f);
110108

111-
if (sample_rate_updated || lp_updated) {
112-
PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
109+
if (sample_rate_changed || lp_updated) {
110+
PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
113111
_filter_sample_rate = _update_rate_hz;
114112

115113
// update software low pass filters
116114
_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
117115
_lp_filter.reset(_acceleration_prev);
118-
119-
// reset state
120-
_sample_rate_incorrect_count = 0;
121116
}
117+
118+
// reset sample interval accumulator
119+
_timestamp_sample_last = 0;
122120
}
123121
}
124122

@@ -167,8 +165,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
167165

168166
_corrections.set_device_id(report.device_id);
169167

170-
// reset sample rate monitor
171-
_sample_rate_incorrect_count = 0;
168+
// reset sample interval accumulator on sensor change
169+
_timestamp_sample_last = 0;
172170

173171
return true;
174172
}
@@ -218,7 +216,17 @@ void VehicleAcceleration::Run()
218216
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
219217

220218
if (sensor_updated) {
221-
perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
219+
// collect sample interval average for filters
220+
if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
221+
_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
222+
_interval_count++;
223+
224+
} else {
225+
_interval_sum = 0.f;
226+
_interval_count = 0.f;
227+
}
228+
229+
_timestamp_sample_last = sensor_data.timestamp_sample;
222230
}
223231

224232
CheckFilters();

‎src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -90,25 +90,25 @@ class VehicleAcceleration : public ModuleParams, public px4::WorkItem
9090

9191
SensorCorrections _corrections;
9292

93-
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
94-
9593
matrix::Vector3f _bias{0.f, 0.f, 0.f};
9694

9795
matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
9896

9997
hrt_abstime _last_publish{0};
100-
hrt_abstime _filter_check_last{0};
10198
static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
10299
float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
103100

104101
math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f};
105102

106103
float _filter_sample_rate{kInitialRateHz};
107-
int _sample_rate_incorrect_count{0};
108104

109105
uint32_t _selected_sensor_device_id{0};
110106
uint8_t _selected_sensor_sub_index{0};
111107

108+
hrt_abstime _timestamp_sample_last{0};
109+
float _interval_sum{0.f};
110+
float _interval_count{0.f};
111+
112112
DEFINE_PARAMETERS(
113113
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
114114
)

‎src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

+24-17
Original file line numberDiff line numberDiff line change
@@ -55,8 +55,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
5555
VehicleAngularVelocity::~VehicleAngularVelocity()
5656
{
5757
Stop();
58-
59-
perf_free(_interval_perf);
6058
}
6159

6260
bool VehicleAngularVelocity::Start()
@@ -88,37 +86,36 @@ void VehicleAngularVelocity::Stop()
8886

8987
void VehicleAngularVelocity::CheckFilters()
9088
{
91-
if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
92-
_filter_check_last = hrt_absolute_time();
89+
// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration)
90+
if (_interval_count > 2500) {
91+
bool sample_rate_changed = false;
9392

9493
// calculate sensor update rate
95-
const float sample_interval_avg = perf_mean(_interval_perf);
94+
const float sample_interval_avg = _interval_sum / _interval_count;
9695

9796
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
9897

99-
const float update_rate_hz = 1.0f / sample_interval_avg;
98+
const float update_rate_hz = 1.e6f / sample_interval_avg;
10099

101100
if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
102101
_update_rate_hz = update_rate_hz;
103102

104103
// check if sample rate error is greater than 1%
105104
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
106-
++_sample_rate_incorrect_count;
105+
sample_rate_changed = true;
107106
}
108107
}
109108
}
110109

111-
const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
112-
113110
const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f);
114111
const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
115112
|| (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f));
116113

117114
const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) >
118115
0.01f);
119116

120-
if (sample_rate_updated || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
121-
PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
117+
if (sample_rate_changed || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
118+
PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
122119
_filter_sample_rate = _update_rate_hz;
123120

124121
// update software low pass filters
@@ -130,10 +127,10 @@ void VehicleAngularVelocity::CheckFilters()
130127

131128
_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
132129
_lp_filter_acceleration.reset(_angular_acceleration_prev);
133-
134-
// reset state
135-
_sample_rate_incorrect_count = 0;
136130
}
131+
132+
// reset sample interval accumulator
133+
_timestamp_sample_last = 0;
137134
}
138135
}
139136

@@ -182,8 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
182179

183180
_corrections.set_device_id(report.device_id);
184181

185-
// reset sample rate monitor
186-
_sample_rate_incorrect_count = 0;
182+
// reset sample interval accumulator on sensor change
183+
_timestamp_sample_last = 0;
187184

188185
return true;
189186
}
@@ -233,7 +230,17 @@ void VehicleAngularVelocity::Run()
233230
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
234231

235232
if (sensor_updated) {
236-
perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
233+
// collect sample interval average for filters
234+
if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
235+
_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
236+
_interval_count++;
237+
238+
} else {
239+
_interval_sum = 0.f;
240+
_interval_count = 0.f;
241+
}
242+
243+
_timestamp_sample_last = sensor_data.timestamp_sample;
237244
}
238245

239246
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.

‎src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -93,16 +93,13 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
9393

9494
SensorCorrections _corrections;
9595

96-
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
97-
9896
matrix::Vector3f _bias{0.f, 0.f, 0.f};
9997

10098
matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f};
10199
matrix::Vector3f _angular_velocity_prev{0.f, 0.f, 0.f};
102100
hrt_abstime _timestamp_sample_prev{0};
103101

104102
hrt_abstime _last_publish{0};
105-
hrt_abstime _filter_check_last{0};
106103
static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
107104
float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
108105

@@ -114,11 +111,14 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
114111
math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 10.0f};
115112

116113
float _filter_sample_rate{kInitialRateHz};
117-
int _sample_rate_incorrect_count{0};
118114

119115
uint32_t _selected_sensor_device_id{0};
120116
uint8_t _selected_sensor_sub_index{0};
121117

118+
hrt_abstime _timestamp_sample_last{0};
119+
float _interval_sum{0.f};
120+
float _interval_count{0.f};
121+
122122
DEFINE_PARAMETERS(
123123
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
124124
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,

0 commit comments

Comments
 (0)