splitted imu management to its own file

include/imu.h

@@ -0,0 +1,38 @@
+#pragma once
+// #define M5STACK_MPU6886 
+#define M5STACK_MPU9250 
+// #define M5STACK_MPU6050
+// #define M5STACK_200Q
+
+#include <M5Stack.h>
+#include <BMM150class.h>
+#include <utility/quaternionFilters.h>
+
+#include <Smoothed.h>
+
+#include <ArduinoJson.h>
+
+extern float accX ;
+extern float accY ;
+extern float accZ ;
+
+extern float gyroX ;
+extern float gyroY ;
+extern float gyroZ ;
+
+extern float pitch ;
+extern float roll ;
+extern float yaw ;
+
+extern float pitch_ahrs ;
+extern float roll_ahrs ;
+extern float yaw_ahrs ;
+
+extern Smoothed<float> yaw_smooth;
+extern Smoothed<float> pitch_smooth;
+extern Smoothed<float> roll_smooth;
+
+void initGyro();
+void calibGyro();
+void setup_imu();
+void handle_imu();

include/wifimgr.h

@@ -1,7 +1,7 @@
 #pragma once
 #include <WiFi.h>
 #include <WebServer.h>
-#include <SPIFFS.h>
+
 using WebServerClass = WebServer;
 
 #define AC_DEBUG
@@ -13,8 +13,7 @@ using WebServerClass = WebServer;
   the valid file system. After including AutoConnect.h, the Sketch can determine
   whether to use FS.h or LittleFS.h by AUTOCONNECT_USE_SPIFFS definition.
 */
-#include <FS.h>
-#include <SPIFFS.h>
+
 
 
 extern AutoConnect portal;

src/imu.cpp

