gyro_fft improve peak finding, parameterize min/max frequencies, remove debug logging

- add min/max frequency parameters for peak detection (IMU_GYRO_FFT_MIN, IMU_GYRO_FFT_MAX)
 - remove full FFT debug logging
 - fix Quinn's second estimator
 - log sensor_gyro_fft
 - fake_gyro use PX4Gyroscope

msg/CMakeLists.txt

@@ -120,7 +120,6 @@ set(msg_files
 	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
-	sensor_gyro_fft_axis.msg
 	sensor_gyro_fifo.msg
 	sensor_mag.msg
 	sensor_preflight_mag.msg

msg/sensor_gyro_fft.msg

@@ -4,12 +4,14 @@ uint64 timestamp_sample
 uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
 float32 dt                # delta time between samples (microseconds)
-float32 scale
 
-uint8 samples             # number of valid samples
+float32 resolution_hz
 
-uint8 peak_index
+uint8[3] peak_index_0
+uint8[3] peak_index_1
 
-float32 peak_frequency_0
-float32 peak_frequency_1
-float32 peak_frequency_2
+float32[3] peak_frequency_0 # largest frequency peak found per sensor axis (0 if none)
+float32[3] peak_frequency_1 # second largest frequency peak found per sensor axis (0 if none)
+
+uint8[3] peak_index_quinns
+float32[3] peak_frequency_quinns

msg/sensor_gyro_fft_axis.msg

@@ -1,23 +0,0 @@
-uint64 timestamp          # time since system start (microseconds)
-uint64 timestamp_sample
-
-uint32 device_id          # unique device ID for the sensor that does not change between power cycles
-
-float32 dt                # delta time between samples (microseconds)
-
-uint16 samples             # number of valid samples
-
-float32 resolution_hz
-
-int16[128] fft
-
-uint16 peak_index
-uint16 peak_index_quinns
-
-float32 peak_frequency
-float32 peak_frequency_quinns
-
-uint8 AXIS_X = 0
-uint8 AXIS_Y = 1
-uint8 AXIS_Z = 2
-uint8 axis

msg/tools/uorb_rtps_message_ids.yaml

@@ -287,8 +287,6 @@ rtps:
     id: 136
   - msg: sensor_gyro_fft
     id: 137
-  - msg: sensor_gyro_fft_axis
-    id: 138
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

src/examples/fake_gyro/CMakeLists.txt

@@ -40,5 +40,6 @@ px4_add_module(
 		FakeGyro.cpp
 		FakeGyro.hpp
 	DEPENDS
+		drivers_gyroscope
 		px4_work_queue
 )

src/examples/fake_gyro/FakeGyro.cpp

@@ -37,8 +37,10 @@ using namespace time_literals;
 
 FakeGyro::FakeGyro() :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	_px4_gyro(1310988) // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 {
+	_px4_gyro.set_scale(math::radians(2000.f) / static_cast<float>(INT16_MAX - 1)); // 2000 degrees/second max
 }
 
 bool FakeGyro::init()
@@ -55,29 +57,26 @@ void FakeGyro::Run()
 		return;
 	}
 
-	sensor_gyro_fifo_s sensor_gyro_fifo{};
-	sensor_gyro_fifo.timestamp_sample = hrt_absolute_time();
-	sensor_gyro_fifo.device_id = 1;
-	sensor_gyro_fifo.samples = GYRO_RATE / (1e6f / SENSOR_RATE);
-	sensor_gyro_fifo.dt = 1e6f / GYRO_RATE; // 8 kHz fake gyro
-	sensor_gyro_fifo.scale = math::radians(2000.f) / static_cast<float>(INT16_MAX - 1); // 2000 degrees/second max
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = hrt_absolute_time();
+	gyro.samples = GYRO_RATE / (1e6f / SENSOR_RATE);
+	gyro.dt = 1e6f / GYRO_RATE; // 8 kHz fake gyro;
 
-	const float dt_s = sensor_gyro_fifo.dt / 1e6f;
-	const float x_freq = 15.f;  // 15 Hz x frequency
-	const float y_freq = 63.5f; // 63.5 Hz y frequency
-	const float z_freq = 99.f;  // 99 Hz z frequency
+	const float dt_s = gyro.dt * 1e-6f;
+	const float x_freq = 15.f;  //  15,0 Hz X frequency
+	const float y_freq = 63.5f; //  63.5 Hz Y frequency
+	const float z_freq = 135.f; // 135.0 Hz Z frequency
 
