sensor fusion + battery voltage

examples/9DOF_IMU_Robot/9DOF_IMU_Robot.ino

@@ -5,7 +5,6 @@ void setup() {
   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");
 }
 
@@ -23,9 +22,9 @@ void loop() {
     formatPrint(NoU3.gyroscope_x);
     formatPrint(NoU3.gyroscope_y);
     formatPrint(NoU3.gyroscope_z);
-    formatPrint(NoU3.magnetometer_X);
-    formatPrint(NoU3.magnetometer_Y);
-    formatPrint(NoU3.magnetometer_Z);
+    formatPrint(NoU3.magnetometer_x);
+    formatPrint(NoU3.magnetometer_y);
+    formatPrint(NoU3.magnetometer_z);
     Serial.println('\t');
   }
 }

examples/PestoLink_ArcadeBot/PestoLink_ArcadeBot.ino

@@ -38,8 +38,8 @@ void loop() {
 
     // This measures your batteries voltage and sends it to PestoLink
     // You could use this value for a lot of cool things, for example make LEDs flash when your batteries are low?
-    float batteryVal = NoU3.getBatteryVoltage();
-    PestoLink.setBatteryVal(batteryVal);
+    float batteryVoltage = NoU3.getBatteryVoltage();
+    PestoLink.printBatteryVoltage(batteryVoltage);
 
     // Here we decide what the throttle and rotation direction will be based on gamepad inputs   
     if (PestoLink.update()) {

examples/sensor_fusion/sensor_calibration_MotionCal/sensor_calibration_MotionCal.ino

@@ -0,0 +1,31 @@
+#include <Alfredo_NoU3.h>
+
+void setup() {
+  Serial.begin(115200);
+  NoU3.beginMotors();
+  NoU3.beginIMUs();
+
+  Serial.println("Accel_X\tAccel_Y\tAccel_Z\tGyro_X\tGyro_Y\tGyro_Z\tMag_X\tMag_Y\tMag_Z");
+}
+
+void loop() {
+  NoU3.updateIMUs();
+
+  if(NoU3.checkDataIMU() == true){
+    printRaw();
+  }
+}
+
+void printRaw(){
+  // 'Raw' values to match expectation of MotionCal
+  Serial.print("Raw:");
+  Serial.print(int(NoU3.acceleration_x*8192.0)); Serial.print(",");
+  Serial.print(int(NoU3.acceleration_y*8192.0)); Serial.print(",");
+  Serial.print(int(NoU3.acceleration_z*8192.0)); Serial.print(",");
+  Serial.print(int(NoU3.gyroscope_x*10.0)); Serial.print(",");
+  Serial.print(int(NoU3.gyroscope_y*10.0)); Serial.print(",");
+  Serial.print(int(NoU3.gyroscope_z*10.0)); Serial.print(",");
+  Serial.print(int(NoU3.magnetometer_x*10.0)); Serial.print(",");
+  Serial.print(int(NoU3.magnetometer_y*10.0)); Serial.print(",");
+  Serial.print(int(NoU3.magnetometer_z*10.0)); Serial.println("");
+}

examples/sensor_fusion/sensor_calibration_measure_accel_and_gyro/sensor_calibration_measure_accel_and_gyro.ino

@@ -0,0 +1,79 @@
+#include <Alfredo_NoU3.h>
+
+const int numMeasurements = 2000;
+float acceleration_x[numMeasurements];
+float acceleration_y[numMeasurements];
+float acceleration_z[numMeasurements];
+float gyroscope_x[numMeasurements];
+float gyroscope_y[numMeasurements];
+float gyroscope_z[numMeasurements];
+
+int currentIndex = 0;
+bool measurementsComplete = false;
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) {
+    ; // Wait here until the Serial Monitor is opened
+  }
+
+  NoU3.beginMotors();
+  NoU3.beginIMUs();
+
+  Serial.println("Serial connection established. Starting data collection...");
+}
+
+void loop() {
+  NoU3.updateIMUs();
+
+  // Check if data is available and measurements are not complete
+  if (NoU3.checkDataIMU() && !measurementsComplete) {
+    // Store measurements in arrays
+    acceleration_x[currentIndex] = NoU3.acceleration_x;
+    acceleration_y[currentIndex] = NoU3.acceleration_y;
+    acceleration_z[currentIndex] = NoU3.acceleration_z;
+    gyroscope_x[currentIndex] = NoU3.gyroscope_x;
+    gyroscope_y[currentIndex] = NoU3.gyroscope_y;
+    gyroscope_z[currentIndex] = NoU3.gyroscope_z;
+
+    currentIndex++;
+
+    // Check if we've reached the target number of measurements
+    if (currentIndex >= numMeasurements) {
+      measurementsComplete = true;
+      calculateAndPrintAverages();
+    }
+  }
+}
+
+void calculateAndPrintAverages() {
+  float sumAx = 0, sumAy = 0, sumAz = 0;
+  float sumGx = 0, sumGy = 0, sumGz = 0;
+
+  // Sum all values for each variable
+  for (int i = 0; i < numMeasurements; i++) {
+    sumAx += acceleration_x[i];
+    sumAy += acceleration_y[i];
+    sumAz += acceleration_z[i];
+    sumGx += gyroscope_x[i];
+    sumGy += gyroscope_y[i];
+    sumGz += gyroscope_z[i];
+  }
+
+  // Calculate averages
+  float avgAx = sumAx / numMeasurements;
+  float avgAy = sumAy / numMeasurements;
+  float avgAz = sumAz / numMeasurements;
+  float avgGx = sumGx / numMeasurements;
+  float avgGy = sumGy / numMeasurements;
+  float avgGz = sumGz / numMeasurements;
+
+  // Print averages
+  Serial.println("Average values after 2000 measurements:");
+  Serial.print("Acceleration X: "); Serial.println(avgAx);
+  Serial.print("Acceleration Y: "); Serial.println(avgAy);
+  Serial.print("Acceleration Z: "); Serial.println(avgAz);
+  Serial.print("Gyroscope X: "); Serial.println(avgGx);
+  Serial.print("Gyroscope Y: "); Serial.println(avgGy);
+  Serial.print("Gyroscope Z: "); Serial.println(avgGz);
+}

