Tag Archives: OLED

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:

AI on the Edge LESSON 43: Adding SSD1306 OLED to Your Raspberry Pi Projects

August 19, 2026 admin

In today’s lesson in the AI on the Edge series, we’re adding a very important piece to our growing collection of skills — learning how to use the SSD1306 OLED display with the SunFounder Fusion AI Lab kit.

One of the key goals of this class is to build intelligent systems that can run completely on the edge — without needing to rely on the cloud. A big part of that is giving our edge devices the ability to communicate clearly with us. That’s exactly where the little SSD1306 OLED really shines. Whether you’re building a smart robot, an autonomous sensor node, or an AI-powered monitoring system, having a crisp, low-power display to show status, results, or even fun animations makes your project feel complete and professional.

In this video, we start with the basics and gradually build up. You’ll learn how to connect the OLED using I2C, initialize it with CircuitPython, and then use the PIL library to create images in memory before sending them to the screen. We explore loading different font sizes so you can create nice, readable layouts. Then we move into drawing shapes — rectangles, circles, and borders — before finishing with a fun bouncing ball animation that brings the whole thing to life.

This lesson is particularly important in our AI on the Edge journey because the SSD1306 is extremely lightweight and uses almost no processing power, making it perfect for running alongside face detection, object tracking, speech recognition, and other AI tasks without slowing down your Raspberry Pi. You’ll see how easy it is to display messages like detection results, confidence scores, system status, or even playful personality messages from your AI creations.

By the end of this lesson, you’ll have the confidence to add a real display to any project in this class. Whether you want to show live face tracking data, sensor readings, or just give your robot a fun way to “talk” to the world, the skills you learn here will be used again and again in future projects.

So if you’re following along with the AI on the Edge series, this is another big step forward. Grab your Fusion AI Hat, open up Thonny, and let’s get that OLED screen lighting up with some personality!

As always, I strongly encourage you to take the code and make it your own. Change the messages, create new animations, and think about how you can use this display in your own AI projects. That’s where the real learning and creativity happens

So far in this class we have been using this schematic:

In this lesson, we will be adding the SSD1306 OLED display. Keep the schatic above, but now add the OLED display to the breadboard. It should be connected to the Fusion Hat as follows:

Connect the VCC pin of OLED display to 3.3V on Fusion HAT+
Connect the GND pin of OLED display to GND on Fusion HAT+
Connect the SCL pin of OLED display to SCL (GPIO 3) on Fusion HAT+
Connect the SDA pin of OLED display to SDA (GPIO 2) on Fusion HAT+

While we are updating our project components, go ahead and connect the neoPixel ring to yout Fusion Hat. Here is the schematic to add the neopixel ring, and we will be using it in future lessons.

In the video, we developed the following code to show you how to put the SSD1306 OLED through its paces using the Fusion AI Hat on the Raspberry Pi 5.

# ====================================================================
# 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 board
import adafruit_ssd1306
from PIL import Image, ImageDraw, ImageFont
import time

W =128
H = 64
I2C_ADDRESS = 0x3C

i2c = board.I2C()
oled = adafruit_ssd1306.SSD1306_I2C(W, H, i2c, addr=I2C_ADDRESS)

fontLarge = ImageFont.truetype("/usr/share/fonts/truetype/dejavu/DejaVuSans-Bold.ttf",14)
fontMedium = ImageFont.truetype("/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",10)
fontSmall = ImageFont.truetype("/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",8)

image = Image.new("1", (W,H))
draw = ImageDraw.Draw(image)
draw.point((int(W/2),int(H/2)), fill =255)
oled.image(image)
oled.show()
ts=1
time.sleep(ts)

image = Image.new("1", (W,H))
draw = ImageDraw.Draw(image)
draw.text( (0,0), "TopTechBoy.com",font=fontLarge, fill=255)
draw.text( (0,16), "SSD1306 OLED",font=fontMedium, fill=255)
draw.text( (0,26), "Fusion AI Hat",font=fontMedium, fill=255)
draw.text( (0,36), "Hello Makers",font=fontMedium, fill=255)
draw.text( (0,46), "Enjoy!",font=fontSmall, fill=255)
oled.image(image)
oled.show()
time.sleep(ts)

image = Image.new("1", (W,H))
draw = ImageDraw.Draw(image)
draw.rectangle((int(.1*W),int(.1*H),int(.9*W),int(.9*H)),
               width=5, outline=255, fill=0)
draw.text((int(W/4),int(H/2.5)),
          "Hello World",font=fontMedium, fill=255)

oled.image(image)
oled.show()
time.sleep(ts)

