Skip to content

Commit

Permalink
Update bbn_wave_freq_m5atomS3.ino
Browse files Browse the repository at this point in the history 
estimate freq trochoidal
  • Loading branch information
@mgrouch
mgrouch authored Sep 9, 2024
1 parent 668a242 commit 195a21c
Showing 1 changed file with 20 additions and 344 deletions.
364 changes: 20 additions & 344 deletions bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
View file
Original file line number Diff line number Diff line change
@@ -1,356 +1,32 @@
/*
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 <M5Unified.h>
#include <M5AtomS3.h>
#include <Arduino.h>
#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;
};

static constexpr const float coefficient_tbl[3] = { 0.5f, (1.0f / 256.0f), (1.0f / 1024.0f) };
static uint8_t calib_countdown = 0;
#ifndef TrochoidalWave_h
#define TrochoidalWave_h

static auto &disp = (M5.Display);
static rect_t rect_text_area;

static int prev_xpos[18];

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;
MinMaxLemire min_max_a;
AranovskiyParams params;
AranovskiyState state;
KalmanSmootherVars kalman_freq;
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);
See https://bareboat-necessities.github.io/my-bareboat/bareboat-math.html
*/

// If you want to stop all calibration, write this.
// M5.Imu.setCalibration(0, 0, 0);
float trochoid_wave_length(float periodSec);
float trochoid_wave_period(float height, float accel);
float trochoid_wave_freq(float height, float accel);

// save calibration values.
M5.Imu.saveOffsetToNVS();
}
}
disp.fillRect(0, 0, rect_text_area.w, rect_text_area.h, TFT_BLACK);
const float g_std = 9.80665; // standard gravity acceleration m/s2

if (c) {
disp.setCursor(2, rect_text_area.h + 1);
disp.setTextColor(TFT_WHITE, TFT_BLACK);
disp.printf("Countdown:%3d", c);
}
float trochoid_wave_length(float periodSec) {
float lengthMeters = g_std * periodSec * periodSec / (2 * PI);
return lengthMeters;
}

void startCalibration(void) {
updateCalibration(30, true);
float trochoid_wave_period(float height, float accel) {
float wave_period = 2.0 * PI * sqrt(fabs(height / accel));
return wave_period;
}

int produce_serial_data = 1;

float heave_bias = 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(&params, &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);

float period_trochoid = 0.0;
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);
}
{
SampleType sample;
sample.timeMicroSec = now;
sample.value = a * g_std;
min_max_lemire_update(&min_max_a, sample, windowMicros);
}
}

float wave_height = min_max_h.max.value - min_max_h.min.value;
heave_bias = (min_max_h.max.value + min_max_h.min.value) / 2.0;

float amp_range = min_max_a.max.value - min_max_a.min.value;

if (amp_range > 0.0001) {
period_trochoid = trochoid_wave_period(wave_height, amp_range);
}

float wave_length = trochoid_wave_length(period_trochoid);
float heave_trochoid = - a * wave_length / (2 * PI); // in trochoid model

if (now - last_refresh >= (produce_serial_data ? 200000 : 1000000)) {
if (produce_serial_data) {
Serial.printf("heave_cm:%.4f", heave * 100);
Serial.printf(",heave_trochoid:%.4f", heave_trochoid * 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_bias_cm:%.4f", heave_bias * 100);
//Serial.printf(",freq:%.4f", freq * 100);
//Serial.printf(",freq_adj:%.4f", freq_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);
}
}
float trochoid_wave_freq(float height, float accel) {
float wave_freq = sqrt(fabs(accel / height)) / (2.0 * PI);
return wave_freq;
}

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);

/*
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(&params, 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);
}

kalman_wave_init_defaults();

last_update = micros();
}

void loop(void) {
AtomS3.update();
delay(3);
repeatMe();
}
#endif

0 comments on commit 195a21c

Please sign in to comment.