dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_angular_velocity: replace ESC/FFT elapsed perf counters with single cycle perf counter 

- non-trivial perf counters (elapsed & interval) are relatively expensive
 - if ESC and FFT notch filtering are enabled this reduces 6 updates (2 per axis) to 1
This commit is contained in:
Daniel Agar 2021-12-21 14:21:10 -05:00 committed by GitHub
parent 5894f302b5
commit 47fcdb1fdb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 12 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -52,18 +52,18 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
{
Stop();
perf_free(_cycle_perf);
perf_free(_filter_reset_perf);
perf_free(_selection_changed_perf);
#if !defined(CONSTRAINED_FLASH)
perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_free(_dynamic_notch_filter_esc_rpm_reset_perf);
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
perf_free(_dynamic_notch_filter_esc_rpm_perf);
perf_free(_dynamic_notch_filter_fft_disable_perf);
perf_free(_dynamic_notch_filter_fft_reset_perf);
perf_free(_dynamic_notch_filter_fft_update_perf);
perf_free(_dynamic_notch_filter_fft_perf);
#endif // CONSTRAINED_FLASH
}
@ -395,7 +395,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
_dynamic_notch_filter_esc_rpm_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM reset");
_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM update");
_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch filter ESC RPM elapsed");
}
} else {
@ -407,7 +406,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
_dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable");
_dynamic_notch_filter_fft_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT reset");
_dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update");
_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch filter FFT elapsed");
}
} else {
@ -666,7 +664,6 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
// Apply dynamic notch filter from ESC RPM
if (_dynamic_notch_esc_rpm_available) {
perf_begin(_dynamic_notch_filter_esc_rpm_perf);
for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
if (_esc_available[esc]) {
@ -676,21 +673,15 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
}
}
}
perf_end(_dynamic_notch_filter_esc_rpm_perf);
}
// Apply dynamic notch filter from FFT
if (_dynamic_notch_fft_available) {
perf_begin(_dynamic_notch_filter_fft_perf);
for (int peak = MAX_NUM_FFT_PEAKS - 1; peak >= 0; peak--) {
if (_dynamic_notch_filter_fft[axis][peak].getNotchFreq() > 0.f) {
_dynamic_notch_filter_fft[axis][peak].applyArray(data, N);
}
}
perf_end(_dynamic_notch_filter_fft_perf);
}
#endif // !CONSTRAINED_FLASH
@ -736,6 +727,8 @@ void VehicleAngularVelocity::Run()
}
}
perf_begin(_cycle_perf);
ParametersUpdate();
_calibration.SensorCorrectionsUpdate(selection_updated);
@ -785,6 +778,8 @@ void VehicleAngularVelocity::Run()
if (!_sensor_fifo_sub.updated()) {
if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample,
angular_velocity_uncalibrated, angular_acceleration_uncalibrated)) {
perf_end(_cycle_perf);
return;
}
}
@ -824,6 +819,8 @@ void VehicleAngularVelocity::Run()
if (!_sensor_sub.updated()) {
if (CalibrateAndPublish(sensor_data.timestamp_sample,
angular_velocity_uncalibrated, angular_acceleration_uncalibrated)) {
perf_end(_cycle_perf);
return;
}
}
@ -835,6 +832,8 @@ void VehicleAngularVelocity::Run()
if (hrt_elapsed_time(&_last_publish) > 500_ms) {
SensorSelectionUpdate(true);
}
perf_end(_cycle_perf);
}
bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sample,
@ -884,18 +883,17 @@ void VehicleAngularVelocity::PrintStatus()
_calibration.PrintStatus();
perf_print_counter(_cycle_perf);
perf_print_counter(_filter_reset_perf);
perf_print_counter(_selection_changed_perf);
#if !defined(CONSTRAINED_FLASH)
perf_print_counter(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_print_counter(_dynamic_notch_filter_esc_rpm_reset_perf);
perf_print_counter(_dynamic_notch_filter_esc_rpm_update_perf);
perf_print_counter(_dynamic_notch_filter_esc_rpm_perf);
perf_print_counter(_dynamic_notch_filter_fft_disable_perf);
perf_print_counter(_dynamic_notch_filter_fft_reset_perf);
perf_print_counter(_dynamic_notch_filter_fft_update_perf);
perf_print_counter(_dynamic_notch_filter_fft_perf);
#endif // CONSTRAINED_FLASH
}

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

@ -158,12 +158,10 @@ private:
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_reset_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_reset_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_perf{nullptr};
bool _dynamic_notch_esc_rpm_available{false};
bool _dynamic_notch_fft_available{false};
@ -178,6 +176,7 @@ private:
bool _fifo_available{false};
bool _update_sample_rate{true};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro filter")};
perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")};
perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")};