-	for (int n = 0; n < sensor_gyro_fifo.samples; n++) {
+	for (int n = 0; n < gyro.samples; n++) {
 		_time += dt_s;
 		const float k = 2.f * M_PI_F * _time;
 
-		sensor_gyro_fifo.x[n] = (INT16_MAX - 1) * sinf(k * x_freq);
-		sensor_gyro_fifo.y[n] = (INT16_MAX - 1) / 2 * sinf(k * y_freq);
-		sensor_gyro_fifo.z[n] = (INT16_MAX - 1) * cosf(k * z_freq);
+		gyro.x[n] = (INT16_MAX - 1) * sinf(k * x_freq);
+		gyro.y[n] = (INT16_MAX - 1) / 2 * sinf(k * y_freq);
+		gyro.z[n] = (INT16_MAX - 1) * cosf(k * z_freq);
 	}
 
-	sensor_gyro_fifo.timestamp = hrt_absolute_time();
-	_sensor_gyro_fifo_pub.publish(sensor_gyro_fifo);
+	_px4_gyro.updateFIFO(gyro);
 }
 
 int FakeGyro::task_spawn(int argc, char *argv[])

src/examples/fake_gyro/FakeGyro.hpp

@@ -38,6 +38,7 @@
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_gyro_fifo.h>
@@ -65,7 +66,7 @@ private:
 
 	void Run() override;
 
-	uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_gyro_fifo_pub{ORB_ID(sensor_gyro_fifo)};
+	PX4Gyroscope _px4_gyro;
 
 	float _time{0.f};
 };

src/examples/gyro_fft/GyroFFT.cpp

@@ -53,7 +53,7 @@ GyroFFT::GyroFFT() :
 	float hanning_window[FFT_LENGTH];
 
 	for (int n = 0; n < FFT_LENGTH; n++) {
-		hanning_window[n] = 0.5f - 0.5f * cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1));
+		hanning_window[n] = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1)));
 	}
 
 	arm_float_to_q15(hanning_window, _hanning_window, FFT_LENGTH);
@@ -91,6 +91,21 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
 				    && (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) {
 
 					if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) {
+						// find corresponding vehicle_imu_status instance
+						for (uint8_t imu_status = 0; imu_status < MAX_SENSOR_COUNT; imu_status++) {
+							uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_status};
+
+							vehicle_imu_status_s vehicle_imu_status;
+
+							if (imu_status_sub.copy(&vehicle_imu_status)) {
+								if (vehicle_imu_status.gyro_device_id == sensor_selection.gyro_device_id) {
+									_vehicle_imu_status_sub.ChangeInstance(imu_status);
+									return true;
+								}
+							}
+						}
+
+						PX4_WARN("unable to find IMU status for gyro %d", sensor_selection.gyro_device_id);
 						return true;
 					}
 				}
@@ -103,6 +118,17 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
 	return false;
 }
 
