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

sensors/vehicle_angular_velocity: only allocate dynamic notch perf counters if enabled (IMU_GYRO_DYN_NF)

Browse Source
This commit is contained in:
Daniel Agar
2021-03-08 10:35:48 -05:00
committed by GitHub
parent 68c171fd4f
commit cbb2fa440b
3 changed files with 39 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+28 -3
View File
@@ -301,6 +301,31 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
break;
}
}
#if !defined(CONSTRAINED_FLASH)
if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm) {
if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) {
_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM update");
}
if (_dynamic_notch_filter_esc_rpm_perf == nullptr) {
_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter");
}
}
if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) {
if (_dynamic_notch_filter_fft_update_perf == nullptr) {
_dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update");
}
if (_dynamic_notch_filter_fft_perf == nullptr) {
_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter");
}
}
#endif // !CONSTRAINED_FLASH
}
}
@@ -337,7 +362,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
{
#if !defined(CONSTRAINED_FLASH)
const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b01;
const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm;
if (enabled && (_esc_status_sub.updated() || force)) {
_dynamic_notch_esc_rpm_available = false;
@@ -390,7 +415,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
{
#if !defined(CONSTRAINED_FLASH)
const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b10;
const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT;
if (enabled && (_sensor_gyro_fft_sub.updated() || force)) {
_dynamic_notch_fft_available = false;
@@ -400,7 +425,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) && (sensor_gyro_fft.device_id == _selected_sensor_device_id)
&& (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 0.05f)) {
float *peak_frequencies[] { sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z};
float *peak_frequencies[] {sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z};
for (int axis = 0; axis < 3; axis++) {
for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) {
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+10 -4
View File
@@ -126,6 +126,12 @@ private:
math::NotchFilterArray<float> _notch_filter_velocity[3] {};
#if !defined(CONSTRAINED_FLASH)
enum DynamicNotch {
EscRpm = 1,
FFT = 2,
};
static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
@@ -133,10 +139,10 @@ private:
math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][3] {};
math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM update")};
perf_counter_t _dynamic_notch_filter_fft_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update")};
perf_counter_t _dynamic_notch_filter_esc_rpm_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter")};
perf_counter_t _dynamic_notch_filter_fft_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter")};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_perf{nullptr};
bool _dynamic_notch_esc_rpm_available{false};
bool _dynamic_notch_fft_available{false};
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
+1 -1
View File
@@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);
* Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN).
* @group Sensors
* @min 0
* @max 2
* @max 3
* @bit 0 ESC RPM
* @bit 1 FFT
*/