@@ -0,0 +1,277 @@
+#include "imu.h"
+
+//Storage
+
+struct Calib {
+  float magoffsetX;
+  float magoffsetY;
+  float magoffsetZ;
+  float magscaleX;
+  float magscaleY;
+  float magscaleZ;
+};
+const char *calibFile = "/calib.txt";  // <- SD library uses 8.3 filenames
+Calib calib;  
+
+#define MADGWICK
+//#define MAHONY
+
+BMM150class bmm150;
+
+float accX = 0.0F;
+float accY = 0.0F;
+float accZ = 0.0F;
+
+float gyroX = 0.0F;
+float gyroY = 0.0F;
+float gyroZ = 0.0F;
+
+int AVERAGENUM_GY = 256;
+float init_gyroX = 0.0F;
+float init_gyroY = 0.0F;
+float init_gyroZ = 0.0F;
+
+float magnetX = 0.0F;
+float magnetY = 0.0F;
+float magnetZ = 0.0F;
+
+//for hard iron correction
+float magoffsetX = 0.0F;
+float magoffsetY = 0.0F;
+float magoffsetZ = 0.0F;
+
+//for soft iron correction
+float magscaleX = 0.0F;
+float magscaleY = 0.0F;
+float magscaleZ = 0.0F;
+
+float pitch = 0.0F;
+float roll = 0.0F;
+float yaw = 0.0F;
+
+float pitch_ahrs = 0.0F;
+float roll_ahrs = 0.0F;
+float yaw_ahrs = 0.0F;
+
+float temp = 0.0F;
+
+uint32_t Now = 0;
+uint32_t lastUpdate = 0;
+float deltat = 0.0f;
+
+//moved to global
+float magnetX1, magnetY1, magnetZ1;
+float head_dir;
+
+Smoothed<float> yaw_smooth;
+Smoothed<float> pitch_smooth;
+Smoothed<float> roll_smooth;
+
+void loadCalibration(const char *calibFile, Calib &calib) {
+  // Open file for reading
+  SPIFFS.begin();
+  File file = SPIFFS.open(calibFile);
+
+  // Allocate a temporary JsonDocument
+  // Don't forget to change the capacity to match your requirements.
+  // Use arduinojson.org/v6/assistant to compute the capacity.
+  StaticJsonDocument<512> doc;
+
+  // Deserialize the JSON document
+  DeserializationError error = deserializeJson(doc, file);
+  if (error){Serial.println(F("Failed to read file"));}
+    
+  else{
+      // Copy values from the JsonDocument to the Config
+    calib.magoffsetX = doc["magoffsetX"];
+    calib.magoffsetY = doc["magoffsetY"];
+    calib.magoffsetZ = doc["magoffsetZ"];
+    calib.magscaleX = doc["magscaleX"];
+    calib.magscaleY = doc["magscaleY"];
+    calib.magscaleZ = doc["magscaleZ"];
+
+
+  // Close the file (Curiously, File's destructor doesn't close the file)
+  file.close();
+  }
+  SPIFFS.end();
+  
+}
+
+// Saves the configuration to a file
+void saveCalibration(const char *calibFile, const Calib &calib) {
+  // Delete existing file, otherwise the configuration is appended to the file
+  SPIFFS.begin();
+  SPIFFS.remove(calibFile);
+
+  // Open file for writing
+  File file = SPIFFS.open(calibFile, FILE_WRITE);
+  if (!file) {
+    Serial.println(F("Failed to create file"));
+    return;
+  }
+
+  // Allocate a temporary JsonDocument
+  // Don't forget to change the capacity to match your requirements.
+  // Use arduinojson.org/assistant to compute the capacity.
+  StaticJsonDocument<256> doc;
+
+  // Set the values in the document
+  doc["magoffsetX"] = calib.magoffsetX;
+  doc["magoffsetY"] = calib.magoffsetY;
+  doc["magoffsetZ"] = calib.magoffsetZ;
+  doc["magscaleX"] = calib.magscaleX;
+  doc["magscaleY"] = calib.magscaleY;
+  doc["magscaleZ"] = calib.magscaleZ;
+
+
+  // Serialize JSON to file
+  if (serializeJson(doc, file) == 0) {
+    Serial.println(F("Failed to write to file"));
+  }
+
+  // Close the file
+  file.close();
+  SPIFFS.end();
+}
+
+
+
+
+void initGyro()
+{
+    M5.Lcd.fillScreen(BLACK);
+    M5.Lcd.setTextColor(WHITE);
+    M5.Lcd.setTextSize(1);
+    M5.Lcd.setCursor(0, 0);
+    M5.Lcd.print("begin gyro calibration");
+
+    for (int i = 0; i < AVERAGENUM_GY; i++)
+    {
+        M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
+        init_gyroX += gyroX;
+        init_gyroY += gyroY;
+        init_gyroZ += gyroZ;
+        delay(5);
+    }
+    init_gyroX /= AVERAGENUM_GY;
+    init_gyroY /= AVERAGENUM_GY;
+    init_gyroZ /= AVERAGENUM_GY;
+    loadCalibration(calibFile, calib);
+    
+    magoffsetX = calib.magoffsetX;
+    magoffsetY = calib.magoffsetY;
+    magoffsetZ = calib.magoffsetZ;
+    magscaleX = calib.magscaleX;
+    magscaleY = calib.magscaleY;
+    magscaleZ = calib.magscaleZ;
+
+}
+
+void calibGyro()
+{
+    M5.Lcd.fillScreen(BLACK);
+    M5.Lcd.setTextColor(WHITE);
+    M5.Lcd.setTextSize(1);
+    M5.Lcd.setCursor(0, 0);
+    M5.Lcd.print("begin calibration in 3 seconds");
+    delay(3000);
+    M5.Lcd.setCursor(0, 10);
+    M5.Lcd.print("Flip + rotate core calibration");
+    bmm150.bmm150_calibrate(15000);
+    delay(100);
+
+    bmm150.Init();
+    bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
+    bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
+
+    calib.magoffsetX = magoffsetX;
+    calib.magoffsetY = magoffsetY;
+    calib.magoffsetZ = magoffsetZ;
+    calib.magscaleX = magscaleX;
+    calib.magscaleY = magscaleY;
+    calib.magscaleZ = magscaleZ;
+
+    saveCalibration(calibFile, calib);
+
+    M5.Lcd.fillScreen(BLACK);
+    M5.Lcd.setTextColor(GREEN, BLACK);
+    M5.Lcd.setTextSize(2);
+}
+
+void setup_imu()
+{
+    yaw_smooth.begin(SMOOTHED_AVERAGE, 100);
+    pitch_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
+    roll_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
+
+    M5.IMU.Init();
+    initGyro();
+    bmm150.Init();
+
+    bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
+    bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
+}
+
+void handle_imu()
+{
+
+    M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
+    gyroX -= init_gyroX;
+    gyroY -= init_gyroY;
+    gyroZ -= init_gyroZ;
+    M5.IMU.getAccelData(&accX, &accY, &accZ);
+    M5.IMU.getAhrsData(&pitch_ahrs, &roll_ahrs, &yaw_ahrs);
+    M5.IMU.getTempData(&temp);
+    bmm150.getMagnetData(&magnetX, &magnetY, &magnetZ);
+
+    magnetX1 = (magnetX - magoffsetX) * magscaleX;
+    magnetY1 = (magnetY - magoffsetY) * magscaleY;
+    magnetZ1 = (magnetZ - magoffsetZ) * magscaleZ;
+
+    head_dir = atan2(magnetX1, magnetY1);
+    if (head_dir < 0)
+        head_dir += 2 * M_PI;
+    if (head_dir > 2 * M_PI)
+        head_dir -= 2 * M_PI;
+    head_dir *= RAD_TO_DEG;
+    Now = micros();
+    deltat = ((Now - lastUpdate) / 1000000.0f); //0.09
+    lastUpdate = Now;
+
+#ifdef MADGWICK
+    MadgwickQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
+                             gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
+                             -magnetX1, magnetY1, -magnetZ1, deltat);
+#endif
+
+#ifdef MAHONY
+    MahonyQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
+                           gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
+                           -magnetX1, magnetY1, -magnetZ1, deltat);
+    //delay(10); // adjust sampleFreq = 50Hz
+#endif
+
+    yaw = atan2(2.0f * (*(getQ() + 1) * *(getQ() + 2) + *getQ() *
+                                                            *(getQ() + 3)),
+                *getQ() * *getQ() + *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) - *(getQ() + 3) * *(getQ() + 3));
+    pitch = -asin(2.0f * (*(getQ() + 1) * *(getQ() + 3) - *getQ() *
+                                                              *(getQ() + 2)));
+    roll = atan2(2.0f * (*getQ() * *(getQ() + 1) + *(getQ() + 2) *
+                                                       *(getQ() + 3)),
+                 *getQ() * *getQ() - *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) + *(getQ() + 3) * *(getQ() + 3));
+    yaw = -0.5 * M_PI - yaw;
+    if (yaw < 0)
+        yaw += 2 * M_PI;
+    if (yaw > 2 * M_PI)
+        yaw -= 2 * M_PI;
+    pitch *= RAD_TO_DEG;
+    yaw *= RAD_TO_DEG;
+    roll *= RAD_TO_DEG;
+
+    yaw_smooth.add(yaw);
+    pitch_smooth.add(pitch);
+    roll_smooth.add(roll);
+
+    
+}

