Tag Archives: SSD1306

Hey everybody, welcome back to AI on the Edge! In Lesson 50 we’re taking things to the next level — we’re combining real-time hand tracking with a physical NeoPixel ring so you can change colors just by pointing at the screen! Using MediaPipe on the Raspberry Pi, we track your index finger tip and turn the big camera view into an interactive color picker. Hover over any of the seven big colored circles (Red, Green, Blue, Cyan, Magenta, Yellow, or Black) and — boom — the NeoPixel ring instantly lights up with that color!

This lesson is super fun because it finally brings together everything we’ve been building: smooth hand detection, coordinate mapping between camera and OLED, visual feedback, and real-world hardware control. You’ll see your finger tracked with a big blue dot, watch the selected color update live on both the screen and the OLED, and control actual RGB lights with nothing but a gesture.

By the end of this lesson you’ll have a working gesture-controlled color mixer that feels like real interactive magic. This is the kind of project that makes people say “Wow, you built that on a Raspberry Pi?!”

This is also a perfect foundation for even cooler future projects — gesture-controlled lighting, touchless interfaces, interactive games, or even a full gesture-controlled robot or light show.

So grab your Raspberry Pi, hook up that NeoPixel ring, fire up the camera, and let’s start waving our hands around like we’re casting spells! As always, the complete code is right below the video. Watch along, run the code, then start tweaking it — maybe add more colors, make the ring react differently to gestures, or combine it with the hand connection drawing from earlier lessons.I’m really excited to see what you create with this one!Now go make something awesome!

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

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

RES = (W,H)
piCam = Picamera2(1)
piCam.preview_configuration.main.size = RES
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)

hands = mp.solutions.hands.Hands(
    model_complexity =  0,
    min_detection_confidence = .5,
    min_tracking_confidence = .5,
    max_num_hands =2)
mpDraw = mp.solutions.drawing_utils

cv2.namedWindow('Camera',cv2.WINDOW_GUI_NORMAL)
cv2.moveWindow('Camera',0,65)
cv2.resizeWindow('Camera',W,H)

import neopixel_spi as neopixel
spi = board.SPI()
strip = neopixel.NeoPixel_SPI(spi, 12,
        pixel_order=neopixel.GRB,auto_write=False)
strip.fill(0)
strip.show()

xStart=int(W/2)
yStart=int(H/10)
margin=100
radius=25
radiusOutline=30
radiusFlash=35
boxW = 4*radius
boxH =int(.75*boxW)
xSpace= int((W-2*margin)/2/3)
colorDot= [(xStart-3*xSpace,yStart),(xStart-2*xSpace,yStart),(xStart-xSpace,yStart),(xStart,yStart),(xStart+xSpace,yStart),(xStart+2*xSpace,yStart),(xStart+3*xSpace,yStart)]
oledPOS=[]
for dot in colorDot:
    oledPOS.append((int(dot[0]/W*wOLED),int(dot[1]/H*hOLED*.4)))
oledText = ['R','G','B','C','M','Y','B']
colorLabel = 'B'
colors =[(0,0,255),(0,255,0),(255,0,0),(255,255,0),(255,0,255),(0,255,255),(0,0,0)]
xBox = xStart-3*xSpace
yBox = int(yStart*1.5)
boxUpperLeft = (int(xBox-radius-boxW/4),yBox)
boxLowerRight = (int(xBox-radius-boxW/4+boxW),yBox+boxH)
colorActive =(0,0,0)
inCircle = False
                 
while True:
    deltaT = time.time() - tStart
    tStart=time.time()
    fps = fps*.95 + (1/deltaT)*.05
    frame= piCam.capture_array()
    frame=cv2.flip(frame,-1)
    frame=cv2.flip(frame,1)
    image = Image.new('1',(wOLED,hOLED))
    draw = ImageDraw.Draw(image)
    for i in range(7):
        if inCircle:
            cv2.circle(frame,colorDot[i],radiusFlash,(255,255,255),-1)
        cv2.circle(frame,colorDot[i],radiusOutline,(200,200,200),-1)
        cv2.circle(frame,colorDot[i],radius,colors[i],-1)
        draw.text(oledPOS[i],oledText[i],fill=255)
    cv2.rectangle(frame,boxUpperLeft,boxLowerRight,colorActive,-1)
    rgbFrame= cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    handResults = hands.process(rgbFrame)
    if handResults.multi_hand_landmarks:
        for myHands in handResults.multi_hand_landmarks:
            landMarks = myHands.landmark
            x = int(landMarks[8].x*W)
            y = int(landMarks[8].y*H)
            xOLED = int(landMarks[8].x*wOLED)
            yOLED = int(landMarks[8].y*hOLED)
            cv2.circle(frame,(x,y),10,(255,0,0), -1)
            draw.ellipse((xOLED-3,yOLED-4,xOLED+3,yOLED+4),fill=255)
            cnt = 0
            for pos in colorDot:
                if abs(x-pos[0])<radius and abs(y-pos[1])<radius:
                    colorActive=colors[cnt]
                    colorLabel =oledText[cnt]
                    #draw.text((int(wOLED/2),int(hOLED/2)),oledText[cnt],fill=255)
                    strip.fill((colorActive[2],colorActive[1],colorActive[0]))
                    strip.show()
                               
                cnt=cnt+1
    draw.text((int(wOLED/2),int(hOLED/2)),colorLabel,fill=255)                    
    oled.image(image)
    oled.show()
    myText = "FPS: "+str(round(fps,1))
    cv2.putText(frame,myText,textLowerLeft,fontFace,fontScale,fontColor,fontThickness)
    cv2.imshow("Camera", frame)
    cv2.moveWindow("Camera",0,60)
    if cv2.waitKey(1)==ord('q'):
        break
