From af3573e464fa75b8a250b0b3d9108234cfd219d5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 15 Mar 2021 00:47:11 -0400 Subject: [PATCH] gyro_fft: log peak frequency magnitudes --- msg/sensor_gyro_fft.msg | 4 ++++ src/modules/gyro_fft/GyroFFT.cpp | 5 ++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/msg/sensor_gyro_fft.msg b/msg/sensor_gyro_fft.msg index 203c43aa8e..66c97e7995 100644 --- a/msg/sensor_gyro_fft.msg +++ b/msg/sensor_gyro_fft.msg @@ -9,3 +9,7 @@ float32 resolution_hz float32[6] peak_frequencies_x # x axis peak frequencies float32[6] peak_frequencies_y # y axis peak frequencies float32[6] peak_frequencies_z # z axis peak frequencies + +uint32[6] peak_magnitude_x # x axis peak frequencies magnitude +uint32[6] peak_magnitude_y # y axis peak frequencies magnitude +uint32[6] peak_magnitude_z # z axis peak frequencies magnitude diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index 8bbb00b23f..386b50fef6 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/src/modules/gyro_fft/GyroFFT.cpp @@ -318,7 +318,8 @@ void GyroFFT::Run() } if (peaks_detected) { - 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}; + uint32_t *peak_magnitude[] {_sensor_gyro_fft.peak_magnitude_x, _sensor_gyro_fft.peak_magnitude_y, _sensor_gyro_fft.peak_magnitude_z}; int num_peaks_found = 0; @@ -333,6 +334,8 @@ void GyroFFT::Run() } peak_frequencies[axis][num_peaks_found] = freq; + peak_magnitude[axis][num_peaks_found] = peaks_magnitude[i]; + num_peaks_found++; } }