src/main.cpp

@@ -14,136 +14,25 @@ OSC configuration custom page
 - sensor calibration
 - smoothing | passage 180-> -180
 
-*/
+- README !!
+- calibration storage and loading
 
+*/
+#include <FS.h>
+#include <SPIFFS.h>
+#include <M5Stack.h>
 #include "wifimgr.h"
 #include "oscmgr.h"
+#include "imu.h"
 
-// #define M5STACK_MPU6886 
-#define M5STACK_MPU9250 
-// #define M5STACK_MPU6050
-// #define M5STACK_200Q
-
-#include <M5Stack.h>
-#include <BMM150class.h>
-#include <utility/quaternionFilters.h>
-
-#include <Smoothed.h>
-
-///////////////////////   SENSOR   ////////////////////////////////////
-
-Smoothed <float> yaw_smooth;
-Smoothed <float> pitch_smooth;
-Smoothed <float> roll_smooth;
-
-int sensor_rate = 10 ;
-long int last_sent ;
-
-// float accX = 0.0F;
-// float accY = 0.0F;
-// float accZ = 0.0F;
-
-// float gyroX = 0.0F;
-// float gyroY = 0.0F;
-// float gyroZ = 0.0F;
-
-// float pitch = 0.0F;
-// float roll  = 0.0F;
-// float yaw   = 0.0F;
 
