telemetry update and IMU progress

examples/sensor_fusion/sensor_calibration_MotionCal/sensor_calibration_MotionCal.ino

@@ -1,16 +1,35 @@
 #include <Alfredo_NoU3.h>
 
+#include <Adafruit_Sensor_Calibration.h>
+Adafruit_Sensor_Calibration_EEPROM cal;
+
+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;
+
 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();
 
+  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;
+
+  //gyr_x -= cal.gyro_zerorate[0];
+  //gyr_y -= cal.gyro_zerorate[1];
+  //gyr_z -= cal.gyro_zerorate[2];
+
   if(NoU3.checkDataIMU() == true){
     printRaw();
   }
@@ -19,13 +38,13 @@ void loop() {
 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("");
+  Serial.print(int(acl_x*8192.0)); Serial.print(",");
+  Serial.print(int(acl_y*8192.0)); Serial.print(",");
+  Serial.print(int(acl_z*8192.0)); Serial.print(",");
+  Serial.print(int(gyr_x*16.0)); Serial.print(",");
+  Serial.print(int(gyr_y*16.0)); Serial.print(",");
+  Serial.print(int(gyr_z*16.0)); Serial.print(",");
+  Serial.print(int(mag_x*10.0)); Serial.print(",");
+  Serial.print(int(mag_y*10.0)); Serial.print(",");
+  Serial.print(int(mag_z*10.0)); Serial.println("");
 }

examples/sensor_fusion/sensor_calibration_write/sensor_calibration_write.ino

@@ -27,9 +27,9 @@ void setup() {
   }
 
   // in uTesla
-  cal.mag_hardiron[0] = -0.02;
-  cal.mag_hardiron[1] = -1.17;
-  cal.mag_hardiron[2] = 7.73;
+  cal.mag_hardiron[0] = 3.6;
+  cal.mag_hardiron[1] = 3.3;
+  cal.mag_hardiron[2] = -14.0;
 
   // in uTesla
   cal.mag_softiron[0] = 1.0;

examples/sensor_fusion/sensor_fusion/sensor_fusion.ino

@@ -5,8 +5,8 @@
 
 // pick your filter! slower == better quality output
 //Adafruit_NXPSensorFusion filter; // slowest
-//Adafruit_Madgwick filter;  // faster than NXP
-Adafruit_Mahony filter;  // fastest/smalleset
+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
@@ -90,7 +90,7 @@ void loop() {
     //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);
+    filter.update(gyr_x * 0.0174533, gyr_y * 0.0174533, gyr_z * 0.0174533, 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) {
@@ -109,16 +109,18 @@ void loop() {
       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);
+      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);
+      */
     }
   }
 }