Compass | Technology Tutorials

In this video lesson we develop a Graphical Digital Compass using python and PyQt5. The data comes from a QMC5883L magnetometer connected to an arduino. The magnetometer is on the GY-87 IMU module. The arduino sends the raw data to python. With the raw data in Python, we then calibrate the sensor, and create a graphical representation of a digital compass. The circuit schematic we are using is:

MPU6050 — Schematic for connecting the GY-87 module to the Arduino

The simple program for collecting the raw data on Arduino is:

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
int x,y,z;
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;
void setup() {
Serial.begin(115200);
mpu.begin();
mpu.setI2CBypass(true);
compass.init();
}
void loop() {
 compass.read();
 x = compass.getX();
 y = compass.getY();
 z = compass.getZ();
Serial.print(x);
Serial.print(',');
Serial.print(y);
Serial.print(',');
Serial.println(z);
 delay(100);
}

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

int x,y,z;

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

Serial.begin(115200);

mpu.begin();

mpu.setI2CBypass(true);

compass.init();

}

void loop() {

compass.read();

x = compass.getX();

y = compass.getY();

z = compass.getZ();

Serial.print(x);

Serial.print(',');

Serial.print(y);

Serial.print(',');

Serial.println(z);

delay(100);

}

On the python side, this is the code we developed which both calibrates the magnetometers, and displays a Digital Compass.

import serial
import numpy as np
from PyQt5 import QtWidgets, QtCore, QtGui
import pyqtgraph as pg
import math
serialPort = 'COM8'
baudRate =115200
to =.1
ser =serial.Serial(serialPort, baudRate, timeout=to)
maxPoints = 2000
xRaw = np.empty(0)
yRaw = np.empty(0)
zRaw = np.empty(0)

xCal = np.empty(0)
yCal = np.empty(0)
zCal = np.empty(0)
x=0
y=0
z=0

calibrated = False
offsets = np.zeros(3)
scales = np.ones(3)

app = QtWidgets.QApplication([])
mainWindow = QtWidgets.QWidget()
mainWindow.setWindowTitle("Magnetometer Calibratioin")
mainLayout =QtWidgets.QVBoxLayout()
mainWindow.setLayout(mainLayout)

statusLabel = QtWidgets.QLabel("Program Running: Not Calibrated")
statusLabel.setAlignment(QtCore.Qt.AlignCenter)
statusLabel.setStyleSheet("color: green; font-weight: bold; font-size: 20px;")
mainLayout.addWidget(statusLabel)

minMaxLabel=QtWidgets.QLabel("Min/Max: N/A")
minMaxLabel.setAlignment(QtCore.Qt.AlignCenter)
minMaxLabel.setStyleSheet("color: green; font-weight: bold; font-size: 16px;")
mainLayout.addWidget(minMaxLabel)

calibrateButton = QtWidgets.QPushButton("Start Calibration")
calibrateButton.setCheckable(True)
mainLayout.addWidget(calibrateButton)


plotLayout =QtWidgets.QGridLayout()
mainLayout.addLayout(plotLayout)
xyPlot = pg.PlotWidget(title="XY Plane")
yzPlot = pg.PlotWidget(title="YZ Plane")
xzPlot = pg.PlotWidget(title="XZ Plane")
for plot in [xyPlot, yzPlot, xzPlot]:
    plot.setAspectLocked(True)
    plot.showGrid(x=True, y=True)
    plot.setMinimumSize(300,300)
plotLayout.addWidget(xyPlot, 0,0)
plotLayout.addWidget(yzPlot, 0,1)
plotLayout.addWidget(xzPlot, 1,0)

xyScatter = pg.ScatterPlotItem(size=5)
yzScatter = pg.ScatterPlotItem(size=5)
xzScatter = pg.ScatterPlotItem(size=5)

xyPlot.addItem(xyScatter)
yzPlot.addItem(yzScatter)
xzPlot.addItem(xzScatter)

headingPlot = pg.PlotWidget(title="Compass Heading")
headingPlot.setAspectLocked(True)
headingPlot.showGrid(x=True, y=True)
headingPlot.setMinimumSize(300,300)
headingPlot.hideAxis('left')
headingPlot.hideAxis('bottom')
plotLayout.addWidget(headingPlot,1,1)

