Tag Archives: SSD1306

This is Getting Crazy Cool!

Hey guys, welcome back to the Fusion AI Lab series, AI on the Edge! In Lesson 44 we just took things up another notch. We’re now running MediaPipe Face Mesh on our Raspberry Pi 5, pulling all 468 facial landmarks in real time, and then drawing a clean, smooth face contour directly onto a tiny 128×64 OLED display!

That’s right — your face is now living on that little monochrome OLED in real time! Every eyebrow raise, smile, head tilt, and eye movement gets faithfully reproduced on the display. We’re pulling the camera feed with picamera2, processing it with MediaPipe’s powerful face mesh, scaling the landmarks down to OLED resolution, and then drawing the official FACEMESH_CONTOURS connections using PIL. The result is surprisingly smooth and fun to watch! This project really shows the power of combining modern AI vision tools with simple embedded hardware. Watching your own face rendered in real time on a tiny OLED is just plain awesome — it feels like sci-fi stuff from just a few years ago, and now we’re doing it on the Edge with our Fusion AI Lab kit!

If you’ve been following along, you’re starting to see how powerful these tools are becoming. We’re no longer just detecting faces — we’re understanding the structure of the face and visualizing it however we want. And we’re just getting started!

So fire up your Raspberry Pi, grab that OLED, and let’s keep pushing the limits. You’re doing some seriously cool stuff!

# ====================================================================
# DISCLAIMER:
# This code is provided as-is for educational and experimental 
# purposes only. The author makes no representations or warranties of 
# any kind concerning the safety, suitability, or accuracy of this 
# code. Use at your own risk. The author assumes no liability for any 
# damages, system failures, security breaches, or network issues 
# resulting from the use or implementation of this script.
# ====================================================================
 
import cv2
import time
from picamera2 import Picamera2
import mediapipe as mp
import numpy as np
#################################
import board
import adafruit_ssd1306
from PIL import Image, ImageDraw, ImageFont
import time

wOLED = 128
hOLED = 64
I2C_ADDRESS = 0x3C
i2c = board.I2C()
oled = adafruit_ssd1306.SSD1306_I2C(wOLED, hOLED, i2c, addr = I2C_ADDRESS)

#################################
W=1280
H=720
tStart = time.time()
fps = 0

piCam = Picamera2(1)
piCam.preview_configuration.main.size = (W, H)
piCam.preview_configuration.main.format = "RGB888"
piCam.preview_configuration.controls.FrameRate = 60
piCam.preview_configuration.align()
piCam.configure("preview")
piCam.start()

textLowerLeft = (int(W*.01),int(H*.05))
fontFace = cv2.FONT_HERSHEY_SIMPLEX
fontThickness = int(W/425)
fontScale = H*.0015
fontColor = (0,0,255)

faceMesh = mp.solutions.face_mesh.FaceMesh(
    
    max_num_faces=1,
    refine_landmarks=True,
    min_detection_confidence=0.5,
    min_tracking_confidence=0.5)

cv2.namedWindow('Camera',cv2.WINDOW_GUI_NORMAL)
cv2.moveWindow('Camera',0,65)
cv2.resizeWindow('Camera',W,H)
while True:
    deltaT = time.time() - tStart
    tStart=time.time()
    fps = fps*.95 + (1/deltaT)*.05
    frame = piCam.capture_array()
    frame = cv2.flip(frame, -1)

    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    results = faceMesh.process(rgb)
    
    if results.multi_face_landmarks:
        for faceLandMarks in results.multi_face_landmarks:
            #print(faceLandmarks)
            image = Image.new("1",(wOLED,hOLED))
            draw = ImageDraw.Draw(image)
            lmAll = []
            for lm in faceLandMarks.landmark:
                xOLED = int(lm.x*wOLED)
                yOLED = int(lm.y*hOLED)
                draw.point((xOLED,yOLED), fill = 255)
                #print(xOLED,yOLED)
                lmAll.append((xOLED,yOLED))
            for idx in mp.solutions.face_mesh.FACEMESH_CONTOURS:
                #print(idx)
                lineStart = idx[0]
                lineEnd = idx[1]
                pt1 = lmAll[lineStart]
                pt2 = lmAll[lineEnd]
                draw.line((pt1[0],pt1[1],pt2[0],pt2[1]), width=1, fill = 255)


            #print('XXXXXXXXXXXXXXXXXXx')
            oled.image(image)
            oled.show()
        #print()

#     if results.multi_face_landmarks:
#         for faceLandmarks in results.multi_face_landmarks:
#             # Optional second pass for glow effect
#             mp.solutions.drawing_utils.draw_landmarks(
#                 image=frame,
#                 landmark_list=faceLandmarks,
#                 connections=mp.solutions.face_mesh.FACEMESH_TESSELATION,
#                 landmark_drawing_spec=None,
#                 connection_drawing_spec=mp.solutions.drawing_utils.DrawingSpec(
#                     color=(100, 100, 100),
#                     thickness=1
#                 )
#             )
#             # === Best looking translucent mesh ===
#             mp.solutions.drawing_utils.draw_landmarks(
#                 image=frame,
#                 landmark_list=faceLandmarks,
#                 connections=mp.solutions.face_mesh.FACEMESH_CONTOURS,
#                 landmark_drawing_spec=None,                    # Hide dots
#                 connection_drawing_spec=mp.solutions.drawing_utils.DrawingSpec(
#                     color=(0, 0, 180),      # Bright cyan/teal
#                     thickness=7
#                 )
#             )
# 
#             # Optional: Extra detail on irises (makes eyes look better)
#             mp.solutions.drawing_utils.draw_landmarks(
#                 image=frame,
#                 landmark_list=faceLandmarks,
#                 connections=mp.solutions.face_mesh.FACEMESH_IRISES,
#                 landmark_drawing_spec=None,
#                 connection_drawing_spec=mp.solutions.drawing_utils.DrawingSpec(
#                     color=(255, 0, 0),
#                     thickness=7
#                 )
#             )

    myText = "FPS: "+str(round(fps,1))
    cv2.putText(frame,myText,textLowerLeft,fontFace,fontScale,fontColor,fontThickness)

    cv2.imshow("Camera", frame)

    if cv2.waitKey(1) == ord('q'):
        break

