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

sensors/vehicle_angular_velocity: add perf counters

Browse Source
This commit is contained in:
Daniel Agar
2021-02-26 19:10:47 -05:00
parent 19de1e57e3
commit d5d5b7d82e
2 changed files with 21 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+16
View File
@@ -51,6 +51,11 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
VehicleAngularVelocity::~VehicleAngularVelocity()
{
Stop();
perf_free(_filter_reset_perf);
perf_free(_dynamic_notch_filter_update_perf);
perf_free(_selection_changed_perf);
perf_free(_dynamic_notch_filter_perf);
}
bool VehicleAngularVelocity::Start()
@@ -158,6 +163,7 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons
}
_reset_filters = false;
perf_count(_filter_reset_perf);
}
void VehicleAngularVelocity::SensorBiasUpdate(bool force)
@@ -209,6 +215,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
_bias.zero();
_fifo_available = true;
perf_count(_selection_changed_perf);
return true;
}
}
@@ -232,6 +240,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
_bias.zero();
_fifo_available = false;
perf_count(_selection_changed_perf);
return true;
}
}
@@ -328,6 +338,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
if ((change_percent > 0.01f) && (_fifo_last_scale > 0.f)) {
dnf.reset(_angular_velocity(axis) / _fifo_last_scale);
}
perf_count(_dynamic_notch_filter_update_perf);
}
_dynamic_notch_available = true;
@@ -404,11 +416,15 @@ void VehicleAngularVelocity::Run()
// Apply dynamic notch filter from FFT
if (_dynamic_notch_available) {
perf_begin(_dynamic_notch_filter_perf);
for (auto &dnf : _dynamic_notch_filter) {
if (dnf[axis].getNotchFreq() > 0.f) {
dnf[axis].applyDF1(data, N);
}
}
perf_end(_dynamic_notch_filter_perf);
}
#endif // !CONSTRAINED_FLASH
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+5
View File
@@ -139,6 +139,11 @@ private:
bool _reset_filters{true};
bool _fifo_available{false};
perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")};
perf_counter_t _dynamic_notch_filter_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter update")};
perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")};
perf_counter_t _dynamic_notch_filter_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch filter")};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,