compassCircle=pg.EllipseROI([-1.5,-1.5],[3.,3.],pen=pg.mkPen('b',width=4),movable=False, resizable=False)
headingPlot.addItem(compassCircle)
compassCircle.removeHandle(0)
compassCircle.removeHandle(0)
compassCircle.setZValue(0)

arrowShaft=pg.PlotCurveItem( x=[0,0],y=[0,1.5],pen=pg.mkPen('r',width=4))
headingPlot.addItem(arrowShaft)
dot=pg.ScatterPlotItem(pos=[(0,1.5)],width=4,size=10,pen=pg.mkPen('y'),brush=pg.mkBrush('y'))
dot.setZValue(1)
headingPlot.addItem(dot)

for angle, label in zip([0,90,180,270],['E','N','W','S']):
    rad = angle/360*2*math.pi
    xPos = np.cos(rad)*1.8
    yPos = np.sin(rad)*1.8
    text=pg.TextItem(label, anchor = (.5,.5), color='g')
    font=QtGui.QFont()
    font.setBold(True)
    font.setPointSize(12)
    text.setFont(font)
    text.setPos(xPos,yPos)
    headingPlot.addItem(text)
    
liveHeading = pg.TextItem("Heading: 0.0\u00B0",anchor= (.5,.5),color='w')
liveHeading.setFont(font)
liveHeading.setPos(0,-2.2)
liveHeading.setZValue(1)
headingPlot.addItem(liveHeading)


def toggleCalibration():
    global calibrated, xRaw, yRaw, zRaw, xCal, yCal, zCal, offsets, scales
    
    if calibrateButton.isChecked():
        calibrateButton.setText("Stop Calibration")
        statusLabel.setText("Mode: Calibration")
        statusLabel.setStyleSheet("color: red; font-weight: bold; font-size: 20px;")
        minMaxLabel.setText("Min/Max: collecting . . .")
        minMaxLabel.setStyleSheet("color: red; font-weight: bold; font-size: 16px;")
        calibrated = False
        xRaw = np.empty(0)
        yRaw = np.empty(0)
        zRaw = np.empty(0)
    if not calibrateButton.isChecked():
        calibrateButton.setText("Start Calibration")
        statusLabel.setText("Mode: Calibrated")
        statusLabel.setStyleSheet("color: green; font-weight: bold; font-size: 20px;")
        
        rawData = np.column_stack((xRaw,yRaw,zRaw))
        minVals = np.min(rawData, axis=0)
        maxVals = np.max(rawData, axis=0)
        offsets = (maxVals + minVals)/2
        scales = 2.0/(maxVals-minVals)
        minMaxLabel.setStyleSheet("color: green; font-weight: bold; font-size: 16px;")
        minMaxLabel.setText("Min(x,y,z: "+str(minVals.round(2))+" Max(x,y,z): "
                            +str(maxVals.round(2))+" Offsets(x,y,z): "+str(offsets.round(2))
                            +"Scales(x,y,z): "+str(scales.round(5)))
        print("Calibration Completed.")
        print("Offsets: ", offsets)
        print("Scales: ", scales)
        
        xCal = np.empty(0)
        yCal = np.empty(0)
        zCal = np.empty(0)
        calibrated = True

calibrateButton.clicked.connect(toggleCalibration)
        
