From edebd8837bdda098d718b09190e899c50f5dbbd4 Mon Sep 17 00:00:00 2001
From: Mikhail Grushinskiy <mgrouch@users.noreply.github.com>
Date: Tue, 10 Sep 2024 19:09:24 -0400
Subject: [PATCH] Update bbn_wave_freq_m5atomS3.ino

---
 .../bbn_wave_freq_m5atomS3.ino                | 24 +++++++++++--------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino b/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
index b1e80c4..6580ff1 100644
--- a/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
+++ b/bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino
@@ -136,6 +136,7 @@ void startCalibration(void) {
 
 int produce_serial_data = 1;
 
+float t = 0.0;
 float heave_avg = 0.0;
 float wave_length = 0.0;
 float heave_trochoid = 0.0;
@@ -175,8 +176,9 @@ void repeatMe() {
     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)
+    //float a = (accel_rotated.z - 1.0);  // acceleration in fractions of g
+    float a = - 0.25 * PI * PI * sin(2 * PI * t * 0.25) / g_std; // dummy test data (amplitude of heave = 1m, 4sec - period)
+    float h =  0.25 * PI * PI / (2 * PI * 0.25) / (2 * PI * 0.25) * sin(2 * PI * t * 0.25);
 
     float speed_prev = waveState.vert_speed;
     kalman_wave_step(&waveState, a * g_std, delta_t);
@@ -185,8 +187,8 @@ void repeatMe() {
     float speed = waveState.vert_speed;
 
     float y = heave;
-    aranovskiy_update(&params, &state, y, delta_t);
-    double freq = state.f;
+    //aranovskiy_update(&params, &state, y, delta_t);
+    double freq = 1.0; //state.f;
 
     if (first) {
       kalman_smoother_set_initial(&kalman_freq, freq);
@@ -230,17 +232,18 @@ void repeatMe() {
         if (produce_serial_data) {
           Serial.printf("heave_cm:%.4f", heave * 100);
           Serial.printf(",heave_trochoid:%.4f", heave_trochoid * 100);
+          Serial.printf(",h_cm:%.4f", h * 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_adj:%.4f", freq_adj * 100);
-          Serial.printf(",freq_troch:%.4f", wave_freq_trochoid * 100);
-          Serial.printf(",freq_troch_adj:%.4f", wave_freq_trochoid_adj * 100);
-          Serial.printf(",period_decisec:%.4f", period * 10);
-          Serial.printf(",period_troch_decisec:%.4f", period_trochoid * 10);
+          //Serial.printf(",freq_adj:%.4f", freq_adj * 100);
+          //Serial.printf(",freq_troch:%.4f", wave_freq_trochoid * 100);
+          //Serial.printf(",freq_troch_adj:%.4f", wave_freq_trochoid_adj * 100);
+          //Serial.printf(",period_decisec:%.4f", period * 10);
+          //Serial.printf(",period_troch_decisec:%.4f", period_trochoid * 10);
           //Serial.printf(",accel abs:%0.4f", g_std * sqrt(accel.x * accel.x + accel.y * accel.y + accel.z * accel.z));
-          Serial.printf(",accel bias:%0.4f", accel_bias);
+          //Serial.printf(",accel bias:%0.4f", accel_bias);
           Serial.println();
         }
         else {
@@ -264,6 +267,7 @@ void repeatMe() {
         got_samples = 0;
       }
     }
+    t = t + delta_t;
   }
   else {
     AtomS3.update();