cv2.destroyAllWindows()
piCam.stop()
oled.fill(0)
oled.show()

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

# ====================================================================

# DISCLAIMER:

# This code is provided as-is for educational and experimental

# purposes only. The author makes no representations or warranties of

# any kind concerning the safety, suitability, or accuracy of this

# code. Use at your own risk. The author assumes no liability for any

# damages, system failures, security breaches, or network issues

# resulting from the use or implementation of this script.

# ====================================================================

import cv2

import time

from picamera2 import Picamera2

import mediapipe as mp

import numpy as np

#################################

import board

import adafruit_ssd1306

from PIL import Image, ImageDraw, ImageFont

import time

wOLED = 128

hOLED = 64

I2C_ADDRESS = 0x3C

i2c = board.I2C()

oled = adafruit_ssd1306.SSD1306_I2C(wOLED, hOLED, i2c, addr = I2C_ADDRESS)

#################################

W=1280

H=720

tStart = time.time()

fps = 0

piCam = Picamera2(1)

piCam.preview_configuration.main.size = (W, H)

piCam.preview_configuration.main.format = "RGB888"

piCam.preview_configuration.controls.FrameRate = 60

piCam.preview_configuration.align()

piCam.configure("preview")

piCam.start()

textLowerLeft = (int(W*.01),int(H*.05))

fontFace = cv2.FONT_HERSHEY_SIMPLEX

fontThickness = int(W/425)

fontScale = H*.0015

fontColor = (0,0,255)

faceMesh = mp.solutions.face_mesh.FaceMesh(

max_num_faces=1,

refine_landmarks=True,

min_detection_confidence=0.5,

min_tracking_confidence=0.5)

cv2.namedWindow('Camera',cv2.WINDOW_GUI_NORMAL)

cv2.moveWindow('Camera',0,65)

cv2.resizeWindow('Camera',W,H)

while True:

deltaT = time.time() - tStart

tStart=time.time()

fps = fps*.95 + (1/deltaT)*.05

frame = piCam.capture_array()

frame = cv2.flip(frame, -1)

rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)

results = faceMesh.process(rgb)

if results.multi_face_landmarks:

for faceLandMarks in results.multi_face_landmarks:

#print(faceLandmarks)

image = Image.new("1",(wOLED,hOLED))

draw = ImageDraw.Draw(image)

lmAll = []

for lm in faceLandMarks.landmark:

xOLED = int(lm.x*wOLED)

yOLED = int(lm.y*hOLED)

draw.point((xOLED,yOLED), fill = 255)

#print(xOLED,yOLED)

lmAll.append((xOLED,yOLED))

for idx in mp.solutions.face_mesh.FACEMESH_CONTOURS:

#print(idx)

lineStart = idx[0]

lineEnd = idx[1]

pt1 = lmAll[lineStart]

pt2 = lmAll[lineEnd]

draw.line((pt1[0],pt1[1],pt2[0],pt2[1]), width=1, fill = 255)

#print('XXXXXXXXXXXXXXXXXXx')

oled.image(image)

oled.show()

#print()

# if results.multi_face_landmarks:

# for faceLandmarks in results.multi_face_landmarks:

# # Optional second pass for glow effect

# mp.solutions.drawing_utils.draw_landmarks(

# image=frame,

# landmark_list=faceLandmarks,

# connections=mp.solutions.face_mesh.FACEMESH_TESSELATION,

# landmark_drawing_spec=None,

# connection_drawing_spec=mp.solutions.drawing_utils.DrawingSpec(

# color=(100, 100, 100),

# thickness=1

# )

# # === Best looking translucent mesh ===

# mp.solutions.drawing_utils.draw_landmarks(

# image=frame,

# landmark_list=faceLandmarks,

# connections=mp.solutions.face_mesh.FACEMESH_CONTOURS,

# landmark_drawing_spec=None, # Hide dots

# connection_drawing_spec=mp.solutions.drawing_utils.DrawingSpec(

# color=(0, 0, 180), # Bright cyan/teal

# thickness=7

# )

# # Optional: Extra detail on irises (makes eyes look better)

# mp.solutions.drawing_utils.draw_landmarks(

# image=frame,

# landmark_list=faceLandmarks,

# connections=mp.solutions.face_mesh.FACEMESH_IRISES,

# landmark_drawing_spec=None,

# connection_drawing_spec=mp.solutions.drawing_utils.DrawingSpec(

# color=(255, 0, 0),

# thickness=7

# )

myText = "FPS: "+str(round(fps,1))

cv2.putText(frame,myText,textLowerLeft,fontFace,fontScale,fontColor,fontThickness)

cv2.imshow("Camera", frame)

if cv2.waitKey(1) == ord('q'):

break

cv2.destroyAllWindows()

piCam.stop()

oled.fill(0)

oled.show()

In this lesson we are still using our standard Fusion AI Lab Kit cirtuit from the earlier lessons:

Fusion Hat Circuit Diagram — This is the circuit we will use moving forward in the class

We have also connected the OLED to the above circuit according to this diagram:

Arduino, Arduino 9-Axis IMU Totorials