def updatePlot():
    global xRaw, yRaw, zRaw, xCal, yCal, zCal
    if ser.in_waiting > 0:
        try:
            line= ser.readline().decode('utf-8').strip()
            values = line.split(',')
            if len(values)==3:
                x = float(values[0])
                y = float(values[1])
                z = float(values[2])
                if not calibrated:
                    xRaw =np.append(xRaw, x)[-maxPoints:]
                    yRaw =np.append(yRaw, y)[-maxPoints:]
                    zRaw =np.append(zRaw, z)[-maxPoints:]
                    xyScatter.setData(x=xRaw, y=yRaw, brush=pg.mkBrush(0,0,255,255))
                    yzScatter.setData(x=yRaw, y=zRaw, brush=pg.mkBrush(0,255,0,255))
                    xzScatter.setData(x=xRaw, y=zRaw, brush=pg.mkBrush(255,0,0,255))
                    
                    if len(xRaw)>10:
                        minVals = np.min(np.column_stack((xRaw,yRaw,zRaw)),axis=0)
                        maxVals = np.max(np.column_stack((xRaw,yRaw,zRaw)),axis=0)
                        
                        minMaxLabel.setStyleSheet("color: red; font-weight: bold; font-size: 16px;")
                        minMaxLabel.setText("Min(x,y,z: "+str(minVals.round(2))+" Max(x,y,z): "
                            +str(maxVals.round(2)))
                
                if calibrated:
                    xC= (x - offsets[0])*scales[0]
                    yC= (y - offsets[1])*scales[1]
                    zC= (z - offsets[2])*scales[2]
                    
                    xCal = np.append(xCal, xC)[-maxPoints:]
                    yCal = np.append(yCal, yC)[-maxPoints:]
                    zCal = np.append(zCal, zC)[-maxPoints:]
                    
                    xyScatter.setData(x=xCal, y=yCal, brush=pg.mkBrush(0,0,255,120))
                    yzScatter.setData(x=yCal, y=zCal, brush=pg.mkBrush(0,255,0,120))
                    xzScatter.setData(x=xCal, y=zCal, brush=pg.mkBrush(255,0,0,120))
                    
                    headRad=math.atan2(yC,xC)
                    headDeg=(headRad*360/2/math.pi)%360
                    compassRad= -headRad+ math.pi/2
                    compassDeg = (-headDeg + 90)%360
                    xDot=1.5*math.cos(compassRad)
                    yDot=1.5*math.sin(compassRad)
                    dot.setData(pos=([(xDot,yDot)]))
                    arrowShaft.setData(x=[0,xDot],y=[0,yDot])
                    liveHeading.setText("Heading: "+str(int(headDeg))+"\u00B0")
                    
                    
        
        except Exception as e:
            print("Parse Error: ", e)

plotTimer =QtCore.QTimer()
plotTimer.timeout.connect(updatePlot)
plotTimer.start(50)
mainWindow.show()
try:
    app.exec_()
finally:
    ser.close()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

import serial

import numpy as np

from PyQt5 import QtWidgets, QtCore, QtGui

import pyqtgraph as pg

import math

serialPort = 'COM8'

baudRate =115200

to =.1

ser =serial.Serial(serialPort, baudRate, timeout=to)

maxPoints = 2000

xRaw = np.empty(0)

yRaw = np.empty(0)

zRaw = np.empty(0)

xCal = np.empty(0)

yCal = np.empty(0)

zCal = np.empty(0)

x=0

y=0

z=0

calibrated = False

offsets = np.zeros(3)

scales = np.ones(3)

app = QtWidgets.QApplication([])

mainWindow = QtWidgets.QWidget()

mainWindow.setWindowTitle("Magnetometer Calibratioin")

mainLayout =QtWidgets.QVBoxLayout()

mainWindow.setLayout(mainLayout)

statusLabel = QtWidgets.QLabel("Program Running: Not Calibrated")

statusLabel.setAlignment(QtCore.Qt.AlignCenter)

statusLabel.setStyleSheet("color: green; font-weight: bold; font-size: 20px;")

mainLayout.addWidget(statusLabel)

minMaxLabel=QtWidgets.QLabel("Min/Max: N/A")

minMaxLabel.setAlignment(QtCore.Qt.AlignCenter)

minMaxLabel.setStyleSheet("color: green; font-weight: bold; font-size: 16px;")

mainLayout.addWidget(minMaxLabel)

calibrateButton = QtWidgets.QPushButton("Start Calibration")

calibrateButton.setCheckable(True)

mainLayout.addWidget(calibrateButton)

plotLayout =QtWidgets.QGridLayout()

mainLayout.addLayout(plotLayout)

xyPlot = pg.PlotWidget(title="XY Plane")

yzPlot = pg.PlotWidget(title="YZ Plane")

xzPlot = pg.PlotWidget(title="XZ Plane")

for plot in [xyPlot, yzPlot, xzPlot]:

plot.setAspectLocked(True)

plot.showGrid(x=True, y=True)

plot.setMinimumSize(300,300)

plotLayout.addWidget(xyPlot, 0,0)

plotLayout.addWidget(yzPlot, 0,1)

plotLayout.addWidget(xzPlot, 1,0)

xyScatter = pg.ScatterPlotItem(size=5)

yzScatter = pg.ScatterPlotItem(size=5)

xzScatter = pg.ScatterPlotItem(size=5)

