adding roll pitch yaw example, including processing.org sketch

examples/RollPitchYaw/RollPitchYaw.ino

@@ -0,0 +1,157 @@
+/**
+   What is this file?
+   This is an arduino example that calculates accurate roll,pitch,yaw from raw gyro/accelerometer data
+   It has a calibration stage which removes most of the gyro drift and a complementary filter
+   that combines gyro and accelerometer angles to produce roll/pitch angles that don't drift (like the gyro angle) and aren't noisy
+   (like the accel angles). As there is no magnetic compass on the nano iot 33, it's not possible to 'complement' the yaw
+   - hence yaw will drift and is 'relative'.
+*/
+#include <Arduino_LSM6DS3.h>
+#include <Wire.h>
+
+float accelX,            accelY,             accelZ,            // units m/s/s i.e. accelZ if often 9.8 (gravity)
+      gyroX,             gyroY,              gyroZ,             // units dps (degrees per second)
+      gyroDriftX,        gyroDriftY,         gyroDriftZ,        // units dps
+      gyroRoll,          gyroPitch,          gyroYaw,           // units degrees (expect major drift)
+      gyroCorrectedRoll, gyroCorrectedPitch, gyroCorrectedYaw,  // units degrees (expect minor drift)
+      accRoll,           accPitch,           accYaw,            // units degrees (roll and pitch noisy, yaw not possible)
+      complementaryRoll, complementaryPitch, complementaryYaw;  // units degrees (excellent roll, pitch, yaw minor drift)
+
+long lastTime;
+long lastInterval;
+
+void setup() {
+
+  Serial.begin(1000000);
+  pinMode(LED_BUILTIN, OUTPUT);
+
+  // this sketch will wait until something connects to serial!
+  // this could be 'serial monitor', 'serial plotter' or 'processing.org P3D client' (see ./processing/RollPitchYaw3d.pde file)
+  while (!Serial);
+
+  if (!IMU.begin()) {
+    Serial.println("Failed to initialize IMU!");
+    while (1);
+  }
+
+  calibrateIMU(250, 250);
+
+  lastTime = micros();
+
+}
+
+/*
+  the gyro's x,y,z values drift by a steady amount. if we measure this when arduino is still
+  we can correct the drift when doing real measurements later
+*/
+void calibrateIMU(int delayMillis, int calibrationMillis) {
+
+  int calibrationCount = 0;
+
+  delay(delayMillis); // to avoid shakes after pressing reset button
+
+  float sumX, sumY, sumZ;
+  int startTime = millis();
+  while (millis() < startTime + calibrationMillis) {
+    if (readIMU()) {
+      // in an ideal world gyroX/Y/Z == 0, anything higher or lower represents drift
+      sumX += gyroX;
+      sumY += gyroY;
+      sumZ += gyroZ;
+
+      calibrationCount++;
+    }
+  }
+
+  if (calibrationCount == 0) {
+    Serial.println("Failed to calibrate");
+  }
+
+  gyroDriftX = sumX / calibrationCount;
+  gyroDriftY = sumY / calibrationCount;
+  gyroDriftZ = sumZ / calibrationCount;
+
+}
+
+/**
+   Read accel and gyro data.
+   returns true if value is 'new' and false if IMU is returning old cached data
+*/
+bool readIMU() {
+  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() ) {
+    IMU.readAcceleration(accelX, accelY, accelZ);
+    IMU.readGyroscope(gyroX, gyroY, gyroZ);
+    return true;
+  }
+  return false;
+}
+
+// the loop function runs over and over again forever
+void loop() {
+
+  if (readIMU()) {
+    long currentTime = micros();
+    lastInterval = currentTime - lastTime; // expecting this to be ~104Hz +- 4%
+    lastTime = currentTime;
+
+    doCalculations();
+    printCalculations();
+
+  }
+
+}
+
+/**
+   I'm expecting, over time, the Arduino_LSM6DS3.h will add functions to do most of this,
+   but as of 1.0.0 this was missing.
+*/
+void doCalculations() {
+  accRoll = atan2(accelY, accelZ) * 180 / M_PI;
+  accPitch = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / M_PI;
+
+  float lastFrequency = (float) 1000000.0 / lastInterval;
+  gyroRoll = gyroRoll + (gyroX / lastFrequency);
+  gyroPitch = gyroPitch + (gyroY / lastFrequency);
+  gyroYaw = gyroYaw + (gyroZ / lastFrequency);
+
+  gyroCorrectedRoll = gyroCorrectedRoll + ((gyroX - gyroDriftX) / lastFrequency);
+  gyroCorrectedPitch = gyroCorrectedPitch + ((gyroY - gyroDriftY) / lastFrequency);
+  gyroCorrectedYaw = gyroCorrectedYaw + ((gyroZ - gyroDriftZ) / lastFrequency);
+
+  complementaryRoll = complementaryRoll + ((gyroX - gyroDriftX) / lastFrequency);
+  complementaryPitch = complementaryPitch + ((gyroY - gyroDriftY) / lastFrequency);
+  complementaryYaw = complementaryYaw + ((gyroZ - gyroDriftZ) / lastFrequency);
+
+  complementaryRoll = 0.98 * complementaryRoll + 0.02 * accRoll;
+  complementaryPitch = 0.98 * complementaryPitch + 0.02 * accPitch;
+}
+
+/**
+   This comma separated format is best 'viewed' using 'serial plotter' or processing.org client (see ./processing/RollPitchYaw3d.pde example)
+*/
+void printCalculations() {
+  Serial.print(gyroRoll);
+  Serial.print(',');
+  Serial.print(gyroPitch);
+  Serial.print(',');
+  Serial.print(gyroYaw);
+  Serial.print(',');
+  Serial.print(gyroCorrectedRoll);
+  Serial.print(',');
+  Serial.print(gyroCorrectedPitch);
+  Serial.print(',');
+  Serial.print(gyroCorrectedYaw);
+  Serial.print(',');
+  Serial.print(accRoll);
+  Serial.print(',');
+  Serial.print(accPitch);
+  Serial.print(',');
+  Serial.print(accYaw);
+  Serial.print(',');
+  Serial.print(complementaryRoll);
+  Serial.print(',');
+  Serial.print(complementaryPitch);
+  Serial.print(',');
+  Serial.print(complementaryYaw);
+  Serial.println("");
+}