examples/sensor_fusion/sensor_calibration_write/sensor_calibration_write.ino

@@ -27,28 +27,28 @@ void setup() {
   }
 
   // in uTesla
-  cal.mag_hardiron[0] = 9.7;
-  cal.mag_hardiron[1] = -0.41;
-  cal.mag_hardiron[2] = 3.57;
+  cal.mag_hardiron[0] = -0.02;
+  cal.mag_hardiron[1] = -1.17;
+  cal.mag_hardiron[2] = 7.73;
 
   // 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;
+  cal.mag_softiron[0] = 1.0;
+  cal.mag_softiron[1] = 0.0;
+  cal.mag_softiron[2] = 0.0;  
+  cal.mag_softiron[3] = 0.0;
+  cal.mag_softiron[4] = 1.0;
+  cal.mag_softiron[5] = 0.0;  
+  cal.mag_softiron[6] = 0.0;
+  cal.mag_softiron[7] = 0.0;
+  cal.mag_softiron[8] = 1.0;
   // 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
+  cal.mag_field = 47.00; // 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.gyro_zerorate[0] = 0.50;
+  cal.gyro_zerorate[1] = 0.40;
+  cal.gyro_zerorate[2] = -0.50;
 
   cal.accel_zerog[0] = 0.0;
   cal.accel_zerog[1] = 0.0;

examples/sensor_fusion/sensor_fusion/sensor_fusion.ino