Arduino IMU Project with Complete Avionics Display for Roll, Pitch and Yaw on a SSD1306 OLED

December 25, 2025 admin

In this video lesson we wrap up our project to create an Arduino IMU using the GY-87 IMU module, with an MPU6050 chip and a QMC5883L Magnetometer. In this lesson we complete the avionics display, creating an accurate graphical output for Roll, Pitch, and Yaw. This is the schematic we are using for this project:

OLED IMU — This schematic shows how to connect the SSD1306 OLED to our IMU Project.

This is the code we develop in the video. Remember, you have to calibrate your sensors, and put your calibration numbers into the code below. I showed you how to do the calibration in THIS LESSON.

// ====================================================================
// DISCLAIMER:
// This code is provided as-is for educational and experimental 
// purposes only. The author makes no representations or warranties of 
// any kind concerning the safety, suitability, or accuracy of this 
// code. Use at your own risk. The author assumes no liability for any 
// damages, system failures, security breaches, or network issues 
// resulting from the use or implementation of this script.
// ====================================================================
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

int screenWidth=128;
int screenHeight=64;
int rst = -1;
Adafruit_SSD1306 dsp(screenWidth, screenHeight, &Wire, rst);

float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;
float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;
float rollA,pitchA,yawM,rollC,pitchC,yawC;
float rollG = 0;
float pitchG =0 ;
float yawG = 0;
float deltaRoll = 0;
float deltaPitch = 0;
float deltaYaw = 0;
float alpha = .1;

float rollCrad;
float pitchCrad;
float MxCalH;
float MyCalH;

int tStart =millis();
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);

dsp.begin(SSD1306_SWITCHCAPVCC,0x3C);
dsp.clearDisplay();
dsp.display();

mpu.begin();
mpu.setI2CBypass(true);
compass.init();
mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
delay(100);
}

void calibrateSensors() {
    const float axOffset = 0.45407887789180545;
    const float ayOffset = 0.09432915233553185;
    const float azOffset = -0.4614157209191969;
    const float axScale = 0.10141341897341144;
    const float ayScale = 0.10141341897341144;
    const float azScale = 0.10141341897341144;
    const float gxOffset = -2.277507235645022;
    const float gyOffset = 0.8794902155258137;
    const float gzOffset = -0.5729577951308232;
    const float mxOffset = -103.48885017294003;
    const float myOffset = -96.80085481152321;
    const float mzOffset = 923.8042898128733;
    const float mxScale = 0.001156333898601669;
    const float myScale = 0.001012882116988885;
    const float mzScale = 0.0009400860982065997;


    AxCal = (AxRaw - axOffset) * axScale;
    AyCal = (AyRaw - ayOffset) * ayScale;
    AzCal = (AzRaw - azOffset) * azScale;
    GxCal = GxRaw*180/PI - gxOffset;
    GyCal = GyRaw*180/PI - gyOffset;
    GzCal = GzRaw*180/PI - gzOffset;
    MxCal = (MxRaw - mxOffset) * mxScale;
    MyCal = (MyRaw - myOffset) * myScale;
    MzCal = (MzRaw - mzOffset) * mzScale;
}
void updateDial(){
int radius=20;
int offset=3;
int x1Center = radius;
int y1Center = 63 - radius;
int x2Center = x1Center+2*radius+offset;
int y2Center = 63-radius;
int x3Center = x2Center + 2*radius +offset;
int y3Center = 63-radius;
int x1;
int y1;
int x2;
int y2;
float angleRad;
float tickScale;
dsp.drawCircle(x1Center,y1Center,radius, WHITE);
for (int i=0; i<=360; i=i+30){
  angleRad = i*PI/180;
  tickScale=.9;
  if (i%90==0){
    tickScale=.7;
  }
  x1 =x1Center + int(tickScale*radius*cos(angleRad));
  y1 =y1Center + int(tickScale*radius*sin(angleRad));
  x2 = x1Center + int(radius*cos(angleRad));
  y2 = y1Center + int(radius*sin(angleRad));
  dsp.drawLine(x1,y1,x2,y2,WHITE);
}
x1=x1Center;
y1=y1Center;
x2= x1Center + radius*cos(-(yawC+90)*PI/180);
y2= y1Center + radius*sin(-(yawC+90)*PI/180);
dsp.drawLine(x1,y1,x2,y2,WHITE);

dsp.drawCircle(x2Center,y2Center,radius,WHITE);
dsp.drawLine(x2Center-radius*cos(rollC*PI/180),y2Center-radius*sin(rollC*PI/180),x2Center+radius*cos(rollC*PI/180),y2Center+radius*sin(rollC*PI/180),WHITE);

dsp.drawCircle(x3Center,y3Center,radius,WHITE);
x1=x3Center-radius*cos(pitchC*PI/180);
y1=y3Center-radius/90.*pitchC;
x2=x3Center+radius*cos(pitchC*PI/180);
y2=y3Center-radius/90.*pitchC;
dsp.drawLine(x1,y1,x2,y2,WHITE);

}

void loop() {
  // put your main code here, to run repeatedly:
compass.read();
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;
AyRaw = a.acceleration.y;
AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;
GyRaw = g.gyro.y;
GzRaw = g.gyro.z;

MxRaw= compass.getX();
MyRaw= compass.getY();
MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;
pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;
deltaPitch = -GyCal*(millis()-tStart)/1000.;
deltaYaw = -GzCal*(millis()-tStart)/1000.;
tStart = millis();

rollG = rollG + deltaRoll;
pitchG = pitchG + deltaPitch;
yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);
pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad=rollC*PI/180;
pitchCrad=pitchC*PI/180;