examples/RollPitchYaw/data/processing/RollPitchYaw3d.pde

@@ -0,0 +1,128 @@
+/**
+ * What is this file?  
+ * This is a processing.org sketch and is *NOT* loaded onto your arduino.  
+ * Instead the processing development environment runs this sketch as a 3d application on your computer.
+ * The sketch connect to your arduino's serial port and reads the roll/pitch/yaw data and displays this
+ * as 3d models.  This sketch assumes you've loaded RollPitchYaw.ino onto your nano iot 33 (or similar) board
+ * 
+ * License LGPL2.1+
+ */
+
+import processing.serial.*;
+import java.awt.event.KeyEvent;
+import java.io.IOException;
+
+Serial arduino;
+
+float gyroRoll,          gyroPitch,          gyroYaw;            // units degrees (expect major drift)
+float gyroCorrectedRoll, gyroCorrectedPitch, gyroCorrectedYaw;   // units degrees (expect minor drift)
+float accRoll,           accPitch,           accYaw;             // units degrees (roll and pitch noisy, yaw not possible)
+float complementaryRoll, complementaryPitch, complementaryYaw;   // units degrees (excellent roll, pitch, yaw minor drift)
+
+
+// NOTE: You need to provide or discover a portName.  Often it's at a fixed index (e.g. 0, 1) in the serial list.  Or you may prefer to hardcode it.  Look at commented lines below for examples.
+//int portIndex = 0;
+int portIndex = 1;
+
+String portName = Serial.list()[portIndex];
+//String portName = "/dev/ttyACM0";  // example Linux portname
+//String portName = "COM1";            // example windows portname
+//String portName = "/dev/tty.usbserial-XXX";
+
+
+
+long lastDataTime = millis();
+boolean connected = false;
+
+void setup() {
+
+  size (1600, 800, P3D);
+  arduino = new Serial(this, portName, 1000000);
+ }
+
+void draw() {
+
+  if (millis() - lastDataTime > 1000 && connected == true) {
+    // we were connected, but have received no data for 1s.  Lets reconnect
+    connected = false;
+    arduino.stop();
+    // best to wait for serial port to free up
+    delay(250);
+    try{
+      arduino = new Serial(this, portName, 1000000);
+    } catch (RuntimeException ex) {
+      // todo: look for better way to handle errors.  the following is equivalent to 'retry'. 
+      connected = true;
+      println(ex);
+    }
+  }
+  textSize(22);
+  translate(width/2, height/2, 0);
+  if (connected) {
+    background(0,150,0); // gray
+    text("Connected to: " + portName, -300, -300);
+  } else {
+    background(150,0,0); // red
+    text("Not connected to: " + portName + ", check portName/portIndex", -300, -300);
+  }
+
+  drawBox("gyro (raw)\n","✓ has yaw\n× major drift", -450, gyroPitch, gyroRoll, gyroYaw);
+  drawBox("gyro (corrected)\n","✓ has yaw\n× minor drift", -150, gyroCorrectedPitch, gyroCorrectedRoll, gyroCorrectedYaw);
+  drawBox("accel (raw)\n","✓ zero drift\n× no yaw + noisy", 150, accPitch, accRoll, accYaw);
+  drawBox("complementary\n","✓great pitch/roll\n× minor yaw drift", 450, complementaryPitch, complementaryRoll, complementaryYaw);
+
+}
+
+/* This draws a moving 3d box representing roll/pitch/yaw including textual stats */
+void drawBox(String name, String description, int xShift, float pitch, float roll, float yaw) {
+  pushMatrix();
+  // Rotate the object
+  translate(xShift,0,0);
+  rotateX(radians(-pitch));
+  rotateZ(radians(-roll));
+  rotateY(radians(yaw));
+
+  textSize(16);
+
+  fill(0, 76, 183);
+  box (160, 40, 300); // Draw box
+  fill(255, 255, 255);
+  text(name, -60, 5, 151);
+  popMatrix();
+  text( name 
+        + description 
+        + "\npitch: " + int(pitch)
+        + "\nroll: " + int(roll)
+        + "\nyaw: " + int(yaw)
+        , xShift -20, 200);
+
+}
+
+
+void serialEvent (Serial myPort) {
+
+  String data = myPort.readStringUntil('\n');
+  // if you got any bytes other than the linefeed:
+  if (data != null) {
+    connected = true;
+    lastDataTime = millis();
+    data = trim(data);
+
+    String items[] = split(data, ',');
+    if (items.length > 1) {
+      //--- Roll,Pitch in degrees
+      gyroRoll = float(items[0]);
+      gyroPitch = float(items[1]);
+      gyroYaw = float(items[2]);
+      gyroCorrectedRoll = float(items[3]);
+      gyroCorrectedPitch = float(items[4]);
+      gyroCorrectedYaw = float(items[5]);
+      accRoll = float(items[6]);
+      accPitch = float(items[7]);
+      accYaw = float(items[8]);
+      complementaryRoll = float(items[9]);
+      complementaryPitch = float(items[10]);
+      complementaryYaw = float(items[11]);
+  }
+  }
+}