+void GyroFFT::VehicleIMUStatusUpdate()
+{
+	vehicle_imu_status_s vehicle_imu_status;
+
+	if (_vehicle_imu_status_sub.update(&vehicle_imu_status)) {
+		if ((vehicle_imu_status.gyro_rate_hz > 0) && (fabsf(vehicle_imu_status.gyro_rate_hz - _gyro_sample_rate_hz) > 1.f)) {
+			_gyro_sample_rate_hz = vehicle_imu_status.gyro_rate_hz;
+		}
+	}
+}
+
 // helper function used for frequency estimation
 static constexpr float tau(float x)
 {
@@ -138,6 +164,10 @@ void GyroFFT::Run()
 
 	SensorSelectionUpdate();
 
+	const float resolution_hz = _gyro_sample_rate_hz / (FFT_LENGTH * 2);
+
+	bool publish = false;
+
 	// run on sensor gyro fifo updates
 	sensor_gyro_fifo_s sensor_gyro_fifo;
 
@@ -189,60 +219,122 @@ void GyroFFT::Run()
 					arm_rfft_q15(&_rfft_q15[axis], _fft_input_buffer, _fft_outupt_buffer);
 					perf_end(_fft_perf);
 
+					static constexpr uint16_t MIN_SNR = 100; // TODO:
+					uint32_t max_peak_0 = 0;
+					uint8_t max_peak_index_0 = 0;
+					bool peak_0_found = false;
+
+					// start at 2 to skip DC
+					// output is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1]
+					for (uint16_t bucket_index = 2; bucket_index < FFT_LENGTH; bucket_index = bucket_index + 2) {
+						const float freq_hz = bucket_index * resolution_hz;
+
+						if (freq_hz > _param_imu_gyro_fft_max.get()) {
+							break;
+						}
 
-					// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
-					int16_t max_peak = 0;
-					uint16_t max_peak_index = 0;
+						if (freq_hz >= _param_imu_gyro_fft_min.get()) {
+							const int16_t real = _fft_outupt_buffer[bucket_index];
+							const int16_t complex = _fft_outupt_buffer[bucket_index + 1];
+							const uint32_t fft_value_squared = real * real + complex * complex;
 
-					// start at 1 to skip DC
-					for (uint16_t bucket_index = 1; bucket_index < FFT_LENGTH; bucket_index++) {
-						if (abs(_fft_outupt_buffer[bucket_index]) >= max_peak) {
-							max_peak_index = bucket_index;
-							max_peak = abs(_fft_outupt_buffer[bucket_index]);
+							if ((fft_value_squared > MIN_SNR) && (fft_value_squared >= max_peak_0)) {
+								max_peak_index_0 = bucket_index;
+								max_peak_0 = fft_value_squared;
+								peak_0_found = true;
+							}
 						}
 					}
 
+					if (peak_0_found) {
+						{
+							// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
+							int16_t real[3] {_fft_outupt_buffer[max_peak_index_0 - 2], _fft_outupt_buffer[max_peak_index_0], _fft_outupt_buffer[max_peak_index_0 + 2]};
+							int16_t imag[3] {_fft_outupt_buffer[max_peak_index_0 - 2 + 1], _fft_outupt_buffer[max_peak_index_0 + 1], _fft_outupt_buffer[max_peak_index_0 + 2 + 1]};
 
-					int k = max_peak_index;
-					float divider = powf(_fft_outupt_buffer[k], 2.f);
-					float ap = (_fft_outupt_buffer[k + 1] * _fft_outupt_buffer[k]) / divider;
-					float dp = -ap  / (1.f - ap);
-					float am = (_fft_outupt_buffer[k - 1] * _fft_outupt_buffer[k]) / divider;
+							const int k = 1;
 
-					float dm = am / (1.f - am);
-					float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm);
+							float divider = (real[k] * real[k] + imag[k] * imag[k]);
 
-					float adjustedBinLocation = k + d;
-					float peakFreqAdjusted = (_gyro_sample_rate * adjustedBinLocation / (FFT_LENGTH * 2));
+							// ap = (X[k + 1].r * X[k].r + X[k+1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
+							float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider;
 
+							// am = (X[k – 1].r * X[k].r + X[k – 1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
+							float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider;
 
-					// publish sensor_gyro_fft_axis (one instance per axis)
-					sensor_gyro_fft_axis_s sensor_gyro_fft_axis{};
-					const int N_publish = math::min((unsigned)FFT_LENGTH,
-									sizeof(sensor_gyro_fft_axis_s::fft) / sizeof(sensor_gyro_fft_axis_s::fft[0]));
-					memcpy(sensor_gyro_fft_axis.fft, _fft_outupt_buffer, N_publish);
+							float dp = -ap  / (1.f - ap);
+							float dm = am / (1.f - am);
+							float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm);
 
-					sensor_gyro_fft_axis.resolution_hz = _gyro_sample_rate / (FFT_LENGTH * 2);
+							uint8_t adjustedBinLocation = roundf(max_peak_index_0 + d);
+							float peakFreqAdjusted = (_gyro_sample_rate_hz * adjustedBinLocation / (FFT_LENGTH * 2));
 
-					sensor_gyro_fft_axis.peak_index = max_peak_index;
-					sensor_gyro_fft_axis.peak_frequency = max_peak_index * sensor_gyro_fft_axis.resolution_hz;
+							_sensor_gyro_fft.peak_index_quinns[axis] = adjustedBinLocation;
+							_sensor_gyro_fft.peak_frequency_quinns[axis] = peakFreqAdjusted;
+						}
+
+
+						// find next peak
+						uint32_t max_peak_1 = 0;
+						uint8_t max_peak_index_1 = 0;
+						bool peak_1_found = false;
+
+						for (uint16_t bucket_index = 2; bucket_index < FFT_LENGTH; bucket_index = bucket_index + 2) {
+							if (bucket_index != max_peak_index_0) {
+								const float freq_hz = bucket_index * resolution_hz;
 
-					sensor_gyro_fft_axis.peak_index_quinns = adjustedBinLocation;
-					sensor_gyro_fft_axis.peak_frequency_quinns = peakFreqAdjusted;
+								if (freq_hz > _param_imu_gyro_fft_max.get()) {
+									break;
+								}
 
-					sensor_gyro_fft_axis.samples = FFT_LENGTH;
-					sensor_gyro_fft_axis.dt = 1e6f / _gyro_sample_rate;
-					sensor_gyro_fft_axis.device_id = sensor_gyro_fifo.device_id;
-					sensor_gyro_fft_axis.axis = axis;
-					sensor_gyro_fft_axis.timestamp_sample = sensor_gyro_fifo.timestamp_sample;
-					sensor_gyro_fft_axis.timestamp = hrt_absolute_time();
-					_sensor_gyro_fft_axis_pub[axis].publish(sensor_gyro_fft_axis);
+								if (freq_hz >= _param_imu_gyro_fft_min.get()) {
+									const int16_t real = _fft_outupt_buffer[bucket_index];
+									const int16_t complex = _fft_outupt_buffer[bucket_index + 1];
+									const uint32_t fft_value_squared = real * real + complex * complex;
+
+									if ((fft_value_squared > MIN_SNR) && (fft_value_squared >= max_peak_1)) {
+										max_peak_index_1 = bucket_index;
+										max_peak_1 = fft_value_squared;
+										peak_1_found = true;
+									}
+								}
+							}
+						}
+
+						if (peak_1_found) {
+							// if 2 peaks found then log them in order
+							_sensor_gyro_fft.peak_index_0[axis] = math::min(max_peak_index_0, max_peak_index_1);
+							_sensor_gyro_fft.peak_index_1[axis] = math::max(max_peak_index_0, max_peak_index_1);
+							_sensor_gyro_fft.peak_frequency_0[axis] = _sensor_gyro_fft.peak_index_0[axis] * resolution_hz;
+							_sensor_gyro_fft.peak_frequency_1[axis] = _sensor_gyro_fft.peak_index_1[axis] * resolution_hz;
+
+						} else {
+							// only 1 peak found
+							_sensor_gyro_fft.peak_index_0[axis] = max_peak_index_0;
+							_sensor_gyro_fft.peak_index_1[axis] = 0;
+							_sensor_gyro_fft.peak_frequency_0[axis] = max_peak_index_0 * resolution_hz;
+							_sensor_gyro_fft.peak_frequency_1[axis] = 0;
+						}
+
+						publish = true;
+					}
 
 					// reset
 					buffer_index = 0;
 				}
 			}
 		}