xyPlot.addItem(xyScatter)

yzPlot.addItem(yzScatter)

xzPlot.addItem(xzScatter)

headingPlot = pg.PlotWidget(title="Compass Heading")

headingPlot.setAspectLocked(True)

headingPlot.showGrid(x=True, y=True)

headingPlot.setMinimumSize(300,300)

headingPlot.hideAxis('left')

headingPlot.hideAxis('bottom')

plotLayout.addWidget(headingPlot,1,1)

compassCircle=pg.EllipseROI([-1.5,-1.5],[3.,3.],pen=pg.mkPen('b',width=4),movable=False, resizable=False)

headingPlot.addItem(compassCircle)

compassCircle.removeHandle(0)

compassCircle.setZValue(0)

arrowShaft=pg.PlotCurveItem( x=[0,0],y=[0,1.5],pen=pg.mkPen('r',width=4))

headingPlot.addItem(arrowShaft)

dot=pg.ScatterPlotItem(pos=[(0,1.5)],width=4,size=10,pen=pg.mkPen('y'),brush=pg.mkBrush('y'))

dot.setZValue(1)

headingPlot.addItem(dot)

for angle, label in zip([0,90,180,270],['E','N','W','S']):

rad = angle/360*2*math.pi

xPos = np.cos(rad)*1.8

yPos = np.sin(rad)*1.8

text=pg.TextItem(label, anchor = (.5,.5), color='g')

font=QtGui.QFont()

font.setBold(True)

font.setPointSize(12)

text.setFont(font)

text.setPos(xPos,yPos)

headingPlot.addItem(text)

liveHeading = pg.TextItem("Heading: 0.0\u00B0",anchor= (.5,.5),color='w')

liveHeading.setFont(font)

liveHeading.setPos(0,-2.2)

liveHeading.setZValue(1)

headingPlot.addItem(liveHeading)

def toggleCalibration():

global calibrated, xRaw, yRaw, zRaw, xCal, yCal, zCal, offsets, scales

if calibrateButton.isChecked():

calibrateButton.setText("Stop Calibration")

statusLabel.setText("Mode: Calibration")

statusLabel.setStyleSheet("color: red; font-weight: bold; font-size: 20px;")

minMaxLabel.setText("Min/Max: collecting . . .")

minMaxLabel.setStyleSheet("color: red; font-weight: bold; font-size: 16px;")

calibrated = False

xRaw = np.empty(0)

yRaw = np.empty(0)

zRaw = np.empty(0)

if not calibrateButton.isChecked():

calibrateButton.setText("Start Calibration")

statusLabel.setText("Mode: Calibrated")

statusLabel.setStyleSheet("color: green; font-weight: bold; font-size: 20px;")

rawData = np.column_stack((xRaw,yRaw,zRaw))

minVals = np.min(rawData, axis=0)

maxVals = np.max(rawData, axis=0)

offsets = (maxVals + minVals)/2

scales = 2.0/(maxVals-minVals)

minMaxLabel.setStyleSheet("color: green; font-weight: bold; font-size: 16px;")

minMaxLabel.setText("Min(x,y,z: "+str(minVals.round(2))+" Max(x,y,z): "

+str(maxVals.round(2))+" Offsets(x,y,z): "+str(offsets.round(2))

+"Scales(x,y,z): "+str(scales.round(5)))

print("Calibration Completed.")

print("Offsets: ", offsets)

print("Scales: ", scales)

xCal = np.empty(0)

yCal = np.empty(0)

zCal = np.empty(0)

calibrated = True

calibrateButton.clicked.connect(toggleCalibration)

def updatePlot():

global xRaw, yRaw, zRaw, xCal, yCal, zCal

if ser.in_waiting > 0:

try:

line= ser.readline().decode('utf-8').strip()

values = line.split(',')

if len(values)==3:

x = float(values[0])

y = float(values[1])

z = float(values[2])

if not calibrated:

xRaw =np.append(xRaw, x)[-maxPoints:]

yRaw =np.append(yRaw, y)[-maxPoints:]

zRaw =np.append(zRaw, z)[-maxPoints:]

xyScatter.setData(x=xRaw, y=yRaw, brush=pg.mkBrush(0,0,255,255))

yzScatter.setData(x=yRaw, y=zRaw, brush=pg.mkBrush(0,255,0,255))

