Hi everyone. I hope that i write everything correct as this is my first time in this forum, but im currently in dispair. For a project i need to get roll/pitch/jaw. I'm using an mpu6050 accelerometer and a QMC5883L magnetometer. I display the angles on my laptop using vpython.
Im using a complimentary filter for sensorfusion.
The math is from the following website: Link
Here is my arduino code:
#include <QMC5883LCompass.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
QMC5883LCompass compass;
Adafruit_MPU6050 mpu;
int compass_x, compass_y, compass_z, compass_a, compass_b;
double accX, accY, accZ, wx, wy, wz;
double thetax, thetay, thetaz, thetaxt, thetayt, thetazt;
double time0, time1, dt;
double anglex, angley, anglez;
double alpha = 0.98;
double acc_off_x, acc_off_y, acc_off_z, w_off_x, w_off_y, w_off_z;
char myArray[3];
void setup() {
Serial.begin(115200);
while (!Serial)
delay(10); // will pause Zero, Leonardo, etc until serial console opens
Serial.println("Adafruit MPU6050 test!");
// Try to initialize!
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
Serial.println("MPU6050 Found!");
mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
mpu.setFilterBandwidth(MPU6050_BAND_260_HZ);
Serial.println("");
delay(100);
compass.init();
// zero accelerometer
Serial.println("Initializing Gyroscope and Accelerometer...");
int iteration = 0;
time0 = micros();
time1 = micros();
sensors_event_t a, g, temp;
while (time0 < time1+10000000) {
iteration++;
mpu.getEvent(&a, &g, &temp);
acc_off_x -= a.acceleration.x;
acc_off_y -= a.acceleration.y;
acc_off_z -= a.acceleration.z-9.81;
w_off_x -= g.gyro.x;
w_off_y -= g.gyro.y;
w_off_z -= g.gyro.z;
time0 = micros();
}
acc_off_x /= iteration;
acc_off_y /= iteration;
acc_off_z /= iteration;
w_off_x /= iteration;
w_off_y /= iteration;
w_off_z /= iteration;
Serial.println("Initializing done...");
Serial.print("Values: ");
Serial.print("acc_off_x: ");
Serial.print(acc_off_x, 4);
Serial.print(" acc_off_y: ");
Serial.print(acc_off_y, 4);
Serial.print(" acc_off_z: ");
Serial.print(acc_off_z, 4);
Serial.print(" w_off_x: ");
Serial.print(w_off_x, 4);
Serial.print(" w_off_y: ");
Serial.print(w_off_y, 4);
Serial.print(" w_off_z: ");
Serial.println(w_off_z, 4);
Serial.println("Starting measurements...");
delay(2000);
}
void loop() {
// Documentation: https://ahrs.readthedocs.io/en/latest/filters/complementary.html
compass.read();
compass_x = compass.getX();
compass_y = compass.getY();
compass_z = compass.getZ();
compass_a = compass.getAzimuth();
compass_b = compass.getBearing(compass_a);
compass.getDirection(myArray, compass_a);
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
accX = a.acceleration.x + acc_off_x;
accY = a.acceleration.y + acc_off_y;
accZ = a.acceleration.z + acc_off_z;
wx = g.gyro.x + w_off_x;
wy = g.gyro.y + w_off_y;
wz = g.gyro.z + w_off_z;
thetax = atan2(accY, accZ);
thetay = atan2(-accX, sqrt(accY*accY + accZ*accZ));
thetaz = atan2(compass_z*sin(thetay)-compass_y*cos(thetay), compass_x*cos(thetax)+sin(thetax)*(compass_y*sin(thetay)+compass_z*cos(thetay)));
time1 = time0;
time0 = micros();
dt = (time0 - time1) / 1000000;
thetaxt = thetaxt + wx * dt;
thetayt = thetayt + wy * dt;
thetazt = thetazt + wz * dt;
anglex = alpha * thetaxt + (1 - alpha) * thetax;
angley = alpha * thetayt + (1 - alpha) * thetay;
anglez = alpha * thetazt + (1 - alpha) * thetaz;
Serial.print(anglex, 4);
Serial.print(",");
Serial.print(angley, 4);
Serial.print(",");
Serial.println(anglez, 4);
}
and the python code:
from time import *
import numpy as np
from vpython import *
import math
import serial
serial_con = serial.Serial
def multiprint(a,b):
for i in range(0,b):
print(a)
scene.range=5
toRad=2*np.pi/360
toDeg=1/toRad
scene.forward=vector(-1,-1,-1)
scene.width=1420
scene.height=800
xarrow=arrow(lenght=2, shaftwidth=.1, color=color.red,axis=vector(1,0,0))
yarrow=arrow(lenght=2, shaftwidth=.1, color=color.green,axis=vector(0,1,0))
zarrow=arrow(lenght=4, shaftwidth=.1, color=color.blue,axis=vector(0,0,1))
frontArrow=arrow(length=4,shaftwidth=.1,color=color.purple,axis=vector(1,0,0))
upArrow=arrow(length=1,shaftwidth=.1,color=color.magenta,axis=vector(0,1,0))
sideArrow=arrow(length=2,shaftwidth=.1,color=color.orange,axis=vector(0,0,1))
bBoard=box(length=6,width=2,height=.2,opacity=.8,pos=vector(0,0,0,))
#bn=box(length=1,width=.75,height=.1, pos=vector(-.5,.1+.05,0),color=color.blue)
#nano=box(lenght=1.75,width=.6,height=.1,pos=vector(-2,.1+.05,0),color=color.green)
myObj=compound([bBoard])
multiprint(" ",2)
print("Initializing...")
multiprint(" ",2)
while True:
try:
serial_con=serial.Serial('/dev/cu.usbmodem139968001',115200)
print("Serial port opend...")
multiprint(" ",2)
sleep(1)
break
except:
pass
while True:
while serial_con.inWaiting()==0:
pass
dataPacket=serial_con.readline()
dataPacket=str(dataPacket,'utf-8')
print(dataPacket)
if "Starting measurements..." in dataPacket:
break
while True:
dataPacket=serial_con.readline()
dataPacket=str(dataPacket,'utf-8')
splitPacket=dataPacket.split(",")
roll=-float(splitPacket[1])
pitch=-float(splitPacket[0])
yaw=-float(splitPacket[2])+np.pi
print("Roll=",roll*toDeg," Pitch=",pitch*toDeg,"Yaw=",yaw*toDeg)
rate(1000)
k=vector(cos(yaw)*cos(pitch), sin(pitch),sin(yaw)*cos(pitch))
y=vector(0,1,0)
s=cross(k,y)
v=cross(s,k)
vrot=v*cos(roll)+cross(k,v)*sin(roll)
frontArrow.axis=k
sideArrow.axis=cross(k,vrot)
upArrow.axis=vrot
myObj.axis=k
myObj.up=vrot
sideArrow.length=2
frontArrow.length=4
upArrow.length=1
If i tilt it around a single axis, everything works perfectly. Even if i do that really fast. But as soon as i start tilting it arround 2 or more axis, it doesn't return to the starting-position even closely. It just points into a random direction. Does anyone have an idea? I tried everithing inclueding switching sensorfusion off completly, and even then it didn't work. Am i just stupid and the mistake is obvious? Thank you very much for any advice.