-// float temp = 0.0F;
 
-#define MADGWICK
-//#define MAHONY
 
-BMM150class bmm150;
-//TaskHandle_t lcdTaskHandle = NULL;
 
-float accX = 0.0F;
-float accY = 0.0F;
-float accZ = 0.0F;
-
-float gyroX = 0.0F;
-float gyroY = 0.0F;
-float gyroZ = 0.0F;
-
-int AVERAGENUM_GY = 256;
-float init_gyroX = 0.0F;
-float init_gyroY = 0.0F;
-float init_gyroZ = 0.0F;
-
-float magnetX = 0.0F;
-float magnetY = 0.0F;
-float magnetZ = 0.0F;
-
-//for hard iron correction
-float magoffsetX = 0.0F;
-float magoffsetY = 0.0F;
-float magoffsetZ = 0.0F;
-
-//for soft iron correction
-float magscaleX = 0.0F;
-float magscaleY = 0.0F;
-float magscaleZ = 0.0F;
-
-float pitch = 0.0F;
-float roll = 0.0F;
-float yaw = 0.0F;
-
-float pitch_ahrs = 0.0F;
-float roll_ahrs = 0.0F;
-float yaw_ahrs = 0.0F;
-
-float temp = 0.0F;
-
-uint32_t Now = 0;
-uint32_t lastUpdate = 0;
-float deltat = 0.0f;
-
-//moved to global
-float magnetX1, magnetY1, magnetZ1;
-float head_dir;
-
-void initGyro() {
-  M5.Lcd.fillScreen(BLACK);
-  M5.Lcd.setTextColor(WHITE);
-  M5.Lcd.setTextSize(1);
-  M5.Lcd.setCursor(0, 0);
-  M5.Lcd.print("begin gyro calibration");
-
-  for (int i = 0;i < AVERAGENUM_GY;i++) {
-    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
-    init_gyroX += gyroX;
-    init_gyroY += gyroY;
-    init_gyroZ += gyroZ;
-    delay(5);
-  }
-  init_gyroX /= AVERAGENUM_GY;
-  init_gyroY /= AVERAGENUM_GY;
-  init_gyroZ /= AVERAGENUM_GY;
-}
-
-void calibGyro(){
-  M5.Lcd.fillScreen(BLACK);
-  M5.Lcd.setTextColor(WHITE);
-  M5.Lcd.setTextSize(1);
-  M5.Lcd.setCursor(0, 0);
-  M5.Lcd.print("begin calibration in 3 seconds");
-  delay(3000);
-  M5.Lcd.setCursor(0, 10);
-  M5.Lcd.print("Flip + rotate core calibration");
-  bmm150.bmm150_calibrate(15000);
-  delay(100);
-
-  bmm150.Init();
-  bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
-  bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
-
-  M5.Lcd.fillScreen(BLACK);
-  M5.Lcd.setTextColor(GREEN, BLACK);
-  M5.Lcd.setTextSize(2);
-}
+///////////////////////   SENSOR   ////////////////////////////////////
 
+int sensor_rate = 10;
+long int last_sent;
 
 ///////////////////////      TFT   ////////////////////////////////////
 
