Created
March 2, 2026 04:30
-
-
Save jhyland87/e845211c9577f7470b040a63c2079622 to your computer and use it in GitHub Desktop.
Get the vibration Hz from QMI8658 output
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #include <Arduino.h> | |
| #include <Wire.h> | |
| #include <QMI8658.h> | |
| #include <arduinoFFT.h> | |
| #define SDA_PIN 8 | |
| #define SCL_PIN 9 | |
| // ================= SETTINGS ================= | |
| static constexpr uint16_t N = 256; // FFT size | |
| static constexpr float FS_HZ = 400.0f; // Sampling rate | |
| static constexpr float SEARCH_MIN_HZ = 45.0f; | |
| static constexpr float SEARCH_MAX_HZ = 75.0f; | |
| static constexpr float AMP_ON_G = 0.015f; // vibration threshold | |
| static constexpr float MIN_SNR = 5.0f; // FFT peak must be this much above noise | |
| // ============================================ | |
| // ---------- FFT frequency detection ---------- | |
| struct FFTResult | |
| { | |
| float freq; | |
| float snr; | |
| }; | |
| QMI8658 imu; | |
| float vReal[N]; | |
| float vImag[N]; | |
| ArduinoFFT FFT(vReal, vImag, N, FS_HZ); | |
| static float zBuf[N]; | |
| // ---------- Hann window ---------- | |
| static inline float hann(uint16_t i) | |
| { | |
| return 0.5f * (1.0f - cosf((2.0f * PI * i) / (N - 1))); | |
| } | |
| // ---------- Median helper ---------- | |
| float median(float *arr, uint16_t len) | |
| { | |
| for (uint16_t i = 0; i < len; i++) | |
| { | |
| uint16_t m = i; | |
| for (uint16_t j = i + 1; j < len; j++) | |
| if (arr[j] < arr[m]) m = j; | |
| float t = arr[i]; | |
| arr[i] = arr[m]; | |
| arr[m] = t; | |
| } | |
| return arr[len / 2]; | |
| } | |
| FFTResult detectFrequency(const float *data) | |
| { | |
| float mean = 0; | |
| for (int i = 0; i < N; i++) | |
| mean += data[i]; | |
| mean /= N; | |
| for (int i = 0; i < N; i++) | |
| { | |
| vReal[i] = (data[i] - mean) * hann(i); | |
| vImag[i] = 0; | |
| } | |
| FFT.compute(FFT_FORWARD); | |
| FFT.complexToMagnitude(); | |
| float binHz = FS_HZ / N; | |
| uint16_t kMin = ceil(SEARCH_MIN_HZ / binHz); | |
| uint16_t kMax = floor(SEARCH_MAX_HZ / binHz); | |
| if (kMin < 1) kMin = 1; | |
| if (kMax > (N/2 - 1)) kMax = N/2 - 1; | |
| uint16_t kPeak = kMin; | |
| float peak = vReal[kMin]; | |
| for (uint16_t k = kMin + 1; k <= kMax; k++) | |
| { | |
| if (vReal[k] > peak) | |
| { | |
| peak = vReal[k]; | |
| kPeak = k; | |
| } | |
| } | |
| // estimate noise floor | |
| float mags[64]; | |
| uint16_t count = 0; | |
| for (uint16_t k = kMin; k <= kMax && count < 64; k++) | |
| mags[count++] = vReal[k]; | |
| float noise = median(mags, count); | |
| if (noise < 1e-9) noise = 1e-9; | |
| float snr = peak / noise; | |
| float freq = kPeak * binHz; | |
| // quadratic interpolation | |
| if (kPeak > 1 && kPeak < N/2 - 1) | |
| { | |
| float a = vReal[kPeak - 1]; | |
| float b = vReal[kPeak]; | |
| float c = vReal[kPeak + 1]; | |
| float denom = (a - 2*b + c); | |
| if (fabs(denom) > 1e-9) | |
| { | |
| float delta = 0.5f * (a - c) / denom; | |
| freq = (kPeak + delta) * binHz; | |
| } | |
| } | |
| return {freq, snr}; | |
| } | |
| // ---------- read accel ---------- | |
| bool readAccelZ(float &z) | |
| { | |
| float ax, ay, az; | |
| if (!imu.readAccel(ax, ay, az)) | |
| return false; | |
| z = az / 9.80665f; // convert to g | |
| return true; | |
| } | |
| // ---------- setup ---------- | |
| void setup() | |
| { | |
| Serial.begin(115200); | |
| Wire.begin(SDA_PIN, SCL_PIN); | |
| if (!imu.begin()) | |
| { | |
| Serial.println("QMI8658 failed"); | |
| while (1); | |
| } | |
| imu.setAccelRange(QMI8658_ACCEL_RANGE_8G); | |
| imu.setAccelODR(QMI8658_ACCEL_ODR_1000HZ); | |
| imu.enableSensors(QMI8658_ENABLE_ACCEL); | |
| imu.setAccelUnit_mps2(true); | |
| Serial.println("freq_fft,z_rms,z_peak,snr"); | |
| } | |
| // ---------- loop ---------- | |
| void loop() | |
| { | |
| uint32_t period = 1000000.0f / FS_HZ; | |
| uint32_t tNext = micros(); | |
| float zMin = 999; | |
| float zMax = -999; | |
| float sumSq = 0; | |
| for (int i = 0; i < N; i++) | |
| { | |
| while ((int32_t)(micros() - tNext) < 0); | |
| tNext += period; | |
| float z; | |
| if (!readAccelZ(z)) | |
| z = (i > 0) ? zBuf[i - 1] : 0; | |
| zBuf[i] = z; | |
| if (z < zMin) zMin = z; | |
| if (z > zMax) zMax = z; | |
| sumSq += z * z; | |
| } | |
| float zRms = sqrt(sumSq / N); | |
| float zPeak = (zMax - zMin) * 0.5f; | |
| FFTResult result = detectFrequency(zBuf); | |
| bool signalPresent = zPeak > AMP_ON_G; | |
| bool valid = signalPresent && result.snr > MIN_SNR; | |
| static float fFiltered = NAN; | |
| if (valid) | |
| { | |
| if (!isfinite(fFiltered)) | |
| fFiltered = result.freq; | |
| fFiltered = 0.85f * fFiltered + 0.15f * result.freq; | |
| } | |
| else | |
| { | |
| fFiltered = NAN; | |
| } | |
| if (isnan(fFiltered)) | |
| Serial.printf("Hz:%-12.3f", 0.0f); | |
| else | |
| Serial.printf("Hz:%-12.3f", fFiltered); | |
| Serial.printf("rms:%-12.3f", zRms); | |
| Serial.printf("peak:%-12.3f", zPeak); | |
| Serial.printf("snr:%-12.3f", result.snr); | |
| Serial.println(); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment