Update bbn_wave_freq_m5atomS3.ino

bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino

@@ -74,7 +74,7 @@ static int prev_xpos[18];
 
 unsigned long now = 0UL, last_refresh = 0UL, last_update = 0UL, last_update_k = 0UL;
 unsigned long got_samples = 0;
-int first = 1;
+int first = 1, kalman_k_first = 1;
 
 float delta_t;  // time step sec
 
@@ -215,14 +215,15 @@ void repeatMe() {
         min_max_lemire_update(&min_max_h, sample, windowMicros);
 
         if (fabs(freq - freq_adj) < 0.3 * freq_adj) { /* sanity check of convergence for freq */
-          if (waveAltState.heave == 0.0f && waveAltState.vert_speed == 0.0f && waveAltState.vert_accel == 0.0f) {
-            waveAltState.heave = heave;
+          float k_hat = - pow(2.0 * PI * freq_adj, 2);
+          if (kalman_k_first) {
+            kalman_k_first = 0;
+            waveAltState.heave = waveState.heave;
             waveAltState.vert_speed = waveState.vert_speed;
-            waveAltState.vert_accel = a * g_std;
+            waveAltState.vert_accel = k_hat * waveState.heave; //a * g_std;
             waveAltState.accel_bias = 0.0f;
             kalman_wave_alt_init_state(&waveAltState);
           }
-          float k_hat = - pow(2.0 * PI * freq_adj, 2);
           float delta_t_k = last_update_k == 0UL ? delta_t : (now - last_update_k) / 1000000.0;
           kalman_wave_alt_step(&waveAltState, a * g_std, k_hat, delta_t_k);
           last_update_k = now;