@@ -0,0 +1,124 @@
+#include <Alfredo_NoU3.h>
+
+#include <Adafruit_Sensor_Calibration.h>
+#include <Adafruit_AHRS.h>
+
+// pick your filter! slower == better quality output
+//Adafruit_NXPSensorFusion filter; // slowest
+//Adafruit_Madgwick filter;  // faster than NXP
+Adafruit_Mahony filter;  // fastest/smalleset
+Adafruit_Sensor_Calibration_EEPROM cal;
+#define FILTER_UPDATE_RATE_HZ 80
+#define PRINT_EVERY_N_UPDATES 10
+
+//#define AHRS_DEBUG_OUTPUT
+
+uint32_t timestamp;
+
+float acl_x = 0, acl_y = 0, acl_z = 0;
+float gyr_x = 0, gyr_y = 0, gyr_z = 0;
+float mag_x = 0, mag_y = 0, mag_z = 0;
+
+float degToRad(float degPerSec) {
+    const float DEG_TO_RAD_CONVERSION = 3.14159265358979323846 / 180.0;
+    return degPerSec * DEG_TO_RAD_CONVERSION;
+}
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) {
+    ;  // Wait here until the Serial Monitor is opened
+  }
+
+  NoU3.beginMotors();
+  NoU3.beginIMUs();
+
+  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) return;
+  timestamp = millis();
+
+  if (!cal.begin()) {
+    Serial.println("Failed to initialize calibration helper");
+  } else if (!cal.loadCalibration()) {
+    Serial.println("No calibration loaded/found");
+  }
+
+  Serial.println("starting filter");
+  filter.begin(FILTER_UPDATE_RATE_HZ);
+  timestamp = millis();
+
+  Serial.println("starting loop");
+}
+
+void loop() {
+
+  float roll, pitch, heading;
+  static uint8_t counter = 0;
+
+  NoU3.updateIMUs();
+
+  if ((millis() - timestamp) > (1000 / FILTER_UPDATE_RATE_HZ)){
+    timestamp = millis();
+
+    acl_x = NoU3.acceleration_x;
+    acl_y = NoU3.acceleration_y;
+    acl_z = NoU3.acceleration_z;
+    gyr_x = NoU3.gyroscope_x;
+    gyr_y = NoU3.gyroscope_y;
+    gyr_z = NoU3.gyroscope_z;
+    mag_x = NoU3.magnetometer_x;
+    mag_y = NoU3.magnetometer_y;
+    mag_z = NoU3.magnetometer_z;
+
+    //hard iron calibration
+    float mx = mag_x - cal.mag_hardiron[0];
+    float my = mag_y - cal.mag_hardiron[1];
+    float mz = mag_z - cal.mag_hardiron[2];
+
+    // soft iron calibration
+    mag_x = mx * cal.mag_softiron[0] + my * cal.mag_softiron[1] + mz * cal.mag_softiron[2];
+    mag_y = mx * cal.mag_softiron[3] + my * cal.mag_softiron[4] + mz * cal.mag_softiron[5];
+    mag_z = mx * cal.mag_softiron[6] + my * cal.mag_softiron[7] + mz * cal.mag_softiron[8];
+
+    // gyro calibration
+    gyr_x -= cal.gyro_zerorate[0];
+    gyr_y -= cal.gyro_zerorate[1];
+    gyr_z -= cal.gyro_zerorate[2];
+
+    // accel calibration
+    //mag_x -= cal.accel_zerog[0];
+    //mag_y -= cal.accel_zerog[1];
+    //mag_z -= cal.accel_zerog[2];
+
+    // Update the SensorFusion filter
+    filter.update(gyr_x, gyr_y, gyr_z, acl_x, acl_y, acl_z, mag_x, mag_y, mag_z);
+
+    // only print the calculated output once in a while
+    if (counter++ >= PRINT_EVERY_N_UPDATES) {
+
+      // reset the counter
+      counter = 0;
+
+      // print the heading, pitch and roll
+      roll = filter.getRoll();
+      pitch = filter.getPitch();
+      heading = filter.getYaw();
+      Serial.print("Orientation: ");
+      Serial.print(heading);
+      Serial.print(", ");
+      Serial.print(pitch);
+      Serial.print(", ");
+      Serial.println(roll);
+
+      float qw, qx, qy, qz;
+      filter.getQuaternion(&qw, &qx, &qy, &qz);
+      // Serial.print("Quaternion: ");
+      // Serial.print(qw, 4);
+      // Serial.print(", ");
+      // Serial.print(qx, 4);
+      // Serial.print(", ");
+      // Serial.print(qy, 4);
+      // Serial.print(", ");
+      // Serial.println(qz, 4);
+    }
+  }
+}