MxCalH=MxCal*cos(pitchCrad)-MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);
MyCalH=MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;
yawM=int(yawM+360)%360;
if (abs(yawM-yawC)>=180){
  yawC=yawM;
}
if (abs(yawM-yawC)<180){
yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);
}

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');
// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');
// Serial.print("LL:");Serial.print(-90);Serial.print(',');
// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');
Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');
Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
Serial.print("LL:");Serial.print(-90);Serial.print(',');
Serial.print("UL:");Serial.println(90);

dsp.clearDisplay();
dsp.setTextColor(WHITE);
dsp.setCursor(0,0);
dsp.print("Ultimate Avionics");
dsp.setCursor(0,9);
dsp.print("R: "); dsp.print(int(rollC));
dsp.setCursor(40,9);
dsp.print("P: ");dsp.print(int(pitchC));
dsp.setCursor(80,9);
dsp.print("Y: ");dsp.print(int(yawC));

updateDial();

dsp.display();
delay(100);
}

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

207

208

209

210

// ====================================================================

// DISCLAIMER:

// This code is provided as-is for educational and experimental

// purposes only. The author makes no representations or warranties of

// any kind concerning the safety, suitability, or accuracy of this

// code. Use at your own risk. The author assumes no liability for any

// damages, system failures, security breaches, or network issues

// resulting from the use or implementation of this script.

// ====================================================================

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#include <math.h>

int screenWidth=128;

int screenHeight=64;

int rst = -1;

Adafruit_SSD1306 dsp(screenWidth, screenHeight, &Wire, rst);

float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;

float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;

float rollA,pitchA,yawM,rollC,pitchC,yawC;

float rollG = 0;

float pitchG =0 ;

float yawG = 0;

float deltaRoll = 0;

float deltaPitch = 0;

float deltaYaw = 0;

float alpha = .1;

float rollCrad;

float pitchCrad;

float MxCalH;

float MyCalH;

int tStart =millis();

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

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

Serial.begin(115200);

dsp.begin(SSD1306_SWITCHCAPVCC,0x3C);

dsp.clearDisplay();

dsp.display();

mpu.begin();

mpu.setI2CBypass(true);

compass.init();

mpu.setGyroRange(MPU6050_RANGE_1000_DEG);

delay(100);

}

void calibrateSensors() {

const float axOffset = 0.45407887789180545;

const float ayOffset = 0.09432915233553185;

const float azOffset = -0.4614157209191969;

const float axScale = 0.10141341897341144;

const float ayScale = 0.10141341897341144;

const float azScale = 0.10141341897341144;

const float gxOffset = -2.277507235645022;

const float gyOffset = 0.8794902155258137;

const float gzOffset = -0.5729577951308232;

const float mxOffset = -103.48885017294003;

const float myOffset = -96.80085481152321;

const float mzOffset = 923.8042898128733;

const float mxScale = 0.001156333898601669;

const float myScale = 0.001012882116988885;

const float mzScale = 0.0009400860982065997;

AxCal = (AxRaw - axOffset) * axScale;

AyCal = (AyRaw - ayOffset) * ayScale;

AzCal = (AzRaw - azOffset) * azScale;

GxCal = GxRaw*180/PI - gxOffset;

GyCal = GyRaw*180/PI - gyOffset;

GzCal = GzRaw*180/PI - gzOffset;

MxCal = (MxRaw - mxOffset) * mxScale;

MyCal = (MyRaw - myOffset) * myScale;

MzCal = (MzRaw - mzOffset) * mzScale;

}

void updateDial(){

int radius=20;

int offset=3;

int x1Center = radius;

int y1Center = 63 - radius;

int x2Center = x1Center+2*radius+offset;

int y2Center = 63-radius;

int x3Center = x2Center + 2*radius +offset;

int y3Center = 63-radius;

int x1;

int y1;

int x2;

int y2;

float angleRad;

float tickScale;

dsp.drawCircle(x1Center,y1Center,radius, WHITE);

for (int i=0; i<=360; i=i+30){

angleRad = i*PI/180;

tickScale=.9;

if (i%90==0){

tickScale=.7;

}

x1 =x1Center + int(tickScale*radius*cos(angleRad));

y1 =y1Center + int(tickScale*radius*sin(angleRad));

x2 = x1Center + int(radius*cos(angleRad));

y2 = y1Center + int(radius*sin(angleRad));

dsp.drawLine(x1,y1,x2,y2,WHITE);

}

x1=x1Center;

y1=y1Center;

x2= x1Center + radius*cos(-(yawC+90)*PI/180);

y2= y1Center + radius*sin(-(yawC+90)*PI/180);

dsp.drawLine(x1,y1,x2,y2,WHITE);

dsp.drawCircle(x2Center,y2Center,radius,WHITE);

dsp.drawLine(x2Center-radius*cos(rollC*PI/180),y2Center-radius*sin(rollC*PI/180),x2Center+radius*cos(rollC*PI/180),y2Center+radius*sin(rollC*PI/180),WHITE);

dsp.drawCircle(x3Center,y3Center,radius,WHITE);

x1=x3Center-radius*cos(pitchC*PI/180);

y1=y3Center-radius/90.*pitchC;

x2=x3Center+radius*cos(pitchC*PI/180);

y2=y3Center-radius/90.*pitchC;

dsp.drawLine(x1,y1,x2,y2,WHITE);

}

void loop() {

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

compass.read();

sensors_event_t a, g, temp;

mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;

AyRaw = a.acceleration.y;

AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;

GyRaw = g.gyro.y;

GzRaw = g.gyro.z;

MxRaw= compass.getX();

MyRaw= compass.getY();

MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;

pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;

deltaPitch = -GyCal*(millis()-tStart)/1000.;

deltaYaw = -GzCal*(millis()-tStart)/1000.;

tStart = millis();

rollG = rollG + deltaRoll;

pitchG = pitchG + deltaPitch;

yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);

pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad=rollC*PI/180;

pitchCrad=pitchC*PI/180;

MxCalH=MxCal*cos(pitchCrad)-MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);

MyCalH=MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;

yawM=int(yawM+360)%360;

if (abs(yawM-yawC)>=180){

yawC=yawM;

}

if (abs(yawM-yawC)<180){

yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);

}

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');

// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');

// Serial.print("LL:");Serial.print(-90);Serial.print(',');

// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');

Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');

Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

Serial.print("LL:");Serial.print(-90);Serial.print(',');

Serial.print("UL:");Serial.println(90);

dsp.clearDisplay();

dsp.setTextColor(WHITE);

dsp.setCursor(0,0);

dsp.print("Ultimate Avionics");

dsp.setCursor(0,9);

dsp.print("R: "); dsp.print(int(rollC));

dsp.setCursor(40,9);

dsp.print("P: ");dsp.print(int(pitchC));

dsp.setCursor(80,9);

dsp.print("Y: ");dsp.print(int(yawC));

updateDial();

dsp.display();

delay(100);

}

Arduino, Arduino 9-Axis IMU Totorials

Arduino IMU Project with SSD1306 OLED and GY-87 Module with Compass and Roll Avionics Display

December 18, 2025 admin

For your convenience, this is the code we developed in the video. Remember, you have to edit the program to use the calibration parameters for your IMU module. I showed you how to get those parameters, and calibrate your sensors in THIS LESSON.

// ====================================================================
// DISCLAIMER:
// This code is provided as-is for educational and experimental 
// purposes only. The author makes no representations or warranties of 
// any kind concerning the safety, suitability, or accuracy of this 
// code. Use at your own risk. The author assumes no liability for any 
// damages, system failures, security breaches, or network issues 
// resulting from the use or implementation of this script.
// ====================================================================
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

int screenWidth=128;
int screenHeight=64;
int rst = -1;
Adafruit_SSD1306 dsp(screenWidth, screenHeight, &Wire, rst);

float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;
float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;
float rollA,pitchA,yawM,rollC,pitchC,yawC;
float rollG = 0;
float pitchG =0 ;
float yawG = 0;
float deltaRoll = 0;
float deltaPitch = 0;
float deltaYaw = 0;
float alpha = .1;

float rollCrad;
float pitchCrad;
float MxCalH;
float MyCalH;

int tStart =millis();
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);

dsp.begin(SSD1306_SWITCHCAPVCC,0x3C);
dsp.clearDisplay();
dsp.display();

mpu.begin();
mpu.setI2CBypass(true);
compass.init();
mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
delay(100);
}

void calibrateSensors() {
    const float axOffset = 0.45407887789180545;
    const float ayOffset = 0.09432915233553185;
    const float azOffset = -0.4614157209191969;
    const float axScale = 0.10141341897341144;
    const float ayScale = 0.10141341897341144;
    const float azScale = 0.10141341897341144;
    const float gxOffset = -2.277507235645022;
    const float gyOffset = 0.8794902155258137;
    const float gzOffset = -0.5729577951308232;
    const float mxOffset = -103.48885017294003;
    const float myOffset = -96.80085481152321;
    const float mzOffset = 923.8042898128733;
    const float mxScale = 0.001156333898601669;
    const float myScale = 0.001012882116988885;
    const float mzScale = 0.0009400860982065997;


    AxCal = (AxRaw - axOffset) * axScale;
    AyCal = (AyRaw - ayOffset) * ayScale;
    AzCal = (AzRaw - azOffset) * azScale;
    GxCal = GxRaw*180/PI - gxOffset;
    GyCal = GyRaw*180/PI - gyOffset;
    GzCal = GzRaw*180/PI - gzOffset;
    MxCal = (MxRaw - mxOffset) * mxScale;
    MyCal = (MyRaw - myOffset) * myScale;
    MzCal = (MzRaw - mzOffset) * mzScale;
}
void updateDial(){
int radius=20;
int offset=3;
int x1Center = radius;
int y1Center = 63 - radius;
int x2Center = x1Center+2*radius+offset;
int y2Center = 63-radius;

int x1;
int y1;
int x2;
int y2;
float angleRad;
float tickScale;
dsp.drawCircle(x1Center,y1Center,radius, WHITE);
for (int i=0; i<=360; i=i+30){
  angleRad = i*PI/180;
  tickScale=.9;
  if (i%90==0){
    tickScale=.7;
  }
  x1 =x1Center + int(tickScale*radius*cos(angleRad));
  y1 =y1Center + int(tickScale*radius*sin(angleRad));
  x2 = x1Center + int(radius*cos(angleRad));
  y2 = y1Center + int(radius*sin(angleRad));
  dsp.drawLine(x1,y1,x2,y2,WHITE);
}
x1=x1Center;
y1=y1Center;
x2= x1Center + radius*cos(-(yawC+90)*PI/180);
y2= y1Center + radius*sin(-(yawC+90)*PI/180);
dsp.drawLine(x1,y1,x2,y2,WHITE);

dsp.drawCircle(x2Center,y2Center,radius,WHITE);
dsp.drawLine(x2Center-radius*cos(rollC*PI/180),y2Center-radius*sin(rollC*PI/180),x2Center+radius*cos(rollC*PI/180),y2Center+radius*sin(rollC*PI/180),WHITE);
}