xzScatter.setData(x=xRaw, y=zRaw, brush=pg.mkBrush(255,0,0,255))

if len(xRaw)>10:

minVals = np.min(np.column_stack((xRaw,yRaw,zRaw)),axis=0)

maxVals = np.max(np.column_stack((xRaw,yRaw,zRaw)),axis=0)

minMaxLabel.setStyleSheet("color: red; font-weight: bold; font-size: 16px;")

minMaxLabel.setText("Min(x,y,z: "+str(minVals.round(2))+" Max(x,y,z): "

+str(maxVals.round(2)))

if calibrated:

xC= (x - offsets[0])*scales[0]

yC= (y - offsets[1])*scales[1]

zC= (z - offsets[2])*scales[2]

xCal = np.append(xCal, xC)[-maxPoints:]

yCal = np.append(yCal, yC)[-maxPoints:]

zCal = np.append(zCal, zC)[-maxPoints:]

xyScatter.setData(x=xCal, y=yCal, brush=pg.mkBrush(0,0,255,120))

yzScatter.setData(x=yCal, y=zCal, brush=pg.mkBrush(0,255,0,120))

xzScatter.setData(x=xCal, y=zCal, brush=pg.mkBrush(255,0,0,120))

headRad=math.atan2(yC,xC)

headDeg=(headRad*360/2/math.pi)%360

compassRad= -headRad+ math.pi/2

compassDeg = (-headDeg + 90)%360

xDot=1.5*math.cos(compassRad)

yDot=1.5*math.sin(compassRad)

dot.setData(pos=([(xDot,yDot)]))

arrowShaft.setData(x=[0,xDot],y=[0,yDot])

liveHeading.setText("Heading: "+str(int(headDeg))+"\u00B0")

except Exception as e:

print("Parse Error: ", e)

plotTimer =QtCore.QTimer()

plotTimer.timeout.connect(updatePlot)

plotTimer.start(50)

mainWindow.show()

try:

app.exec_()

finally:

ser.close()

In this lesson we show you how to build a demo tilt compensated compass using the BNO055 9-axis sensor. We go through some trigonometry to help you understand conceptually how the device works.

To play along at home, you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor.

The code below is provided for your convenience. It is intended only for bench top demos, and should not be used in real applications. Just for fun, not for drones, or other actual control applications.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;

float thetaG=0;
float phiG=0;

float theta;
float phi;

float thetaRad;
float phiRad;

float Xm;
float Ym;
float psi;


float dt;
unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
millisOld=millis();
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta+gyr.y()*dt)*.95+thetaM*.05;
phi=(phi-gyr.x()*dt)*.95+ phiM*.05;
thetaG=thetaG+gyr.y()*dt;
phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);
thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);
Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(acc.x()/9.8);
Serial.print(",");
Serial.print(acc.y()/9.8);
Serial.print(",");
Serial.print(acc.z()/9.8);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(",");
Serial.print(thetaM);
Serial.print(",");
Serial.print(phiM);
Serial.print(",");
Serial.print(thetaFnew);
Serial.print(",");
Serial.print(phiFnew);
Serial.print(",");
Serial.print(thetaG);
Serial.print(",");
Serial.print(phiG);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.print(phi);
Serial.print(",");
Serial.println(psi);

phiFold=phiFnew;
thetaFold=thetaFnew;

 
delay(BNO055_SAMPLERATE_DELAY_MS);
}

100

101

102

103

104

105

106

107

108

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

float thetaM;

float phiM;

float thetaFold=0;

float thetaFnew;

float phiFold=0;

float phiFnew;

float thetaG=0;

float phiG=0;

float theta;

float phi;

float thetaRad;

float phiRad;

float Xm;

float Ym;

float psi;

float dt;

unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

millisOld=millis();

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiFnew=.95*phiFold+.05*phiM;

thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;

millisOld=millis();

theta=(theta+gyr.y()*dt)*.95+thetaM*.05;

phi=(phi-gyr.x()*dt)*.95+ phiM*.05;

thetaG=thetaG+gyr.y()*dt;

phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);

thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);

Technology Tutorials

Tag Archives: Compass

Calibrated Compass Display in Python and PyQt5

9-Axis IMU LESSON 10: Making a Tilt Compensated Compass with Arduino

Making The World a Better Place One High Tech Project at a Time. Enjoy!