encapsulated imu setup

examples/9DOF_IMU_Robot/9DOF_IMU_Robot.ino

@@ -1,41 +1,9 @@
 #include <Alfredo_NoU3.h>
 
-int interruptPinLSM6 = 48;
-int interruptPinMMC5 = 47;
-
-float acceleration_x, acceleration_y, acceleration_z;
-float gyroscope_x, gyroscope_y, gyroscope_z;
-float magnetometer_X, magnetometer_Y, magnetometer_Z;
-
-volatile bool newDataAvailableLSM6 = true;
-volatile bool newDataAvailableMMC5 = true;
-
 void setup() {
-  NoU3.begin();
   Serial.begin(115200);
-
-  // Initialize LSM6
-  if (LSM6.begin(Wire1) == false) {
-    Serial.println("LSM6 did not respond - check your wiring. Freezing.");
-    while (true) {};
-  }
-  pinMode(interruptPinLSM6, INPUT);
-  attachInterrupt(digitalPinToInterrupt(interruptPinLSM6), interruptRoutineLSM6, RISING);
-  LSM6.enableInterrupt();
-
-  // Initialize MMC5
-  if (MMC5.begin(Wire1) == false) {
-    Serial.println("MMC5983MA did not respond - check your wiring. Freezing.");
-    while (true) {};
-  }
-  MMC5.softReset();
-  MMC5.setFilterBandwidth(800);
-  MMC5.setContinuousModeFrequency(100);
-  MMC5.enableAutomaticSetReset();
-  MMC5.enableContinuousMode();
-  pinMode(interruptPinMMC5, INPUT);
-  attachInterrupt(digitalPinToInterrupt(interruptPinMMC5), interruptRoutineMMC5, RISING);
-  MMC5.enableInterrupt();
+  NoU3.beginMotors();
+  NoU3.beginIMUs();
 
   Serial.println();
   Serial.println("Accel_X\tAccel_Y\tAccel_Z\tGyro_X\tGyro_Y\tGyro_Z\tMag_X\tMag_Y\tMag_Z");
@@ -44,50 +12,24 @@ void setup() {
 long lastPrintMs = 0;  // Stores the last time the function was called
 
 void loop() {
-  // Check LSM6 for new data
-  if (newDataAvailableLSM6) {
-    newDataAvailableLSM6 = false;
-
-    if (LSM6.accelerationAvailable()) {
-      LSM6.readAcceleration(&acceleration_x, &acceleration_y, &acceleration_z);
-    }
 
-    if (LSM6.gyroscopeAvailable()) {
-      LSM6.readGyroscope(&gyroscope_x, &gyroscope_y, &gyroscope_z);
-    }
-  }
-
-  // Check MMC5983MA for new data
-  if (newDataAvailableMMC5) {
-    newDataAvailableMMC5 = false;
-    MMC5.clearMeasDoneInterrupt();
-
-    MMC5.readAccelerometer(&magnetometer_X, &magnetometer_Y, &magnetometer_Z);  // Results in µT (microteslas).
-  }
+  NoU3.updateIMUs();
 
-  if (millis() - lastPrintMs >= 100) {
+  if (millis() - lastPrintMs >= 200) {
     lastPrintMs = millis();  // Update the last time the function was called
-    formatPrint(acceleration_x);
-    formatPrint(acceleration_y);
-    formatPrint(acceleration_z);
-    formatPrint(gyroscope_x);
-    formatPrint(gyroscope_y);
-    formatPrint(gyroscope_z);
-    formatPrint(magnetometer_X);
-    formatPrint(magnetometer_Y);
-    formatPrint(magnetometer_Z);
+    formatPrint(NoU3.acceleration_x);
+    formatPrint(NoU3.acceleration_y);
+    formatPrint(NoU3.acceleration_z);
+    formatPrint(NoU3.gyroscope_x);
+    formatPrint(NoU3.gyroscope_y);
+    formatPrint(NoU3.gyroscope_z);
+    formatPrint(NoU3.magnetometer_X);
+    formatPrint(NoU3.magnetometer_Y);
+    formatPrint(NoU3.magnetometer_Z);
     Serial.println('\t');
   }
 }
 
-void interruptRoutineLSM6() {
-  newDataAvailableLSM6 = true;
-}
-
-void interruptRoutineMMC5() {
-  newDataAvailableMMC5 = true;
-}
-
 void formatPrint(float num) {
   char buffer[11];
   snprintf(buffer, sizeof(buffer), "%7.3f   ", num);

examples/sensor_fusion/sensor_calibration_read/sensor_calibration_read.ino

@@ -0,0 +1,63 @@
+#include "Adafruit_Sensor_Calibration.h"
+
+// select either EEPROM or SPI FLASH storage:
+#ifdef ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM
+  Adafruit_Sensor_Calibration_EEPROM cal;
+#else
+  Adafruit_Sensor_Calibration_SDFat cal;
+#endif
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) delay(10);
+
+  delay(100);
+  Serial.println("Calibration filesys test");
+  if (!cal.begin()) {
+    Serial.println("Failed to initialize calibration helper");
+    while (1) yield();
+  }
+  Serial.print("Has EEPROM: "); Serial.println(cal.hasEEPROM());
+  Serial.print("Has FLASH: "); Serial.println(cal.hasFLASH());
+
+  if (! cal.loadCalibration()) {
+    Serial.println("**WARNING** No calibration loaded/found");
+  }
+  cal.printSavedCalibration();
+
+  Serial.println("Calibrations found: ");
+  Serial.print("\tMagnetic Hard Offset: ");
+  for (int i=0; i<3; i++) {
+    Serial.print(cal.mag_hardiron[i]); 
+    if (i != 2) Serial.print(", ");
+  }
+  Serial.println();
+
+  Serial.print("\tMagnetic Soft Offset: ");
+  for (int i=0; i<9; i++) {
+    Serial.print(cal.mag_softiron[i]); 
+    if (i != 8) Serial.print(", ");
+  }
+  Serial.println();
+
+  Serial.print("\tMagnetic Field Magnitude: ");
+  Serial.println(cal.mag_field);
+
+  Serial.print("\tGyro Zero Rate Offset: ");
+  for (int i=0; i<3; i++) {
+    Serial.print(cal.gyro_zerorate[i]); 
+    if (i != 2) Serial.print(", ");
+  }
+  Serial.println();
+
+  Serial.print("\tAccel Zero G Offset: ");
+  for (int i=0; i<3; i++) {
+    Serial.print(cal.accel_zerog[i]); 
+    if (i != 2) Serial.print(", ");
+  }
+  Serial.println();
+}
+
+void loop() {
+
+}

examples/sensor_fusion/sensor_calibration_write/sensor_calibration_write.ino

@@ -0,0 +1,68 @@
+#include "Adafruit_Sensor_Calibration.h"
+
+// select either EEPROM or SPI FLASH storage:
+#ifdef ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM
+  Adafruit_Sensor_Calibration_EEPROM cal;
+#else
+  Adafruit_Sensor_Calibration_SDFat cal;
+#endif
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) delay(10);
+
+  delay(100);
+  Serial.println("Calibration filesys test");
+  if (!cal.begin()) {
+    Serial.println("Failed to initialize calibration helper");
+    while (1) yield();
+  }
+  Serial.print("Has EEPROM: "); Serial.println(cal.hasEEPROM());
+  Serial.print("Has FLASH: "); Serial.println(cal.hasFLASH());
+
+  if (! cal.loadCalibration()) {
+    Serial.println("No calibration loaded/found... will start with defaults");
+  } else {
+    Serial.println("Loaded existing calibration");
+  }
+
+  // in uTesla
+  cal.mag_hardiron[0] = 9.7;
+  cal.mag_hardiron[1] = -0.41;
+  cal.mag_hardiron[2] = 3.57;
+
+  // in uTesla
+  cal.mag_softiron[0] = 0.991;
+  cal.mag_softiron[1] = -0.001;
+  cal.mag_softiron[2] = 0.002;  
+  cal.mag_softiron[3] = -0.001;
+  cal.mag_softiron[4] = 1.002;
+  cal.mag_softiron[5] = -0.011;  
+  cal.mag_softiron[6] = 0.002;
+  cal.mag_softiron[7] = 0.011;
+  cal.mag_softiron[8] = 1.008;
+  // Earth total magnetic field strength in uTesla (dependent on location and time of the year),
+  // visit: https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml#igrfwmm)
+  cal.mag_field = 47.27; // approximate value for locations along the equator
+
+  // in Radians/s
+  cal.gyro_zerorate[0] = 0.0;
+  cal.gyro_zerorate[1] = 0.0;
+  cal.gyro_zerorate[2] = 0.0;
+
+  cal.accel_zerog[0] = 0.0;
+  cal.accel_zerog[1] = 0.0;
+  cal.accel_zerog[2] = 0.0;
+
+  if (! cal.saveCalibration()) {
+    Serial.println("**WARNING** Couldn't save calibration");
+  } else {
+    Serial.println("Wrote calibration");    
+  }
+
+  cal.printSavedCalibration();
+}
+
+void loop() {
+
+}

library.properties

@@ -7,3 +7,4 @@ paragraph=Supports drving motors and servos. Has helper methods for different dr
 category=Device Control
 url=https://github.com/AlfredoSystems/Alfredo_NoU3
 architectures=esp32
+depends=Adafruit Sensor Calibration, Adafruit AHRS