@@ -167,9 +56,9 @@ int barShift = 15;
 void drawSensorInfo(int offsetX, int offsetY){
   String printString = "";
   //printString += gyroX + ", " + gyroY + ", " + gyroZ;
-  float temp = map(yaw, -180, 180, 0, 100);
+  // float temp = map(yaw, -180, 180, 0, 100);
   M5.Lcd.fillRect(offsetX, offsetY, barWidth, barHeight,0);
-  M5.Lcd.progressBar(offsetX, offsetY, barWidth, barHeight,temp);
+  M5.Lcd.progressBar(offsetX, offsetY, barWidth, barHeight,30);
   M5.Lcd.fillRect(offsetX, offsetY + barShift, barWidth, barHeight,0);
   M5.Lcd.progressBar(offsetX, offsetY + barShift, barWidth, barHeight,30);
   M5.Lcd.fillRect(offsetX, offsetY + 2*barShift, barWidth, barHeight,0);
@@ -203,24 +92,15 @@ void setup() {
   Serial.begin(115200);
   Serial.println();
 
-  yaw_smooth.begin(SMOOTHED_AVERAGE, 100);
-  pitch_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
-  roll_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
-
-  M5.begin();
+  setup_imu();
+    M5.begin();
   /*
     Power chip connected to gpio21, gpio22, I2C device
     Set battery charging voltage and current
     If used battery, please call this function in your project
   */
   M5.Power.begin();
-  M5.IMU.Init();
-  initGyro();
-  bmm150.Init();
-
-  bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
-  bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
-
+  
   M5.Lcd.fillScreen(BLACK);
   M5.Lcd.setTextWrap(true, true);
   M5.Lcd.setTextColor(BLUE , BLACK);
@@ -300,64 +180,12 @@ void loop() {
     }
 
   
-
-  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
-  gyroX -= init_gyroX;
-  gyroY -= init_gyroY;
-  gyroZ -= init_gyroZ;
-  M5.IMU.getAccelData(&accX,&accY,&accZ);
-  M5.IMU.getAhrsData(&pitch_ahrs,&roll_ahrs,&yaw_ahrs);
-  M5.IMU.getTempData(&temp);
-  bmm150.getMagnetData(&magnetX, &magnetY, &magnetZ);
-
-  magnetX1 = (magnetX - magoffsetX) * magscaleX;
-  magnetY1 = (magnetY - magoffsetY) * magscaleY;
-  magnetZ1 = (magnetZ - magoffsetZ) * magscaleZ;
-
-  head_dir = atan2(magnetX1, magnetY1);
-  if(head_dir < 0)
-    head_dir += 2*M_PI;
-  if(head_dir > 2*M_PI)
-    head_dir -= 2*M_PI;
-  head_dir *= RAD_TO_DEG;
-  Now = micros();
-  deltat = ((Now - lastUpdate) / 1000000.0f);//0.09
-  lastUpdate = Now;
-
-#ifdef MADGWICK
-  MadgwickQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
-                           gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
-                           -magnetX1, magnetY1, -magnetZ1, deltat);
-#endif
-
-#ifdef MAHONY
-  MahonyQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
-                           gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
-                           -magnetX1, magnetY1, -magnetZ1, deltat);
-  //delay(10); // adjust sampleFreq = 50Hz
-#endif
-
-  yaw = atan2(2.0f * (*(getQ() + 1) * *(getQ() + 2) + *getQ() *
-                                                          *(getQ() + 3)),
-              *getQ() * *getQ() + *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) - *(getQ() + 3) * *(getQ() + 3));
-  pitch = -asin(2.0f * (*(getQ() + 1) * *(getQ() + 3) - *getQ() *
-                                                            *(getQ() + 2)));
-  roll = atan2(2.0f * (*getQ() * *(getQ() + 1) + *(getQ() + 2) *
-                                                     *(getQ() + 3)),
-               *getQ() * *getQ() - *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) + *(getQ() + 3) * *(getQ() + 3));
-  yaw = -0.5*M_PI-yaw;
-  if(yaw < 0)
-    yaw += 2*M_PI;
-  if(yaw > 2*M_PI)
-    yaw -= 2*M_PI;
-  pitch *= RAD_TO_DEG;
-  yaw *= RAD_TO_DEG;
-  roll *= RAD_TO_DEG;
-
-  yaw_smooth.add(yaw);
-  pitch_smooth.add(pitch);
-  roll_smooth.add(roll);
-
+  if (calib_flag)
+    {
+        calib_flag = false;
+        calibGyro();
+    }
+  handle_imu();
 
   portal.handleClient();
   handle_OSC(); 
@@ -372,10 +200,7 @@ void loop() {
     sendOSC_3f("/ahrs_smooth", yaw_smooth.get(), pitch_smooth.get(), roll_smooth.get());
   }
 
-  if(calib_flag){
-    calib_flag = false ;
-    calibGyro();
-    }
+ 
 
   drawNetworkInfo(0, 65);
   drawSensorInfo(0, 120);