void loop() {
  // put your main code here, to run repeatedly:
compass.read();
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;
AyRaw = a.acceleration.y;
AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;
GyRaw = g.gyro.y;
GzRaw = g.gyro.z;

MxRaw= compass.getX();
MyRaw= compass.getY();
MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;
pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;
deltaPitch = -GyCal*(millis()-tStart)/1000.;
deltaYaw = -GzCal*(millis()-tStart)/1000.;
tStart = millis();

rollG = rollG + deltaRoll;
pitchG = pitchG + deltaPitch;
yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);
pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad=rollC*PI/180;
pitchCrad=pitchC*PI/180;

MxCalH=MxCal*cos(pitchCrad)-MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);
MyCalH=MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;
yawM=int(yawM+360)%360;
if (abs(yawM-yawC)>=180){
  yawC=yawM;
}
if (abs(yawM-yawC)<180){
yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);
}

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');
// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');
// Serial.print("LL:");Serial.print(-90);Serial.print(',');
// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');
Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');
Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
Serial.print("LL:");Serial.print(-90);Serial.print(',');
Serial.print("UL:");Serial.println(90);

dsp.clearDisplay();
dsp.setTextColor(WHITE);
dsp.setCursor(0,0);
dsp.print("Ultimate Avionics");
dsp.setCursor(0,9);
dsp.print("R: "); dsp.print(int(rollC));
dsp.setCursor(40,9);
dsp.print("P: ");dsp.print(int(pitchC));
dsp.setCursor(80,9);
dsp.print("Y: ");dsp.print(int(yawC));

updateDial();

dsp.display();
delay(100);
}

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

// ====================================================================

// DISCLAIMER:

// This code is provided as-is for educational and experimental

// purposes only. The author makes no representations or warranties of

// any kind concerning the safety, suitability, or accuracy of this

// code. Use at your own risk. The author assumes no liability for any

// damages, system failures, security breaches, or network issues

// resulting from the use or implementation of this script.

// ====================================================================

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#include <math.h>

int screenWidth=128;

int screenHeight=64;

int rst = -1;

Adafruit_SSD1306 dsp(screenWidth, screenHeight, &Wire, rst);

float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;

float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;

float rollA,pitchA,yawM,rollC,pitchC,yawC;

float rollG = 0;

float pitchG =0 ;

float yawG = 0;

float deltaRoll = 0;

float deltaPitch = 0;

float deltaYaw = 0;

float alpha = .1;

float rollCrad;

float pitchCrad;

float MxCalH;

float MyCalH;

int tStart =millis();

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

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

Serial.begin(115200);

dsp.begin(SSD1306_SWITCHCAPVCC,0x3C);

dsp.clearDisplay();

dsp.display();

mpu.begin();

mpu.setI2CBypass(true);

compass.init();

mpu.setGyroRange(MPU6050_RANGE_1000_DEG);

delay(100);

}

void calibrateSensors() {

const float axOffset = 0.45407887789180545;

const float ayOffset = 0.09432915233553185;

const float azOffset = -0.4614157209191969;

const float axScale = 0.10141341897341144;

const float ayScale = 0.10141341897341144;

const float azScale = 0.10141341897341144;

const float gxOffset = -2.277507235645022;

const float gyOffset = 0.8794902155258137;

const float gzOffset = -0.5729577951308232;

const float mxOffset = -103.48885017294003;

const float myOffset = -96.80085481152321;

const float mzOffset = 923.8042898128733;

const float mxScale = 0.001156333898601669;

const float myScale = 0.001012882116988885;

const float mzScale = 0.0009400860982065997;

AxCal = (AxRaw - axOffset) * axScale;

AyCal = (AyRaw - ayOffset) * ayScale;

AzCal = (AzRaw - azOffset) * azScale;

GxCal = GxRaw*180/PI - gxOffset;

GyCal = GyRaw*180/PI - gyOffset;

GzCal = GzRaw*180/PI - gzOffset;

MxCal = (MxRaw - mxOffset) * mxScale;

MyCal = (MyRaw - myOffset) * myScale;

MzCal = (MzRaw - mzOffset) * mzScale;

}

void updateDial(){

int radius=20;

int offset=3;

int x1Center = radius;

int y1Center = 63 - radius;

int x2Center = x1Center+2*radius+offset;

int y2Center = 63-radius;

int x1;

int y1;

int x2;

int y2;

float angleRad;

float tickScale;

dsp.drawCircle(x1Center,y1Center,radius, WHITE);

for (int i=0; i<=360; i=i+30){

angleRad = i*PI/180;

tickScale=.9;

if (i%90==0){

tickScale=.7;

}

x1 =x1Center + int(tickScale*radius*cos(angleRad));

y1 =y1Center + int(tickScale*radius*sin(angleRad));

x2 = x1Center + int(radius*cos(angleRad));

y2 = y1Center + int(radius*sin(angleRad));

dsp.drawLine(x1,y1,x2,y2,WHITE);

}

x1=x1Center;

y1=y1Center;

x2= x1Center + radius*cos(-(yawC+90)*PI/180);

y2= y1Center + radius*sin(-(yawC+90)*PI/180);

dsp.drawLine(x1,y1,x2,y2,WHITE);

dsp.drawCircle(x2Center,y2Center,radius,WHITE);

dsp.drawLine(x2Center-radius*cos(rollC*PI/180),y2Center-radius*sin(rollC*PI/180),x2Center+radius*cos(rollC*PI/180),y2Center+radius*sin(rollC*PI/180),WHITE);

}

void loop() {

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

compass.read();

sensors_event_t a, g, temp;

mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;

AyRaw = a.acceleration.y;

AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;

GyRaw = g.gyro.y;

GzRaw = g.gyro.z;

MxRaw= compass.getX();

MyRaw= compass.getY();

MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;

pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;

deltaPitch = -GyCal*(millis()-tStart)/1000.;

deltaYaw = -GzCal*(millis()-tStart)/1000.;

tStart = millis();

rollG = rollG + deltaRoll;

pitchG = pitchG + deltaPitch;

yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);

pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad=rollC*PI/180;

pitchCrad=pitchC*PI/180;

MxCalH=MxCal*cos(pitchCrad)-MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);

MyCalH=MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;

yawM=int(yawM+360)%360;

if (abs(yawM-yawC)>=180){

yawC=yawM;

}

if (abs(yawM-yawC)<180){

yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);

}

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');

// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');

// Serial.print("LL:");Serial.print(-90);Serial.print(',');

// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');

Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');

Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

Serial.print("LL:");Serial.print(-90);Serial.print(',');

Serial.print("UL:");Serial.println(90);

dsp.clearDisplay();

dsp.setTextColor(WHITE);

dsp.setCursor(0,0);

dsp.print("Ultimate Avionics");

dsp.setCursor(0,9);

dsp.print("R: "); dsp.print(int(rollC));

dsp.setCursor(40,9);

dsp.print("P: ");dsp.print(int(pitchC));

dsp.setCursor(80,9);

dsp.print("Y: ");dsp.print(int(yawC));

updateDial();

dsp.display();

delay(100);

}

Arduino, Arduino 9-Axis IMU Totorials

Arduino Digital Compass Project with SSD1306 OLED Display

November 26, 2025 admin

In this video lesson we add an SSD1306 OLED to our Arduino Uno R4 WiFi IMU project. The schematic we use for the project is:

The code below is the program we develop in the video. Remember, you have to use your calibration parameters in the program below. I showed you how to calibrate your system in THIS LESSON.

// ====================================================================
// DISCLAIMER:
// This code is provided as-is for educational and experimental 
// purposes only. The author makes no representations or warranties of 
// any kind concerning the safety, suitability, or accuracy of this 
// code. Use at your own risk. The author assumes no liability for any 
// damages, system failures, security breaches, or network issues 
// resulting from the use or implementation of this script.
// ====================================================================
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

int screenWidth=128;
int screenHeight=64;
int rst = -1;
Adafruit_SSD1306 dsp(screenWidth, screenHeight, &Wire, rst);

float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;
float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;
float rollA,pitchA,yawM,rollC,pitchC,yawC;
float rollG = 0;
float pitchG =0 ;
float yawG = 0;
float deltaRoll = 0;
float deltaPitch = 0;
float deltaYaw = 0;
float alpha = .1;

float rollCrad;
float pitchCrad;
float MxCalH;
float MyCalH;

int tStart =millis();
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);

dsp.begin(SSD1306_SWITCHCAPVCC,0x3C);
dsp.clearDisplay();
dsp.display();

mpu.begin();
mpu.setI2CBypass(true);
compass.init();
mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
delay(100);
}

void calibrateSensors() {
    const float axOffset = 0.45407887789180545;
    const float ayOffset = 0.09432915233553185;
    const float azOffset = -0.4614157209191969;
    const float axScale = 0.10141341897341144;
    const float ayScale = 0.10141341897341144;
    const float azScale = 0.10141341897341144;
    const float gxOffset = -2.277507235645022;
    const float gyOffset = 0.8794902155258137;
    const float gzOffset = -0.5729577951308232;
    const float mxOffset = -103.48885017294003;
    const float myOffset = -96.80085481152321;
    const float mzOffset = 923.8042898128733;
    const float mxScale = 0.001156333898601669;
    const float myScale = 0.001012882116988885;
    const float mzScale = 0.0009400860982065997;


    AxCal = (AxRaw - axOffset) * axScale;
    AyCal = (AyRaw - ayOffset) * ayScale;
    AzCal = (AzRaw - azOffset) * azScale;
    GxCal = GxRaw*180/PI - gxOffset;
    GyCal = GyRaw*180/PI - gyOffset;
    GzCal = GzRaw*180/PI - gzOffset;
    MxCal = (MxRaw - mxOffset) * mxScale;
    MyCal = (MyRaw - myOffset) * myScale;
    MzCal = (MzRaw - mzOffset) * mzScale;
}
void loop() {
  // put your main code here, to run repeatedly:
compass.read();
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;
AyRaw = a.acceleration.y;
AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;
GyRaw = g.gyro.y;
GzRaw = g.gyro.z;

MxRaw= compass.getX();
MyRaw= compass.getY();
MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;
pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;
deltaPitch = -GyCal*(millis()-tStart)/1000.;
deltaYaw = -GzCal*(millis()-tStart)/1000.;
tStart = millis();

rollG = rollG + deltaRoll;
pitchG = pitchG + deltaPitch;
yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);
pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad=rollC*PI/180;
pitchCrad=pitchC*PI/180;

MxCalH=MxCal*cos(pitchCrad)-MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);
MyCalH=MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;
yawM=int(yawM+360)%360;
if (abs(yawM-yawC)>=180){
  yawC=yawM;
}
if (abs(yawM-yawC)<180){
yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);
}

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');
// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');
// Serial.print("LL:");Serial.print(-90);Serial.print(',');
// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');
Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');
Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
Serial.print("LL:");Serial.print(-90);Serial.print(',');
Serial.print("UL:");Serial.println(90);

dsp.clearDisplay();
dsp.setTextColor(WHITE);
dsp.setCursor(0,0);
dsp.print("Ultimate Avionics");
dsp.setCursor(0,9);
dsp.print("R: "); dsp.print(int(rollC));
dsp.setCursor(40,9);
dsp.print("P: ");dsp.print(int(pitchC));
dsp.setCursor(80,9);
dsp.print("Y: ");dsp.print(int(yawC));

dsp.display();
delay(100);
}

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

// ====================================================================

// DISCLAIMER:

// This code is provided as-is for educational and experimental