src/Alfredo_NoU3.cpp

@@ -55,6 +55,8 @@ void NoU_Agent::beginIMUs()
     if (LSM6.begin(Wire1) == false)
     {
         Serial.println("LSM6 did not respond.");
+    } else {
+        //Serial.println("LSM6 is good.");
     }
     pinMode(PIN_INTERRUPT_LSM6, INPUT);
     attachInterrupt(digitalPinToInterrupt(PIN_INTERRUPT_LSM6), interruptRoutineLSM6, RISING);
@@ -63,7 +65,9 @@ void NoU_Agent::beginIMUs()
     // Initialize MMC5
     if (MMC5.begin(Wire1) == false)
     {
-        Serial.println("MMC5983MA did not respond.");
+        Serial.println("MMC5 did not respond.");
+    } else {
+        //Serial.println("MMC5 is good.");
     }
     MMC5.softReset();
     MMC5.setFilterBandwidth(800);
@@ -82,11 +86,14 @@ void NoU_Agent::updateIMUs()
     newDataAvailableLSM6 = false;
 
     if (LSM6.accelerationAvailable()) {
-      LSM6.readAcceleration(&acceleration_x, &acceleration_y, &acceleration_z);
+      LSM6.readAcceleration(&acceleration_x, &acceleration_y, &acceleration_z);// result in Gs
+      newDataAvailableIMU = true;
     }
 
     if (LSM6.gyroscopeAvailable()) {
-      LSM6.readGyroscope(&gyroscope_x, &gyroscope_y, &gyroscope_z);
+      LSM6.readGyroscope(&gyroscope_x, &gyroscope_y, &gyroscope_z);  // Results in degrees per second
+    newDataAvailableIMU = true;
+
     }
   }
 
@@ -95,10 +102,18 @@ void NoU_Agent::updateIMUs()
     newDataAvailableMMC5 = false;
     MMC5.clearMeasDoneInterrupt();
 
-    MMC5.readAccelerometer(&magnetometer_X, &magnetometer_Y, &magnetometer_Z);  // Results in µT (microteslas).
+    MMC5.readAccelerometer(&magnetometer_x, &magnetometer_y, &magnetometer_z);  // Results in µT (microteslas)
+    newDataAvailableIMU = true;
   }
 }
 
+bool NoU_Agent::checkDataIMU(){ 
+    bool result = newDataAvailableIMU;
+    newDataAvailableIMU = false;
+    return result;
+};
+
+
 NoU_Motor::NoU_Motor(uint8_t motorPort)
 {
     this->motorPort = motorPort;

src/Alfredo_NoU3.h

@@ -55,10 +55,13 @@ class NoU_Agent {
         void updateIMUs();
 		float getBatteryVoltage(){ return analogReadMilliVolts(PIN_SNS_VIN) * 0.001 * 7.818; };
 		float getVersionVoltage(){ return analogReadMilliVolts(PIN_SNS_VERSION) * 0.001 ; };
+        bool checkDataIMU();
 
         float acceleration_x, acceleration_y, acceleration_z;
         float gyroscope_x, gyroscope_y, gyroscope_z;
-        float magnetometer_X, magnetometer_Y, magnetometer_Z;
+        float magnetometer_x, magnetometer_y, magnetometer_z;
+
+        bool newDataAvailableIMU;
 };
 
 class NoU_Motor {