Update bbn_wave_freq_m5atomS3.ino

bbn_wave_freq_m5atomS3/bbn_wave_freq_m5atomS3.ino

@@ -17,10 +17,54 @@
 */
 
 #include <M5Unified.h>
+#include "M5AtomS3.h"
 #include "AranovskiyFilter.h"
 #include "KalmanSmoother.h"
 #include "TrochoidalWave.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;
+
+static auto &dsp = (M5.Display);
+static rect_t rect_graph_area;
+static rect_t rect_text_area;
+
+static int prev_xpos[18];
+
 unsigned long now = 0UL, last_refresh = 0UL, last_update = 0UL;
 int got_samples = 0;
 
@@ -41,7 +85,62 @@ KalmanSmootherVars kalman;
 
 int first = 1;
 
+const char* 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));
+    dsp.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();
+    }
+  }
+  auto backcolor = TFT_BLACK; //(c == 0) ? TFT_BLACK : TFT_BLUE;
+  dsp.fillRect(rect_text_area.x, rect_text_area.y, rect_text_area.w, rect_text_area.h, backcolor);
+
+  if (c) {
+    dsp.setCursor(rect_text_area.x + 2, rect_text_area.y + 1);
+    dsp.setTextColor(TFT_WHITE, TFT_BLACK);
+    dsp.printf("Countdown:%d ", c);
+  }
+}
+
+void startCalibration(void) {
+  updateCalibration(10, true);
+}
+
 void repeatMe() {
+  static uint32_t frame_count = 0;
+  static uint32_t prev_sec = 0;
+
   auto imu_update = M5.Imu.update();
   if (imu_update) {
     m5::imu_3d_t accel;
@@ -65,28 +164,91 @@ void repeatMe() {
 
     double a = y;  // acceleration in fractions of g
     double period = (state.f > 0 ? 1.0 / state.f : 9999.0);    // or use freq_adj
-    double heave = - a * trochoid_wave_length(period) / (2 * PI);
+    double wave_length = trochoid_wave_length(period);
+    double heave = - a * wave_length / (2 * PI);
 
     if (now - last_refresh >= 1000000) {
       M5.Lcd.setCursor(0, 10);
       M5.Lcd.clear();  // Delay 100ms
 
-      M5.Lcd.printf("sec: %d\n\n", now / 1000000);
-      M5.Lcd.printf("period sec: %0.4f\n\n", (state.f > 0 ? 1.0 / state.f : 9999.0));
-      M5.Lcd.printf("period adj: %0.4f\n\n", (freq_adj > 0 ? 1.0 / freq_adj : 9999.0));
-      M5.Lcd.printf("samples: %d\n\n", got_samples);
-      M5.Lcd.printf("heave: %0.4f\n\n", heave);
-      M5.Lcd.printf("%0.3f %0.3f %0.3f\n\n", accel.x, accel.y, accel.z - 1.0);
+      M5.Lcd.printf("imu: %s\n", name);
+      M5.Lcd.printf("sec: %d\n", now / 1000000);
+      M5.Lcd.printf("period sec: %0.4f\n", (state.f > 0 ? 1.0 / state.f : 9999.0));
+      M5.Lcd.printf("period adj: %0.4f\n", (freq_adj > 0 ? 1.0 / freq_adj : 9999.0));
+      M5.Lcd.printf("samples: %d\n", got_samples);
+      M5.Lcd.printf("wave length: %0.4f\n", wave_length);
+      M5.Lcd.printf("heave: %0.4f\n", heave);
+      M5.Lcd.printf("%0.3f %0.3f %0.3f\n", accel.x, accel.y, accel.z);
 
       last_refresh = now;
       got_samples = 0;
     }
+    ++frame_count;
+  }
+  else {
+    //M5.update();
+    AtomS3.update();
+    // Calibration is initiated when ascreen is clicked.
+    if (AtomS3.BtnA.wasPressed()) {
+      startCalibration();
+    }
+  }
+  int32_t sec = millis() / 1000;
+  if (prev_sec != sec) {
+    prev_sec = sec;
+    //M5_LOGI("sec:%d  frame:%d", sec, frame_count);
+    frame_count = 0;
+    if (calib_countdown) {
+      updateCalibration(calib_countdown - 1);
+    }
+    if ((sec & 7) == 0) {
+      // prevent WDT.
+      vTaskDelay(1);
+    }
   }
 }
 
 void setup(void) {
   auto cfg = M5.config();
-  M5.begin(cfg);
+  AtomS3.begin(cfg);
+
+  auto imu_type = M5.Imu.getType();
+  switch (imu_type) {
+    case m5::imu_none:        name = "not found";   break;
+    case m5::imu_sh200q:      name = "sh200q";      break;
+    case m5::imu_mpu6050:     name = "mpu6050";     break;
+    case m5::imu_mpu6886:     name = "mpu6886";     break;
+    case m5::imu_mpu9250:     name = "mpu9250";     break;
+    case m5::imu_bmi270:      name = "bmi270";      break;
+    default:                  name = "unknown";     break;
+  };
+  M5.Lcd.setCursor(0, 10);
+  M5.Lcd.clear();  // Delay 100ms
+  M5.Lcd.printf("imu: %s\n", name);
+
+  if (imu_type == m5::imu_none) {
+    for (;;) {
+      delay(1);
+    }
+  }
+
+  int32_t w = dsp.width();
+  int32_t h = dsp.height();
+  if (w < h) {
+    dsp.setRotation(dsp.getRotation() ^ 1);
+    w = dsp.width();
+    h = dsp.height();
+  }
+  int32_t graph_area_h = ((h - 8) / 18) * 18;
+  int32_t text_area_h = h - graph_area_h;
+
+  rect_graph_area = { 0, 0, w, graph_area_h };
+  rect_text_area = {0, graph_area_h, w, text_area_h };
+
+  // Read calibration values from NVS.
+  if (!M5.Imu.loadOffsetFromNVS()) {
+    startCalibration();
+  }
 
   aranovskiy_default_params(&params, omega_up, k_gain);
   aranovskiy_init_state(&state, t_0, x1_0, theta_0, sigma_0);
@@ -96,8 +258,8 @@ void setup(void) {
 }
 
 void loop(void) {
-  M5.update();
+  //M5.update();
+  AtomS3.update();
   delay(3);
   repeatMe();
 }
-