// purposes only. The author makes no representations or warranties of

// any kind concerning the safety, suitability, or accuracy of this

// code. Use at your own risk. The author assumes no liability for any

// damages, system failures, security breaches, or network issues

// resulting from the use or implementation of this script.

// ====================================================================

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#include <math.h>

int screenWidth=128;

int screenHeight=64;

int rst = -1;

Adafruit_SSD1306 dsp(screenWidth, screenHeight, &Wire, rst);

float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;

float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;

float rollA,pitchA,yawM,rollC,pitchC,yawC;

float rollG = 0;

float pitchG =0 ;

float yawG = 0;

float deltaRoll = 0;

float deltaPitch = 0;

float deltaYaw = 0;

float alpha = .1;

float rollCrad;

float pitchCrad;

float MxCalH;

float MyCalH;

int tStart =millis();

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

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

Serial.begin(115200);

dsp.begin(SSD1306_SWITCHCAPVCC,0x3C);

dsp.clearDisplay();

dsp.display();

mpu.begin();

mpu.setI2CBypass(true);

compass.init();

mpu.setGyroRange(MPU6050_RANGE_1000_DEG);

delay(100);

}

void calibrateSensors() {

const float axOffset = 0.45407887789180545;

const float ayOffset = 0.09432915233553185;

const float azOffset = -0.4614157209191969;

const float axScale = 0.10141341897341144;

const float ayScale = 0.10141341897341144;

const float azScale = 0.10141341897341144;

const float gxOffset = -2.277507235645022;

const float gyOffset = 0.8794902155258137;

const float gzOffset = -0.5729577951308232;

const float mxOffset = -103.48885017294003;

const float myOffset = -96.80085481152321;

const float mzOffset = 923.8042898128733;

const float mxScale = 0.001156333898601669;

const float myScale = 0.001012882116988885;

const float mzScale = 0.0009400860982065997;

AxCal = (AxRaw - axOffset) * axScale;

AyCal = (AyRaw - ayOffset) * ayScale;

AzCal = (AzRaw - azOffset) * azScale;

GxCal = GxRaw*180/PI - gxOffset;

GyCal = GyRaw*180/PI - gyOffset;

GzCal = GzRaw*180/PI - gzOffset;

MxCal = (MxRaw - mxOffset) * mxScale;

MyCal = (MyRaw - myOffset) * myScale;

MzCal = (MzRaw - mzOffset) * mzScale;

}

void loop() {

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

compass.read();

sensors_event_t a, g, temp;

mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;

AyRaw = a.acceleration.y;

AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;

GyRaw = g.gyro.y;

GzRaw = g.gyro.z;

MxRaw= compass.getX();

MyRaw= compass.getY();

MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;

pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;

deltaPitch = -GyCal*(millis()-tStart)/1000.;

deltaYaw = -GzCal*(millis()-tStart)/1000.;

tStart = millis();

rollG = rollG + deltaRoll;

pitchG = pitchG + deltaPitch;

yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);

pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad=rollC*PI/180;

pitchCrad=pitchC*PI/180;

MxCalH=MxCal*cos(pitchCrad)-MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);

MyCalH=MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;

yawM=int(yawM+360)%360;

if (abs(yawM-yawC)>=180){

yawC=yawM;

}

if (abs(yawM-yawC)<180){

yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);

}

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');

// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');

// Serial.print("LL:");Serial.print(-90);Serial.print(',');

// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');

Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');

Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

Serial.print("LL:");Serial.print(-90);Serial.print(',');

Serial.print("UL:");Serial.println(90);

dsp.clearDisplay();

dsp.setTextColor(WHITE);

dsp.setCursor(0,0);

dsp.print("Ultimate Avionics");

dsp.setCursor(0,9);

dsp.print("R: "); dsp.print(int(rollC));

dsp.setCursor(40,9);

dsp.print("P: ");dsp.print(int(pitchC));

dsp.setCursor(80,9);

dsp.print("Y: ");dsp.print(int(yawC));

dsp.display();

delay(100);

}

Raspberry Pi Pico

Getting Geared Up for the Ultimate GPS Tracker with Raspberry Pi Pico W

May 22, 2025 admin

GPS Project — Gearing Up for the Ultimate GPS Challenge

Guys in this class we will be building the Ultimate GPS Tracker. This will build on all you learned in our most excellent Raspberry Pi Pico W class, and now we will begin working on a real project. Hopefully most of the gear you will already have from the various kits you already have, but I give amazon links to the gear we will be using in the project. You Will Need:

Raspberry Pi Pico W: ORDER HERE
Adafruit Ultimate GPS: ORDER HERE
External Antenna (OPTIONAL): ORDER HERE
Button Battery For Quicker Fix: ORDER HERE
Soldering Iron: ORDER HERE
Solder Practice Components: ORDER HERE
Extra Solder: ORDER HERE
Safety Glasses: ORDER HERE
Breadvolt Power Supply (OPTIONAL): ORDER HERE
Charging Cable (If You Don’t Have): ORDER HERE
OLED SSD1306: (ORDER HERE)
Breadboard and Wires (If You Don’t Have): ORDER HERE

Technology Tutorials

Tag Archives: SSD1306

AI on the Edge LESSON 44: Displaying Live MediaPipe FaceMesh Avatar on SSD1306 OLED in OpenCV

Arduino IMU Project with Complete Avionics Display for Roll, Pitch and Yaw on a SSD1306 OLED

Arduino IMU Project with SSD1306 OLED and GY-87 Module with Compass and Roll Avionics Display

Arduino Digital Compass Project with SSD1306 OLED Display

Getting Geared Up for the Ultimate GPS Tracker with Raspberry Pi Pico W

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