cv2.destroyAllWindows()
print('Program Terminated')

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

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

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

import adafruit_ssd1306

from PIL import Image,ImageDraw,ImageFont

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

RES = (W,H)

piCam = Picamera2(1)

piCam.preview_configuration.main.size = RES

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)

hands = mp.solutions.hands.Hands(

model_complexity = 0,

min_detection_confidence = .5,

min_tracking_confidence = .5,

max_num_hands =2)

mpDraw = mp.solutions.drawing_utils

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

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

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

import neopixel_spi as neopixel

spi = board.SPI()

strip = neopixel.NeoPixel_SPI(spi, 12,

pixel_order=neopixel.GRB,auto_write=False)

strip.fill(0)

strip.show()

xStart=int(W/2)

yStart=int(H/10)

margin=100

radius=25

radiusOutline=30

radiusFlash=35

boxW = 4*radius

boxH =int(.75*boxW)

xSpace= int((W-2*margin)/2/3)

colorDot= [(xStart-3*xSpace,yStart),(xStart-2*xSpace,yStart),(xStart-xSpace,yStart),(xStart,yStart),(xStart+xSpace,yStart),(xStart+2*xSpace,yStart),(xStart+3*xSpace,yStart)]

oledPOS=[]

for dot in colorDot:

oledPOS.append((int(dot[0]/W*wOLED),int(dot[1]/H*hOLED*.4)))

oledText = ['R','G','B','C','M','Y','B']

colorLabel = 'B'

colors =[(0,0,255),(0,255,0),(255,0,0),(255,255,0),(255,0,255),(0,255,255),(0,0,0)]

xBox = xStart-3*xSpace

yBox = int(yStart*1.5)

boxUpperLeft = (int(xBox-radius-boxW/4),yBox)

boxLowerRight = (int(xBox-radius-boxW/4+boxW),yBox+boxH)

colorActive =(0,0,0)

inCircle = False

while True:

deltaT = time.time() - tStart

tStart=time.time()

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

frame= piCam.capture_array()

frame=cv2.flip(frame,-1)

frame=cv2.flip(frame,1)

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

draw = ImageDraw.Draw(image)

for i in range(7):

if inCircle:

cv2.circle(frame,colorDot[i],radiusFlash,(255,255,255),-1)

cv2.circle(frame,colorDot[i],radiusOutline,(200,200,200),-1)

cv2.circle(frame,colorDot[i],radius,colors[i],-1)

draw.text(oledPOS[i],oledText[i],fill=255)

cv2.rectangle(frame,boxUpperLeft,boxLowerRight,colorActive,-1)

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

handResults = hands.process(rgbFrame)

if handResults.multi_hand_landmarks:

for myHands in handResults.multi_hand_landmarks:

landMarks = myHands.landmark

x = int(landMarks[8].x*W)

y = int(landMarks[8].y*H)

xOLED = int(landMarks[8].x*wOLED)

yOLED = int(landMarks[8].y*hOLED)

cv2.circle(frame,(x,y),10,(255,0,0), -1)

draw.ellipse((xOLED-3,yOLED-4,xOLED+3,yOLED+4),fill=255)

cnt = 0

for pos in colorDot:

if abs(x-pos[0])<radius and abs(y-pos[1])<radius:

colorActive=colors[cnt]

colorLabel =oledText[cnt]

#draw.text((int(wOLED/2),int(hOLED/2)),oledText[cnt],fill=255)

strip.fill((colorActive[2],colorActive[1],colorActive[0]))

strip.show()

cnt=cnt+1

draw.text((int(wOLED/2),int(hOLED/2)),colorLabel,fill=255)

oled.image(image)

oled.show()

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

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

cv2.imshow("Camera", frame)

cv2.moveWindow("Camera",0,60)

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

break

cv2.destroyAllWindows()

print('Program Terminated')

This is the core circuit we use for the class:

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

This is the schematic to add the OLED:

Then this is the schematic to add the NeoPixel

AI On the Edge, Raspberry Pi

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

August 23, 2026 admin

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:

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

Technology Tutorials

Tag Archives: SSD1306

AI on the Edge LESSON 50: Control NeoPixel Ring With Hand Gestures and MediaPipe

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

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