Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 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() VehicleAngularVelocity::~VehicleAngularVelocity()
{ {
Stop(); 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() bool VehicleAngularVelocity::Start()
@@ -158,6 +163,7 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons
} }
_reset_filters = false; _reset_filters = false;
perf_count(_filter_reset_perf);
} }
void VehicleAngularVelocity::SensorBiasUpdate(bool force) void VehicleAngularVelocity::SensorBiasUpdate(bool force)
@@ -209,6 +215,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
_bias.zero(); _bias.zero();
_fifo_available = true; _fifo_available = true;
perf_count(_selection_changed_perf);
return true; return true;
} }
} }
@@ -232,6 +240,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
_bias.zero(); _bias.zero();
_fifo_available = false; _fifo_available = false;
perf_count(_selection_changed_perf);
return true; return true;
} }
} }
@@ -328,6 +338,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
if ((change_percent > 0.01f) && (_fifo_last_scale > 0.f)) { if ((change_percent > 0.01f) && (_fifo_last_scale > 0.f)) {
dnf.reset(_angular_velocity(axis) / _fifo_last_scale); dnf.reset(_angular_velocity(axis) / _fifo_last_scale);
} }
perf_count(_dynamic_notch_filter_update_perf);
} }
_dynamic_notch_available = true; _dynamic_notch_available = true;
@@ -404,11 +416,15 @@ void VehicleAngularVelocity::Run()
// Apply dynamic notch filter from FFT // Apply dynamic notch filter from FFT
if (_dynamic_notch_available) { if (_dynamic_notch_available) {
perf_begin(_dynamic_notch_filter_perf);
for (auto &dnf : _dynamic_notch_filter) { for (auto &dnf : _dynamic_notch_filter) {
if (dnf[axis].getNotchFreq() > 0.f) { if (dnf[axis].getNotchFreq() > 0.f) {
dnf[axis].applyDF1(data, N); dnf[axis].applyDF1(data, N);
} }
} }
perf_end(_dynamic_notch_filter_perf);
} }
#endif // !CONSTRAINED_FLASH #endif // !CONSTRAINED_FLASH
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+5
View File
@@ -139,6 +139,11 @@ private:
bool _reset_filters{true}; bool _reset_filters{true};
bool _fifo_available{false}; 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( DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff, (ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq, (ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,