Update bbn_wave_freq_m5atomS3.ino

bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino

@@ -238,7 +238,7 @@ 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(",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);
@@ -347,7 +347,7 @@ void setup(void) {
      Even 2cm bias in heave is too much to affect frequency a lot
   */
   double omega_init = 0.04 * (2 * PI);  // init frequency Hz * 2 * PI (start converging from omega_init/2)
-  double k_gain = 200.0; // Aranovskiy gain. Higher value will give faster convergence, but too high will overflow float
+  double k_gain = 175.0; // Aranovskiy gain. Higher value will give faster convergence, but too high will overflow float
   double t_0 = 0.0;
   double x1_0 = 0.0;
   double theta_0 = - (omega_init * omega_init / 4.0);