+
+		if (publish) {
+			_sensor_gyro_fft.dt = 1e6f / _gyro_sample_rate_hz;
+			_sensor_gyro_fft.device_id = sensor_gyro_fifo.device_id;
+			_sensor_gyro_fft.resolution_hz = resolution_hz;
+			_sensor_gyro_fft.timestamp_sample = sensor_gyro_fifo.timestamp_sample;
+			_sensor_gyro_fft.timestamp = hrt_absolute_time();
+			_sensor_gyro_fft_pub.publish(_sensor_gyro_fft);
+
+			publish = false;
+		}
 	}
 
 	perf_end(_cycle_perf);

src/examples/gyro_fft/GyroFFT.hpp

@@ -46,9 +46,9 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gyro_fft.h>
-#include <uORB/topics/sensor_gyro_fft_axis.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/vehicle_imu_status.h>
 
 #include "arm_math.h"
 #include "arm_const_structs.h"
@@ -76,18 +76,15 @@ public:
 private:
 	void Run() override;
 	bool SensorSelectionUpdate(bool force = false);
+	void VehicleIMUStatusUpdate();
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
 	uORB::Publication<sensor_gyro_fft_s> _sensor_gyro_fft_pub{ORB_ID(sensor_gyro_fft)};
-	uORB::Publication<sensor_gyro_fft_axis_s> _sensor_gyro_fft_axis_pub[3] {
-		ORB_ID(sensor_gyro_fft_axis),
-		ORB_ID(sensor_gyro_fft_axis),
-		ORB_ID(sensor_gyro_fft_axis),
-	};
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
+	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
 
@@ -106,9 +103,16 @@ private:
 	q15_t _fft_input_buffer[FFT_LENGTH] {};
 	q15_t _fft_outupt_buffer[FFT_LENGTH * 2] {};
 
-	float _gyro_sample_rate{8000}; // 8 kHz default
+	float _gyro_sample_rate_hz{8000}; // 8 kHz default
 
 	int _fft_buffer_index[3] {};
 
 	unsigned _gyro_last_generation{0};
+
+	sensor_gyro_fft_s _sensor_gyro_fft{};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::IMU_GYRO_FFT_MIN>) _param_imu_gyro_fft_min,
+		(ParamFloat<px4::params::IMU_GYRO_FFT_MAX>) _param_imu_gyro_fft_max
+	)
 };

src/examples/gyro_fft/parameters.c

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* IMU gyro FFT minimum frequency.
+*
+* @min 1
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 30.0f);
+
+/**
+* IMU gyro FFT maximum frequency.
+*
+* @min 1
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 200.0f);

src/modules/logger/logged_topics.cpp

@@ -80,6 +80,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("safety");
 	add_topic("sensor_combined");
 	add_topic("sensor_correction");
+	add_topic("sensor_gyro_fft");
 	add_topic("sensor_preflight_mag", 500);
 	add_topic("sensor_selection");
 	add_topic("sensors_status_imu", 200);