image = Image.new("1", (W,H))
draw = ImageDraw.Draw(image)
draw.rectangle((int(.1*W),int(.1*H),int(.45*W),int(.45*H)),width=1, outline=255, fill=0)
draw.rectangle((int(.55*W),int(.1*H),int(.9*W),int(.45*H)),width=1, outline=255, fill=255)
d=25
draw.ellipse((int(.1*W),int(.55*H),int(.1*W)+d,int(.55*H)+d),width=1, outline=255, fill=0)
draw.ellipse((int(.55*W),int(.55*H),int(.55*W)+d,int(.55*H)+d),width=1, outline=255, fill=255)
oled.image(image)
oled.show()
time.sleep(ts)

xCenter=40
yCenter=25
radius=7

dx = 1
dy = 1

for frame in range(500):
    image = Image.new("1", (W,H))
    draw = ImageDraw.Draw(image)
    draw.rectangle((0,0,W-1,H-1), width=1, outline = 255, fill=0) 
    draw.ellipse((xCenter-radius,yCenter-radius,xCenter+radius,yCenter+radius), width=1,outline=255,fill=255)
    #draw.rectangle((0,0,W-1,H-1), width=1, outline = 255, fill=0)    
    oled.image(image)
    oled.show()
    
    xCenter = xCenter + dx
    yCenter = yCenter + dy

    if xCenter <= radius or xCenter >= W-1-radius:
        dx = -dx
    if yCenter <= radius or yCenter >= H-1-radius:
        dy = -dy

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

# 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 board

import adafruit_ssd1306

from PIL import Image, ImageDraw, ImageFont

import time

W =128

H = 64

I2C_ADDRESS = 0x3C

i2c = board.I2C()

oled = adafruit_ssd1306.SSD1306_I2C(W, H, i2c, addr=I2C_ADDRESS)

fontLarge = ImageFont.truetype("/usr/share/fonts/truetype/dejavu/DejaVuSans-Bold.ttf",14)

fontMedium = ImageFont.truetype("/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",10)

fontSmall = ImageFont.truetype("/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",8)

image = Image.new("1", (W,H))

draw = ImageDraw.Draw(image)

draw.point((int(W/2),int(H/2)), fill =255)

oled.image(image)

oled.show()

ts=1

time.sleep(ts)

image = Image.new("1", (W,H))

draw = ImageDraw.Draw(image)

draw.text( (0,0), "TopTechBoy.com",font=fontLarge, fill=255)

draw.text( (0,16), "SSD1306 OLED",font=fontMedium, fill=255)

draw.text( (0,26), "Fusion AI Hat",font=fontMedium, fill=255)

draw.text( (0,36), "Hello Makers",font=fontMedium, fill=255)

draw.text( (0,46), "Enjoy!",font=fontSmall, fill=255)

oled.image(image)

oled.show()

time.sleep(ts)

image = Image.new("1", (W,H))

draw = ImageDraw.Draw(image)

draw.rectangle((int(.1*W),int(.1*H),int(.9*W),int(.9*H)),

width=5, outline=255, fill=0)

draw.text((int(W/4),int(H/2.5)),

"Hello World",font=fontMedium, fill=255)

oled.image(image)

oled.show()

time.sleep(ts)

image = Image.new("1", (W,H))

draw = ImageDraw.Draw(image)

draw.rectangle((int(.1*W),int(.1*H),int(.45*W),int(.45*H)),width=1, outline=255, fill=0)

draw.rectangle((int(.55*W),int(.1*H),int(.9*W),int(.45*H)),width=1, outline=255, fill=255)

d=25

draw.ellipse((int(.1*W),int(.55*H),int(.1*W)+d,int(.55*H)+d),width=1, outline=255, fill=0)

draw.ellipse((int(.55*W),int(.55*H),int(.55*W)+d,int(.55*H)+d),width=1, outline=255, fill=255)

oled.image(image)

oled.show()

time.sleep(ts)

xCenter=40

yCenter=25

radius=7

dx = 1

dy = 1

for frame in range(500):

image = Image.new("1", (W,H))

draw = ImageDraw.Draw(image)

draw.rectangle((0,0,W-1,H-1), width=1, outline = 255, fill=0)

draw.ellipse((xCenter-radius,yCenter-radius,xCenter+radius,yCenter+radius), width=1,outline=255,fill=255)

#draw.rectangle((0,0,W-1,H-1), width=1, outline = 255, fill=0)

oled.image(image)

oled.show()

xCenter = xCenter + dx

yCenter = yCenter + dy

if xCenter <= radius or xCenter >= W-1-radius:

dx = -dx

if yCenter <= radius or yCenter >= H-1-radius:

dy = -dy

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);

Technology Tutorials

Tag Archives: OLED

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

AI on the Edge LESSON 43: Adding SSD1306 OLED to Your Raspberry Pi Projects

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

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