diff --git a/examples/RollPitchYaw/RollPitchYaw.ino b/examples/RollPitchYaw/RollPitchYaw.ino new file mode 100644 index 0000000..9e4e54c --- /dev/null +++ b/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 +#include + +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(""); +} diff --git a/examples/RollPitchYaw/data/processing/RollPitchYaw3d.pde b/examples/RollPitchYaw/data/processing/RollPitchYaw3d.pde new file mode 100644 index 0000000..d4a53b8 --- /dev/null +++ b/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]); + } + } +} diff --git a/examples/RollPitchYaw/data/processing/RollPitchYaw3d.png b/examples/RollPitchYaw/data/processing/RollPitchYaw3d.png new file mode 100644 index 0000000..5b575e9 Binary files /dev/null and b/examples/RollPitchYaw/data/processing/RollPitchYaw3d.png differ