diff --git a/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino b/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino index e81d978..1ce64a4 100644 --- a/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino +++ b/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino @@ -1,32 +1,372 @@ -#ifndef TrochoidalWave_h -#define TrochoidalWave_h - /* + Copyright 2024, Mikhail Grushinskiy + + Estimate vessel heave (vertical displacement) in ocean waves using IMU on esp32 (m5atomS3) + + See: https://bareboat-necessities.github.io/my-bareboat/bareboat-math.html + + Instead of FFT method for finding main wave frequency we could use Aranovskiy frequency estimator which is a simple on-line filter. + + Ref: + + Alexey A. Bobtsov, Nikolay A. Nikolaev, Olga V. Slita, Alexander S. Borgul, Stanislav V. Aranovskiy + + The New Algorithm of Sinusoidal Signal Frequency Estimation. + + 11th IFAC International Workshop on Adaptation and Learning in Control and Signal Processing July 3-5, 2013. Caen, France + +*/ + +#include +#include +#include +#include "AranovskiyFilter.h" +#include "KalmanSmoother.h" +#include "TrochoidalWave.h" +#include "Mahony_AHRS.h" +#include "Quaternion.h" +#include "MinMaxLemire.h" +#include "KalmanForWave.h" + +// Strength of the calibration operation; +// 0: disables calibration. +// 1 is weakest and 255 is strongest. +static constexpr const uint8_t calib_value = 64; + +// This sample code performs calibration by clicking on a button or screen. +// After 10 seconds of calibration, the results are stored in NVS. +// The saved calibration values are loaded at the next startup. +// +// === How to calibration === +// ※ Calibration method for Accelerometer +// Change the direction of the main unit by 90 degrees +// and hold it still for 2 seconds. Repeat multiple times. +// It is recommended that as many surfaces as possible be on the bottom. +// +// ※ Calibration method for Gyro +// Simply place the unit on a quiet desk and hold it still. +// It is recommended that this be done after the accelerometer calibration. +// +// ※ Calibration method for geomagnetic sensors +// Rotate the main unit slowly in multiple directions. +// It is recommended that as many surfaces as possible be oriented to the north. +// +// Values for extremely large attitude changes are ignored. +// During calibration, it is desirable to move the device as gently as possible. + +struct rect_t { + int32_t x; + int32_t y; + int32_t w; + int32_t h; +}; - See https://bareboat-necessities.github.io/my-bareboat/bareboat-math.html - - */ +static constexpr const float coefficient_tbl[3] = { 0.5f, (1.0f / 256.0f), (1.0f / 1024.0f) }; +static uint8_t calib_countdown = 0; -float trochoid_wave_length(float periodSec); -float trochoid_wave_period(float height, float accel); -float trochoid_wave_freq(float height, float accel); +static auto &disp = (M5.Display); +static rect_t rect_text_area; -const float g_std = 9.80665; // standard gravity acceleration m/s2 +static int prev_xpos[18]; -float trochoid_wave_length(float periodSec) { - float lengthMeters = g_std * periodSec * periodSec / (2 * PI); - return lengthMeters; +unsigned long now = 0UL, last_refresh = 0UL, last_update = 0UL; +unsigned long got_samples = 0; +int first = 1; + +float delta_t; // time step sec + +MinMaxLemire min_max_h; +AranovskiyParams params; +AranovskiyState state; +KalmanSmootherVars kalman_freq; +KalmanSmootherVars kalman_freq_tr; +KalmanSmootherVars kalman_heave_tr; +Mahony_AHRS_Vars mahony; +KalmanWaveState waveState; + +const char* imu_name; + +void updateCalibration(uint32_t c, bool clear = false) { + calib_countdown = c; + + if (c == 0) { + clear = true; + } + + if (clear) { + memset(prev_xpos, 0, sizeof(prev_xpos)); + disp.fillScreen(TFT_BLACK); + if (c) { + // Start calibration. + M5.Imu.setCalibration(calib_value, calib_value, calib_value); + // The actual calibration operation is performed each time during M5.Imu.update. + // + // There are three arguments, which can be specified in the order of Accelerometer, gyro, and geomagnetic. + // If you want to calibrate only the Accelerometer, do the following. + // M5.Imu.setCalibration(100, 0, 0); + // + // If you want to calibrate only the gyro, do the following. + // M5.Imu.setCalibration(0, 100, 0); + // + // If you want to calibrate only the geomagnetism, do the following. + // M5.Imu.setCalibration(0, 0, 100); + } + else { + // Stop calibration. (Continue calibration only for the geomagnetic sensor) + M5.Imu.setCalibration(0, 0, calib_value); + + // If you want to stop all calibration, write this. + // M5.Imu.setCalibration(0, 0, 0); + + // save calibration values. + M5.Imu.saveOffsetToNVS(); + } + } + disp.fillRect(0, 0, rect_text_area.w, rect_text_area.h, TFT_BLACK); + + if (c) { + disp.setCursor(2, rect_text_area.h + 1); + disp.setTextColor(TFT_WHITE, TFT_BLACK); + disp.printf("Countdown:%3d", c); + } } -float trochoid_wave_period(float height, float accel) { - float wave_period = 2.0 * PI * sqrt(fabs(height / accel)); - return wave_period; +void startCalibration(void) { + updateCalibration(30, true); } -float trochoid_wave_freq(float height, float accel) { - float wave_freq = sqrt(fabs(accel / height)) / (2.0 * PI); - return wave_freq; +int produce_serial_data = 1; + +float heave_avg = 0.0; +float wave_length = 0.0; +float heave_trochoid = 0.0; +float heave_trochoid_adj = 0.0; +float period_trochoid = 0.0; +float wave_freq_trochoid = 0.0; +float wave_freq_trochoid_adj = 0.0; + +void repeatMe() { + static uint32_t prev_sec = 0; + + auto imu_update = M5.Imu.update(); + if (imu_update) { + m5::imu_3d_t accel; + M5.Imu.getAccelData(&accel.x, &accel.y, &accel.z); + + m5::imu_3d_t gyro; + M5.Imu.getGyroData(&gyro.x, &gyro.y, &gyro.z); + + got_samples++; + + now = micros(); + delta_t = ((now - last_update) / 1000000.0); + last_update = now; + + float pitch, roll, yaw; + mahony_AHRS_update(&mahony, gyro.x * DEG_TO_RAD, gyro.y * DEG_TO_RAD, gyro.z * DEG_TO_RAD, + accel.x, accel.y, accel.z, &pitch, &roll, &yaw, delta_t); + + Quaternion quaternion; + Quaternion_set(mahony.q0, mahony.q1, mahony.q2, mahony.q3, &quaternion); + float v[3] = {accel.x, accel.y, accel.z}; + float rotated_a[3]; + Quaternion_rotate(&quaternion, v, rotated_a); + + m5::imu_3d_t accel_rotated; + accel_rotated.x = rotated_a[0]; + accel_rotated.y = rotated_a[1]; + accel_rotated.z = rotated_a[2]; + + float a = (accel_rotated.z - 1.0); // acceleration in fractions of g + //float a = - 0.25 * PI * PI * sin(2 * PI * state.t * 0.25) / g_std; // dummy test data (amplitude of heave = 1m, 4sec - period) + + kalman_wave_step(&waveState, a * g_std, delta_t); + float heave = waveState.heave; // in meters + + float y = heave; + aranovskiy_update(¶ms, &state, y, delta_t); + float freq = state.f; + + if (first) { + kalman_smoother_set_initial(&kalman_freq, freq); + first = 0; + } + float freq_adj = kalman_smoother_update(&kalman_freq, freq); + + if (freq_adj > 0.001 && freq_adj < 1000.0) { + float period = 1.0 / freq_adj; + if (period < 30.0) { + uint32_t windowMicros = 3 * period * 1000000; + if (period_trochoid > 0.0001) { + windowMicros = 3 * period_trochoid * 1000000; + } + if (windowMicros <= 10 * 1000000) { + windowMicros = 10 * 1000000; + } + SampleType sample; + sample.timeMicroSec = now; + sample.value = heave; + min_max_lemire_update(&min_max_h, sample, windowMicros); + } + + float wave_height = min_max_h.max.value - min_max_h.min.value; + heave_avg = (min_max_h.max.value + min_max_h.min.value) / 2.0; + + if (fabs(heave) > 0.0001 && fabs(a) > 0.0001) { + wave_freq_trochoid = trochoid_wave_freq(heave, a * g_std); + wave_freq_trochoid_adj = kalman_smoother_update(&kalman_freq_tr, wave_freq_trochoid); + if (fabs(wave_freq_trochoid_adj) > 0.0001) { + period_trochoid = 1.0 / wave_freq_trochoid_adj; + } + } + if (fabs(period_trochoid) > 0.0001) { + wave_length = trochoid_wave_length(period_trochoid); + heave_trochoid = - a * wave_length / (2 * PI); // in trochoid model + heave_trochoid_adj = kalman_smoother_update(&kalman_heave_tr, heave_trochoid); + } + + if (now - last_refresh >= (produce_serial_data ? 200000 : 1000000)) { + if (produce_serial_data) { + Serial.printf("heave_cm:%.4f", heave * 100); + Serial.printf(",heave_trochoid_adj:%.4f", heave_trochoid_adj * 100); + //Serial.printf(",height_cm:%.4f", wave_height * 100); + //Serial.printf(",max_cm:%.4f", min_max_h.max.value * 100); + //Serial.printf(",min_cm:%.4f", min_max_h.min.value * 100); + //Serial.printf(",heave_avg_cm:%.4f", heave_avg * 100); + //Serial.printf(",freq:%.4f", freq * 100); + //Serial.printf(",freq_adj:%.4f", freq_adj * 100); + //Serial.printf(",wave_freq_trochoid:%.4f", wave_freq_trochoid * 100); + //Serial.printf(",wave_freq_trochoid_adj:%.4f", wave_freq_trochoid_adj * 100); + //Serial.printf(",period_decisec:%.4f", period * 10); + //Serial.printf(",period_trochoid_decisec:%.4f", period_trochoid * 10); + Serial.println(); + } + else { + disp.fillRect(0, 0, rect_text_area.w, rect_text_area.h, TFT_BLACK); + M5.Lcd.setCursor(0, 2); + M5.Lcd.printf("imu: %s\n", imu_name); + M5.Lcd.printf("sec: %d\n", now / 1000000); + M5.Lcd.printf("samples: %d\n", got_samples); + M5.Lcd.printf("period sec: %0.4f\n", period); + M5.Lcd.printf("wave len: %0.4f\n", wave_length); + M5.Lcd.printf("heave: %0.4f\n", heave); + M5.Lcd.printf("wave height:%0.4f\n", wave_height); + M5.Lcd.printf("range %0.4f %0.4f\n", min_max_h.min.value, min_max_h.max.value); + M5.Lcd.printf("%0.3f %0.3f %0.3f\n", accel.x, accel.y, accel.z); + M5.Lcd.printf("accel abs: %0.4f\n", sqrt(accel.x * accel.x + accel.y * accel.y + accel.z * accel.z)); + M5.Lcd.printf("accel vert: %0.4f\n", (accel_rotated.z - 1.0)); + M5.Lcd.printf("%0.1f %0.1f %0.1f\n", pitch, roll, yaw); + } + + last_refresh = now; + got_samples = 0; + } + } + } + else { + AtomS3.update(); + // Calibration is initiated when screen is clicked. Screen on atomS3 is a button + if (AtomS3.BtnA.isPressed()) { + startCalibration(); + } + } + int32_t sec = millis() / 1000; + if (prev_sec != sec) { + prev_sec = sec; + if (calib_countdown) { + updateCalibration(calib_countdown - 1); + } + if ((sec & 7) == 0) { + // prevent WDT. + vTaskDelay(1); + } + } } +void setup(void) { + auto cfg = M5.config(); + AtomS3.begin(cfg); + Serial.begin(115200); + + auto imu_type = M5.Imu.getType(); + switch (imu_type) { + case m5::imu_none: imu_name = "not found"; break; + case m5::imu_sh200q: imu_name = "sh200q"; break; + case m5::imu_mpu6050: imu_name = "mpu6050"; break; + case m5::imu_mpu6886: imu_name = "mpu6886"; break; + case m5::imu_mpu9250: imu_name = "mpu9250"; break; + case m5::imu_bmi270: imu_name = "bmi270"; break; + default: imu_name = "unknown"; break; + }; + disp.fillRect(0, 0, rect_text_area.w, rect_text_area.h, TFT_BLACK); + M5.Lcd.setCursor(0, 2); + M5.Lcd.printf("imu: %s\n", imu_name); + + if (imu_type == m5::imu_none) { + for (;;) { + delay(1); + } + } + + int32_t w = disp.width(); + int32_t h = disp.height(); + if (w < h) { + disp.setRotation(disp.getRotation() ^ 1); + w = disp.width(); + h = disp.height(); + } + int32_t text_area_h = ((h - 8) / 18) * 18; + rect_text_area = {0, 0, w, text_area_h }; + + // Read calibration values from NVS. + if (!M5.Imu.loadOffsetFromNVS()) { + startCalibration(); + } + + float twoKp = (2.0f * 1.0f); + float twoKi = (2.0f * 0.0001f); + mahony_AHRS_init(&mahony, twoKp, twoKi); -#endif + /* + Accelerometer bias creates heave bias and Aranovskiy filter gives + lower frequency (i. e. higher period). + Even 2cm bias in heave is too much to affect frequency a lot + */ + float omega_init = 0.04 * (2 * PI); // init frequency Hz * 2 * PI (start converging from omega_init/2) + float k_gain = 200.0; // Aranovskiy gain. Higher value will give faster convergence, but too high will overflow float + float t_0 = 0.0; + float x1_0 = 0.0; + float theta_0 = - (omega_init * omega_init / 4.0); + float sigma_0 = theta_0; + aranovskiy_default_params(¶ms, omega_init, k_gain); + aranovskiy_init_state(&state, t_0, x1_0, theta_0, sigma_0); + + { + float process_noise_covariance = 0.1; + float measurement_uncertainty = 2.0; + float estimation_uncertainty = 100.0; + kalman_smoother_init(&kalman_freq, process_noise_covariance, measurement_uncertainty, estimation_uncertainty); + } + { + float process_noise_covariance = 0.01; + float measurement_uncertainty = 2.0; + float estimation_uncertainty = 100.0; + kalman_smoother_init(&kalman_freq_tr, process_noise_covariance, measurement_uncertainty, estimation_uncertainty); + } + { + float process_noise_covariance = 0.2; + float measurement_uncertainty = 2.0; + float estimation_uncertainty = 100.0; + kalman_smoother_init(&kalman_heave_tr, process_noise_covariance, measurement_uncertainty, estimation_uncertainty); + } + + kalman_wave_init_defaults(); + + last_update = micros(); +} + +void loop(void) { + AtomS3.update(); + delay(3); + repeatMe(); +}