All posts by admin

In this video lesson we show how to create a tilt compensated digital compass. Calculating heading based simply on the measured magnetometer values in the X and Y directions only works accurately when the compass is sitting flat, or horizontal with the earth’s surface. If we introduce a tilt, either by applying pitch or roll to the system, calculated heading, or yaw will no longer be accurate. In the video above, we show you how to mathematically ‘un-tilt’ the sensor to get accurate heading readings when the device is not perfectly flat.

We are working with a GY-87 9-axis IMU, and an Arduino Uno R4 WiFi. Below is the schematic we are using in this project:

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

For your convenience, the code developed in this video lesson is included below. Please notice that the calibration constants in the code below are for my GY-87 module. You need to calibrate your own module, as my numbers below would likely be different from your numbers. We showed how to do the calibration in THIS LESSON.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
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);
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;
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);

delay(100);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

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

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;

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

delay(100);

}

Arduino

High Performance Roll, Pitch and Yaw values from the GY-87 IMU Module

November 6, 2025 admin

In this video lesson I show you how to use calibrated sensors and Complimentary Filters to perform Sensor Fusion to get high performance IMU data from the GY-87 IMU module. We end up with Roll, Pitch and Yaw that is fast, accurate, low noise, and no drift. The work we do in this lesson uses the calibration data generated in last weeks lesson, if you have not completed that lesson you need to do it before proceeding here. The schematic we are using in this lesson is:

This is the code we developed in this weeks lesson. Note that in the callibrateSensors() function, you need to use the calibration parameters for your module (as explained in last weeks lesson).

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
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;
int tStart =millis();
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
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;

yawM = atan2(MyCal,MxCal)*180./PI;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);
pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);
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);

delay(100);
}

100

101

102

103

104

105

106

107

108

109

110

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

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;

int tStart =millis();

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

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

Serial.begin(115200);

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;

yawM = atan2(MyCal,MxCal)*180./PI;

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

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

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

delay(100);

}

Arduino, Python

Ultimate 9-axis Program for Easily and Accurately Calibrating a 9-axis IMU on Arduino

October 23, 2025 admin

In this video lesson we show how to easily calibrate a 9-axis IMU. We are using the GY-87 IMU module which contains a MPU6050 for measuring acceleration and rotational velocity, and the QMC5883L magnetometer. In this work, we have three programs. The first is simple arduino program for measuring and printing the data from the 9 sensors. Then, the second program is a python program on your PC which will allow you to simply and accurately calibrate the 9 sensors, from the data coming from the first program. Then the third program is a program on the arduino that reads the data from the sensors, and then uses the calibration data that was generated to create accurate, calibrated sensor data.

This is the schematic of the circuit we are working with:

Then this is the arduino code we use to calibrate the sensors:

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
mpu.begin();
mpu.setI2CBypass(true);
compass.init();
mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
delay(100);
}

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

Serial.print(AxRaw);
Serial.print(',');
Serial.print(AyRaw);
Serial.print(',');
Serial.print(AzRaw);
Serial.print(',');
Serial.print(GxRaw);
Serial.print(',');
Serial.print(GyRaw);
Serial.print(',');
Serial.print(GzRaw);
Serial.print(',');
Serial.print(MxRaw);
Serial.print(',');
Serial.print(MyRaw);
Serial.print(',');
Serial.println(MzRaw);
delay(100);
}

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

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

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

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

Serial.begin(115200);

mpu.begin();

mpu.setI2CBypass(true);

compass.init();

mpu.setGyroRange(MPU6050_RANGE_1000_DEG);

delay(100);

}

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

Serial.print(AxRaw);

Serial.print(',');

Serial.print(AyRaw);

Serial.print(',');

Serial.print(AzRaw);

Serial.print(',');

Serial.print(GxRaw);

Serial.print(',');

Serial.print(GyRaw);

Serial.print(',');

Serial.print(GzRaw);

Serial.print(',');

Serial.print(MxRaw);

Serial.print(',');

Serial.print(MyRaw);

Serial.print(',');

Serial.println(MzRaw);

delay(100);

}

This next program is to be run on your PC. It is a python program that will read the data coming from the arduino, and will then help you calibrate your sensors.

import serial
import numpy as np
from PyQt5 import QtWidgets, QtCore, QtGui
import pyqtgraph as pg
import pyqtgraph.opengl as gl
from pyqtgraph.opengl import GLViewWidget, GLLinePlotItem, GLMeshItem
import time
import os
import math# --- Serial Port Configuration ---
# Strategy: Set up serial communication with the IMU to receive raw sensor data:
# ax, ay, az (m/s²), gx, gy, gz (rad/s), mx, my, mz (arbitrary units from QMC5883L).
# Convert gyro data to °/s immediately after reading for consistency with plots and calibration.
serialPort = 'COM8'  # Serial port for IMU communication (adjust as needed)
baudRate = 115200  # Baud rate matching IMU configuration
timeout = 0.1  # Serial read timeout in seconds
accMagMaxPoints = 1000  # Max points for accelerometer/magnetometer plots
gyroMaxPoints = 100  # Max points for gyroscope plots
trailMaxPoints = 100  # Max points for 3D magnetic trail
gyroTimeWindow = 5.0  # Time window for gyro plots in seconds
alpha = 0.3  # Low-pass filter constant for smoothing accelerometer/magnetometer data
epsilon = 1e-6  # Small constant to prevent division by zero in calculations# --- Calibration Variables ---
# Strategy: Store calibration parameters to correct sensor biases and scales.
# Gyro offsets are in °/s (converted from rad/s on input).
# Accelerometer in m/s², magnetometer in µT after calibration.
gxOffset = 0.0  # Gyroscope x-axis offset (°/s)
gyOffset = 0.0  # Gyroscope y-axis offset (°/s)
gzOffset = 0.0  # Gyroscope z-axis offset (°/s)
accCalibrating = False  # Flag for accelerometer calibration mode
magCalibrating = False  # Flag for magnetometer calibration mode
accMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}  # Min accelerometer readings (m/s²)
accMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}  # Max accelerometer readings (m/s²)
magMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}  # Min magnetometer readings (arb. units)
magMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}  # Max magnetometer readings (arb. units)
magRawMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}  # Min raw magnetometer readings
magRawMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}  # Max raw magnetometer readings
accOffset = {'x': 0.0, 'y': 0.0, 'z': 0.0}  # Accelerometer offsets (m/s²)
accScale = {'x': 1.0, 'y': 1.0, 'z': 1.0}  # Accelerometer scales
magOffset = {'x': 0.0, 'y': 0.0, 'z': 0.0}  # Magnetometer offsets (arb. units)
magScale = {'x': 1.0, 'y': 1.0, 'z': 1.0}  # Magnetometer scales (to µT)
accFiltered = {'x': None, 'y': None, 'z': None}  # Filtered accelerometer data (m/s²)
magFiltered = {'x': None, 'y': None, 'z': None}  # Filtered magnetometer data (arb. units)
axSamples = []  # Accelerometer x-axis samples during calibration
aySamples = []  # Accelerometer y-axis samples during calibration
azSamples = []  # Accelerometer z-axis samples during calibration
mxSamples = []  # Magnetometer x-axis samples during calibration
mySamples = []  # Magnetometer y-axis samples during calibration
mzSamples = []  # Magnetometer z-axis samples during calibration
trailBuffer = []  # Buffer for 3D magnetic vector trail# --- Data Storage ---
# Strategy: Store raw and calibrated sensor data in numpy arrays for efficient plotting.
# Gyro data is stored in °/s after conversion from rad/s.
accRaw = {'x': np.empty(0), 'y': np.empty(0), 'z': np.empty(0)}  # Raw accelerometer data (m/s²)
gyroRaw = {'x': np.empty(0), 'y': np.empty(0), 'z': np.empty(0)}  # Calibrated gyroscope data (°/s)
magRaw = {'x': np.empty(0), 'y': np.empty(0), 'z': np.empty(0)}  # Calibrated magnetometer data (µT)
timePoints = np.empty(0)  # Timestamps for gyro plots
startTime = QtCore.QTime.currentTime()  # Start time for elapsed time calculation# --- Serial Connection Initialization ---
try:
    ser = serial.Serial(serialPort, baudRate, timeout=timeout)  # Open serial port
    ser.reset_input_buffer()  # Clear input buffer to remove stale data
except serial.SerialException as e:
    msg = QtWidgets.QMessageBox()
    msg.setWindowTitle("Serial Error")
    msg.setText(f"Failed to open {serialPort}: {str(e)}\nCheck: 1) Port in Device Manager, 2) Close Arduino IDE Serial Monitor, 3) Run as admin")
    msg.exec_()
    exit(1)# --- Generate Arduino Code ---
# Strategy: Generate Arduino code with calibration parameters for onboard processing.
# Gyro offsets are in °/s to match Python processing.

def generateArduinoCode(to_file=False):
    code = [
        "void calibrateSensors() {",
        "    const float axOffset = " + str(accOffset['x']) + ";",
        "    const float ayOffset = " + str(accOffset['y']) + ";",
        "    const float azOffset = " + str(accOffset['z']) + ";",
        "    const float axScale = " + str(accScale['x']) + ";",
        "    const float ayScale = " + str(accScale['y']) + ";",
        "    const float azScale = " + str(accScale['z']) + ";",
        "    const float gxOffset = " + str(gxOffset) + ";",
        "    const float gyOffset = " + str(gyOffset) + ";",
        "    const float gzOffset = " + str(gzOffset) + ";",
        "    const float mxOffset = " + str(magOffset['x']) + ";",
        "    const float myOffset = " + str(magOffset['y']) + ";",
        "    const float mzOffset = " + str(magOffset['z']) + ";",
        "    const float mxScale = " + str(magScale['x']) + ";",
        "    const float myScale = " + str(magScale['y']) + ";",
        "    const float mzScale = " + str(magScale['z']) + ";",
        "",
        "    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;",
        "}"
    ]
    if to_file:
        try:
            file_path = os.path.join(os.getcwd(), 'calData.txt')
            with open(file_path, 'w', encoding='utf-8') as f:
                f.write("\n".join(code) + "\n")
            if os.path.exists(file_path) and os.path.getsize(file_path) > 0:
                with open(file_path, 'r', encoding='utf-8') as f:
                    if f.read().strip():
                        return True
                    raise Exception("File is empty")
            raise Exception("File was not created or is inaccessible")
        except Exception as e:
            msg = QtWidgets.QMessageBox()
            msg.setWindowTitle("File Write Error")
            msg.setText("Error writing calData.txt: " + str(e))
            msg.exec_()
            return False
    return True# --- Load Calibration Files ---
def loadCalibration():
    global gxOffset, gyOffset, gzOffset, accOffset, accScale, magOffset, magScale, plots, mag3DView
    loaded = []
    try:
        if os.path.exists('gCal.txt'):
            with open('gCal.txt', 'r') as f:
                values = list(map(float, f.readline().strip().split(',')))
                if len(values) == 3:
                    gxOffset, gyOffset, gzOffset = values  # Load gyro offsets (°/s)
                    loaded.append(f"Gyro (°/s): Gx={gxOffset:.6f}, Gy={gyOffset:.6f}, Gz={gzOffset:.6f}")
        if os.path.exists('accCal.txt'):
            with open('accCal.txt', 'r') as f:
                values = list(map(float, f.readline().strip().split(',')))
                if len(values) == 6:
                    accOffset['x'], accScale['x'], accOffset['y'], accScale['y'], accOffset['z'], accScale['z'] = values
                    for key in ['accXY', 'accYZ', 'accXZ']:
                        plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2))  # ±1.2 m/s²
                    loaded.append(f"Acc (m/s²): Offsets x={accOffset['x']:.6f}, y={accOffset['y']:.6f}, z={accOffset['z']:.6f}, " +
                                 f"Scales x={accScale['x']:.6f}, y={accScale['y']:.6f}, z={accScale['z']:.6f}")
        if os.path.exists('magCal.txt'):
            with open('magCal.txt', 'r') as f:
                values = list(map(float, f.readline().strip().split(',')))
                if len(values) == 6:
                    magOffset['x'], magScale['x'], magOffset['y'], magScale['y'], magOffset['z'], magScale['z'] = values
                    for key in ['magXY', 'magYZ', 'magXZ']:
                        plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2))  # ±1.2 µT
                    mag3DView.opts['distance'] = 3
                    mag3DView.setCameraPosition(distance=3, elevation=30, azimuth=45)
                    loaded.append(f"Mag (µT): Offsets x={magOffset['x']:.6f}, y={magOffset['y']:.6f}, z={magOffset['z']:.6f}, " +
                                 f"Scales x={magScale['x']:.6f}, y={magScale['y']:.6f}, z={magScale['z']:.6f}")
        if loaded:
            msg = QtWidgets.QMessageBox()
            msg.setWindowTitle("Calibration Loaded")
            msg.setText("Loaded calibration:\n" + "\n".join(loaded))
            msg.exec_()
            generateArduinoCode()
        else:
            msg = QtWidgets.QMessageBox()
            msg.setWindowTitle("No Calibration Found")
            msg.setText("No calibration files found. Please calibrate the sensors.")
            msg.exec_()
    except Exception as e:
        msg = QtWidgets.QMessageBox()
        msg.setWindowTitle("Calibration Load Error")
        msg.setText(f"Error loading calibration files: {str(e)}")
        msg.exec_()# --- Save Calibration ---
def saveCalibration():
    global gxOffset, gyOffset, gzOffset, accOffset, accScale, magOffset, magScale
    saved = []
    try:
        with open('gCal.txt', 'w') as f:
            f.write(f"{gxOffset},{gyOffset},{gzOffset}\n")
            saved.append(f"Gyro (°/s): Gx={gxOffset:.6f}, Gy={gyOffset:.6f}, Gz={gzOffset:.6f}")
        with open('accCal.txt', 'w') as f:
            f.write(f"{accOffset['x']},{accScale['x']},{accOffset['y']},{accScale['y']},{accOffset['z']},{accScale['z']}\n")
            saved.append(f"Acc (m/s²): Offsets x={accOffset['x']:.6f}, y={accOffset['y']:.6f}, z={accOffset['z']:.6f}, " +
                        f"Scales x={accScale['x']:.6f}, y={accScale['y']:.6f}, z={accScale['z']:.6f}")
        with open('magCal.txt', 'w') as f:
            f.write(f"{magOffset['x']},{magScale['x']},{magOffset['y']},{magScale['y']},{magOffset['z']},{magScale['z']}\n")
            saved.append(f"Mag (µT): Offsets x={magOffset['x']:.6f}, y={magOffset['y']:.6f}, z={magOffset['z']:.6f}, " +
                        f"Scales x={magScale['x']:.6f}, y={magScale['y']:.6f}, z={magScale['z']:.6f}")
        msg = QtWidgets.QMessageBox()
        msg.setWindowTitle("Calibration Saved")
        msg.setText("Saved calibration:\n" + "\n".join(saved))
        msg.exec_()
        generateArduinoCode()
    except Exception as e:
        msg = QtWidgets.QMessageBox()
        msg.setWindowTitle("Calibration Save Error")
        msg.setText(f"Error saving calibration files: {str(e)}")
        msg.exec_()# --- GUI Setup - Main Window ---
app = QtWidgets.QApplication([])
mainWindow = QtWidgets.QWidget()
mainWindow.setWindowTitle("9-Axis IMU Visualization (Acc/Mag: 1000 pts, Gyro: 100 pts)")
mainWindow.setMinimumSize(1000, 800)
mainLayout = QtWidgets.QVBoxLayout()
mainWindow.setLayout(mainLayout)
plotGrid = QtWidgets.QGridLayout()
plots = {}
scatterItems = {}
curveItems = {}
attitudeItems = {}
plotTitles = [
    ('accXY', 'Acc XY (m/s²)', 'X', 'Y', (255, 0, 0, 255)),  # Red
    ('accYZ', 'Acc YZ (m/s²)', 'Y', 'Z', (0, 255, 0, 255)),  # Green
    ('accXZ', 'Acc XZ (m/s²)', 'X', 'Z', (0, 0, 255, 255)),  # Blue
    ('magXY', 'Mag XY (µT)', 'X', 'Y', (0, 255, 255, 255)),  # Cyan
    ('magYZ', 'Mag YZ (µT)', 'Y', 'Z', (255, 0, 255, 255)),  # Magenta
    ('magXZ', 'Mag XZ (µT)', 'X', 'Z', (255, 255, 0, 255)),  # Yellow
    ('gyroX', 'Gyro X (°/s)', 'Time (s)', 'X', None),
    ('gyroY', 'Gyro Y (°/s)', 'Time (s)', 'Y', None),
    ('gyroZ', 'Gyro Z (°/s)', 'Time (s)', 'Z', None)
]
for idx, (key, title, xLabel, yLabel, color) in enumerate(plotTitles):
    plot = pg.PlotWidget(title=title)
    plot.setMinimumSize(200, 200)
    if 'acc' in key or 'mag' in key:
        plot.setAspectLocked(True)
        scatterItems[key] = pg.ScatterPlotItem(size=5, brush=pg.mkBrush(*color), pen=None)
        plot.addItem(scatterItems[key])
        if 'acc' in key:
            plot.setRange(xRange=(-10, 10), yRange=(-10, 10))  # ±10 m/s²
        if 'mag' in key:
            plot.setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2))  # ±1.2 µT
        plot.showGrid(x=True, y=True)
        plot.setLabel('bottom', xLabel)
        plot.setLabel('left', yLabel)
    elif 'gyro' in key:
        plot.showGrid(x=True, y=True)
        plot.setLabel('bottom', xLabel)
        plot.setLabel('left', yLabel)
        plot.setRange(yRange=(-5, 5))  # ±0.1 °/s for calibrated gyro
        curveItems[key] = plot.plot(pen=pg.mkPen('b', width=2))
    plotGrid.addWidget(plot, idx // 3, idx % 3)
    plots[key] = plot
mainLayout.addLayout(plotGrid)
buttonLayout = QtWidgets.QHBoxLayout()
calibrateGyroButton = QtWidgets.QPushButton("Calibrate Gyro")
calibrateGyroButton.setMinimumSize(200, 50)
calibrateGyroButton.setStyleSheet("background-color: #00FF00; color: black; font-size: 16px;")
calibrateAccButton = QtWidgets.QPushButton("Calibrate Accelerometer")
calibrateAccButton.setMinimumSize(200, 50)
calibrateAccButton.setStyleSheet("background-color: #FFFF00; color: black; font-size: 16px;")
calibrateMagButton = QtWidgets.QPushButton("Calibrate Magnetometer")
calibrateMagButton.setMinimumSize(200, 50)
calibrateMagButton.setStyleSheet("background-color: #FFFF00; color: black; font-size: 16px;")
saveCalButton = QtWidgets.QPushButton("Save Calibration")
saveCalButton.setMinimumSize(200, 50)
saveCalButton.setStyleSheet("background-color: #0000FF; color: white; font-size: 16px;")
generateCodeButton = QtWidgets.QPushButton("Generate Code")
generateCodeButton.setMinimumSize(200, 50)
generateCodeButton.setStyleSheet("background-color: #0000FF; color: white; font-size: 16px;")
stopButton = QtWidgets.QPushButton("Stop")
stopButton.setMinimumSize(200, 50)
stopButton.setStyleSheet("background-color: #FF0000; color: white; font-size: 16px;")
buttonLayout.addWidget(calibrateGyroButton)
buttonLayout.addWidget(calibrateAccButton)
buttonLayout.addWidget(calibrateMagButton)
buttonLayout.addWidget(saveCalButton)
buttonLayout.addWidget(generateCodeButton)
buttonLayout.addWidget(stopButton)
mainLayout.addLayout(buttonLayout)# --- 3D Magnetometer and Attitude Window ---
mag3DWindow = QtWidgets.QWidget()
mag3DWindow.setWindowTitle("3D Magnetic Field Vector and Attitude (µT, °)")
mag3DWindow.setMinimumSize(800, 800)
mag3DLayout = QtWidgets.QGridLayout()
mag3DWindow.setLayout(mag3DLayout)
mag3DView = GLViewWidget()
mag3DView.setCameraPosition(distance=3, elevation=30, azimuth=45)
mag3DLayout.addWidget(mag3DView, 0, 0)
sphereMesh = gl.MeshData.sphere(rows=20, cols=20, radius=1.0)
sphere = gl.GLMeshItem(meshdata=sphereMesh, smooth=True, color=(0.3, 0.3, 0.3, 0.2), shader='shaded', drawFaces=True)
mag3DView.addItem(sphere)
circleMesh = gl.MeshData.cylinder(rows=1, cols=50, radius=[1.0, 1.0], length=0.001)
xyCircle = gl.GLMeshItem(meshdata=circleMesh, smooth=True, color=(1, 0.5, 0, 0.4), shader='shaded', glOptions='additive')
xyCircle.rotate(90, 1, 0, 0)
xyCircle.setDepthValue(-5)
mag3DView.addItem(xyCircle)
for axis, color in zip([(1.2, 0, 0), (0, 1.2, 0), (0, 0, 1.2)], [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]):
    line = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], axis]), color=color, width=2, antialias=True)
    mag3DView.addItem(line)
vectorArrow = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], [0, 0, 0]]), color=(1, 0, 0, 1), width=3, antialias=True)
vectorArrow.setDepthValue(10)
mag3DView.addItem(vectorArrow)
xyProjection = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], [0, 0, 0]]), color=(0, 1, 1, 1), width=2, antialias=True)
xyProjection.setDepthValue(10)
mag3DView.addItem(xyProjection)
verticalComponent = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], [0, 0, 0]]), color=(1, 0, 1, 1), width=2, antialias=True)
verticalComponent.setDepthValue(10)
mag3DView.addItem(verticalComponent)
vectorTrail = gl.GLLinePlotItem(pos=np.array([[0, 0, 0]]), color=(1, 1, 0, 0.6), width=1, antialias=True)
vectorTrail.setDepthValue(10)
mag3DView.addItem(vectorTrail)
attitudePlots = {}
attitudePlotTitles = [
    ('rollPlot', 'Roll (°)', '', ''),
    ('pitchPlot', 'Pitch (°)', '', ''),
    ('yawPlot', 'Yaw (°)', '', '')
]
for idx, (key, title, xLabel, yLabel) in enumerate(attitudePlotTitles):
    plot = pg.PlotWidget(title=title)
    plot.setMinimumSize(200, 200)
    plot.setRange(xRange=(-1.5, 1.5), yRange=(-1.5, 1.5))
    plot.setAspectLocked(True)
    plot.hideAxis('bottom')
    plot.hideAxis('left')
    readout = pg.TextItem("0.0°", anchor=(0.5, 0.5), color=(0, 255, 0))
    readout.setFont(QtGui.QFont("Courier New", 14, QtGui.QFont.Bold))
    if key in ['rollPlot', 'pitchPlot']:
        readout.setPos(0, -1.4)
    else:
        readout.setPos(0, -1.5)
    readout.setZValue(15)
    plot.addItem(readout)
    attitudeItems[f'{key}_readout'] = readout
    if key == 'rollPlot':
        skyX = np.array([-1.5, 1.5, 1.5, -1.5])
        skyY = np.array([0, 0, 1.5, 1.5])
        groundX = np.array([-1.5, 1.5, 1.5, -1.5])
        groundY = np.array([-1.5, -1.5, 0, 0])
        sky = pg.PlotDataItem(x=skyX, y=skyY, pen=None, fillLevel=1.5, brush=pg.mkBrush(0, 191, 255, 255))
        ground = pg.PlotDataItem(x=groundX, y=groundY, pen=None, fillLevel=-1.5, brush=pg.mkBrush(139, 69, 19, 255))
        plot.addItem(sky)
        plot.addItem(ground)
        horizon = pg.PlotDataItem(x=[-1, 1], y=[0, 0], pen=pg.mkPen(color=(0, 100, 0), width=3))
        plot.addItem(horizon)
        attitudeItems['roll_horizon'] = horizon
        for xPos, anchor in [(-1.2, (1, 0.5)), (1.2, (0, 0.5))]:
            text = pg.TextItem("0°", anchor=anchor, color=(255, 255, 0))
            text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))
            text.setPos(xPos, 0)
            text.setZValue(10)
            plot.addItem(text)
        for angle in [30, 60]:
            rad = math.radians(angle)
            sinRad = math.sin(rad)
            cosRad = math.cos(rad)
            text = pg.TextItem(f"{angle}°", anchor=(0, 0.5), color=(255, 255, 0))
            text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))
            text.setPos(1.2 * cosRad, 1.2 * sinRad)
            text.setZValue(10)
            plot.addItem(text)
            text = pg.TextItem(f"{angle}°", anchor=(1, 0.5), color=(255, 255, 0))
            text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))
            text.setPos(-1.2 * cosRad, 1.2 * sinRad)
            text.setZValue(10)
            plot.addItem(text)
            text = pg.TextItem(f"-{angle}°", anchor=(1, 0.5), color=(255, 255, 0))
            text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))
            text.setPos(-1.2 * cosRad, -1.2 * sinRad)
            text.setZValue(10)
            plot.addItem(text)
            text = pg.TextItem(f"-{angle}°", anchor=(0, 0.5), color=(255, 255, 0))
            text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))
            text.setPos(1.2 * cosRad, -1.2 * sinRad)
            text.setZValue(10)
            plot.addItem(text)
    elif key == 'pitchPlot':
        skyX = np.array([-1.5, 1.5, 1.5, -1.5])
        skyY = np.array([0, 0, 1.5, 1.5])
        groundX = np.array([-1.5, 1.5, 1.5, -1.5])
        groundY = np.array([-1.5, -1.5, 0, 0])
        sky = pg.PlotDataItem(x=skyX, y=skyY, pen=None, fillLevel=1.5, brush=pg.mkBrush(0, 191, 255, 255))
        ground = pg.PlotDataItem(x=groundX, y=groundY, pen=None, fillLevel=-1.5, brush=pg.mkBrush(139, 69, 19, 255))
        plot.addItem(sky)
        plot.addItem(ground)
        ladder = []
        for angle in [-90, -60, -30, -10, 0, 10, 30, 60, 90]:
            y = angle / 90 * 1.2
            if angle == 0:
                line = pg.PlotDataItem(x=[-1, 1], y=[y, y], pen=pg.mkPen(color=(0, 100, 0), width=3))
            else:
                line = pg.PlotDataItem(x=[-0.5, -0.2, -0.2, 0.2, 0.2, 0.5], y=[y, y, y+0.05, y+0.05, y, y], pen=pg.mkPen(color=(0, 100, 0), width=2))
                textLeft = pg.TextItem(f"{angle}°", anchor=(1, 0.5), color=(255, 255, 0))
                textLeft.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))
                textLeft.setPos(-0.6, y)
                textRight = pg.TextItem(f"{angle}°", anchor=(0, 0.5), color=(255, 255, 0))
                textRight.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))
                textRight.setPos(0.6, y)
                plot.addItem(textLeft)
                plot.addItem(textRight)
            plot.addItem(line)
            ladder.append(line)
        attitudeItems['pitch_ladder'] = ladder
    elif key == 'yawPlot':
        theta = np.linspace(0, 2 * np.pi, 100)
        xCircle = 1.2 * np.sin(theta)
        yCircle = 1.2 * np.cos(theta)
        circle = pg.PlotDataItem(x=xCircle, y=yCircle, pen=pg.mkPen('w', width=2), fillLevel=0, brush=pg.mkBrush(0, 0, 0, 255))
        plot.addItem(circle)
        for angle, label in [(0, 'N'), (90, 'E'), (180, 'S'), (270, 'W')]:
            rad = math.radians(angle)
            x = 1.3 * math.sin(rad)
            y = 1.3 * math.cos(rad)
            text = pg.TextItem(label, anchor=(0.5, 0.5), color=(255, 255, 0))
            text.setFont(QtGui.QFont("Arial", 12))
            text.setPos(x, y)
            plot.addItem(text)
        needle = pg.PlotDataItem(x=[0, 0], y=[0, 1], pen=pg.mkPen('r', width=3))
        plot.addItem(needle)
        attitudeItems['yaw_needle'] = needle
    if key == 'rollPlot':
        mag3DLayout.addWidget(plot, 0, 1)
    elif key == 'pitchPlot':
        mag3DLayout.addWidget(plot, 1, 0)
    elif key == 'yawPlot':
        mag3DLayout.addWidget(plot, 1, 1)
    attitudePlots[key] = plot# --- Calibrate Gyroscope ---
def calibrateGyro():
    global gxOffset, gyOffset, gzOffset
    msg = QtWidgets.QMessageBox()
    msg.setWindowTitle("Calibrate Gyro")
    msg.setText("Keep the device still for calibration. Press OK to start.")
    msg.exec_()
    gxSamples = []
    gySamples = []
    gzSamples = []
    sampleCount = 0
    ser.reset_input_buffer()
    while sampleCount < 200:
        if ser.in_waiting <= 0: continue
        try:
            line = ser.readline().decode('utf-8').strip()
            values = line.split(',')
            if len(values) != 9: continue
            ax, ay, az, gx, gy, gz, mx, my, mz = map(float, values)
            # Convert gyro data from rad/s to °/s
            gx = gx * 180.0 / math.pi
            gy = gy * 180.0 / math.pi
            gz = gz * 180.0 / math.pi
            gxSamples.append(gx)
            gySamples.append(gy)
            gzSamples.append(gz)
            sampleCount += 1
        except Exception:
            pass
        time.sleep(0.01)
    if not gxSamples: return
    gxOffset = np.mean(gxSamples)  # Offset in °/s
    gyOffset = np.mean(gySamples)  # Offset in °/s
    gzOffset = np.mean(gzSamples)  # Offset in °/s
    try:
        with open('gCal.txt', 'w') as f:
            f.write(f"{gxOffset},{gyOffset},{gzOffset}\n")
        msg = QtWidgets.QMessageBox()
        msg.setWindowTitle("Gyro Calibration Complete")
        msg.setText(f"Offsets (°/s): Gx={gxOffset:.6f}, Gy={gyOffset:.6f}, Gz={gzOffset:.6f}\nSaved to gCal.txt")
        msg.exec_()
        generateArduinoCode()
    except Exception as e:
        msg = QtWidgets.QMessageBox()
        msg.setWindowTitle("Gyro Calibration Error")
        msg.setText(f"Error saving gyro offsets: {str(e)}")
        msg.exec_()# --- Calibrate Accelerometer ---
def calibrateAcc():
    global accCalibrating, accMin, accMax, accOffset, accScale, accRaw, axSamples, aySamples, azSamples, plots
    if accCalibrating:
        calibrateAccButton.setText("Calibrate Accelerometer")
        accCalibrating = False
        if axSamples:
            accMin['x'] = min(axSamples)
            accMin['y'] = min(aySamples)
            accMin['z'] = min(azSamples)
            accMax['x'] = max(axSamples)
            accMax['y'] = max(aySamples)
            accMax['z'] = max(azSamples)
            accOffset['x'] = (accMin['x'] + accMax['x']) / 2.0
            accOffset['y'] = (accMin['y'] + accMax['y']) / 2.0
            accOffset['z'] = (accMin['z'] + accMax['z']) / 2.0
            rangeX = accMax['x'] - accMin['x']
            rangeY = accMax['y'] - accMin['y']
            rangeZ = accMax['z'] - accMin['z']
            avgRange = (rangeX + rangeY + rangeZ) / 3.0
            accScale['x'] = 2.0 / avgRange if avgRange != 0 else 1.0
            accScale['y'] = 2.0 / avgRange if avgRange != 0 else 1.0
            accScale['z'] = 2.0 / avgRange if avgRange != 0 else 1.0
            if avgRange < 0.5 or avgRange > 20.0:
                msg = QtWidgets.QMessageBox()
                msg.setWindowTitle("Calibration Warning")
                msg.setText(f"Average range {avgRange:.6f} is unusual.\nEnsure full 3D rotation during calibration.")
                msg.exec_()
            accRaw['x'] = np.empty(0)
            accRaw['y'] = np.empty(0)
            accRaw['z'] = np.empty(0)
            try:
                with open('accCal.txt', 'w') as f:
                    f.write(f"{accOffset['x']},{accScale['x']},{accOffset['y']},{accScale['y']},{accOffset['z']},{accScale['z']}\n")
                msg = QtWidgets.QMessageBox()
                msg.setWindowTitle("Acc Calibration Complete")
                msg.setText(f"Offsets (m/s²): Ax={accOffset['x']:.6f}, Ay={accOffset['y']:.6f}, Az={accOffset['z']:.6f}\n" +
                            f"Scales: Ax={accScale['x']:.6f}, Ay={accScale['y']:.6f}, Az={accScale['z']:.6f}\nSaved to accCal.txt")
                msg.exec_()
                generateArduinoCode()
            except Exception as e:
                msg = QtWidgets.QMessageBox()
                msg.setWindowTitle("Acc Calibration Error")
                msg.setText(f"Error saving acc offsets: {str(e)}")
                msg.exec_()
            for key in ['accXY', 'accYZ', 'accXZ']:
                plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2))
        return
    accCalibrating = True
    accMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}
    accMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}
    axSamples = []
    aySamples = []
    azSamples = []
    accRaw['x'] = np.empty(0)
    accRaw['y'] = np.empty(0)
    accRaw['z'] = np.empty(0)
    calibrateAccButton.setText("Stop Acc Calibration")
    msg = QtWidgets.QMessageBox()
    msg.setWindowTitle("Accelerometer Calibration")
    msg.setText("Rotate device in all directions (full 3D rotation) to capture min/max values for all axes.")
    msg.exec_()# --- Calibrate Magnetometer ---
def calibrateMag():
    global magCalibrating, magMin, magMax, magOffset, magScale, magRaw, mxSamples, mySamples, mzSamples, magRawMin, magRawMax, plots, mag3DView, vectorArrow, vectorTrail, xyProjection, verticalComponent, trailBuffer
    if magCalibrating:
        calibrateMagButton.setText("Calibrate Magnetometer")
        magCalibrating = False
        if mxSamples:
            magMin['x'] = min(mxSamples)
            magMin['y'] = min(mySamples)
            magMin['z'] = min(mzSamples)
            magMax['x'] = max(mxSamples)
            magMax['y'] = max(mySamples)
            magMax['z'] = max(mzSamples)
            
            # --- CENTER OFFSET (unchanged) ---
            magOffset['x'] = (magMin['x'] + magMax['x']) / 2.0
            magOffset['y'] = (magMin['y'] + magMax['y']) / 2.0
            magOffset['z'] = (magMin['z'] + magMax['z']) / 2.0

            # --- PER-AXIS SCALING TO ±1.0 (NEW) ---
            rangeX = magMax['x'] - magMin['x']
            rangeY = magMax['y'] - magMin['y']
            rangeZ = magMax['z'] - magMin['z']

            magScale['x'] = 2.0 / rangeX if rangeX > 1e-6 else 1.0
            magScale['y'] = 2.0 / rangeY if rangeY > 1e-6 else 1.0
            magScale['z'] = 2.0 / rangeZ if rangeZ > 1e-6 else 1.0

            # Clear buffers
            magRaw['x'] = np.empty(0)
            magRaw['y'] = np.empty(0)
            magRaw['z'] = np.empty(0)
            magRawMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}
            magRawMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}
            trailBuffer = []
            vectorArrow.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))
            vectorTrail.setData(pos=np.array([[0, 0, 0]]))
            xyProjection.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))
            verticalComponent.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))

            try:
                with open('magCal.txt', 'w') as f:
                    f.write(f"{magOffset['x']},{magScale['x']},{magOffset['y']},{magScale['y']},{magOffset['z']},{magScale['z']}\n")
                msg = QtWidgets.QMessageBox()
                msg.setWindowTitle("Mag Calibration Complete")
                msg.setText(
                    f"Offsets (arb. units): Mx={magOffset['x']:.6f}, My={magOffset['y']:.6f}, Mz={magOffset['z']:.6f}\n"
                    f"Scales (per-axis to ±1): X={magScale['x']:.6f}, Y={magScale['y']:.6f}, Z={magScale['z']:.6f}\n"
                    f"Saved to magCal.txt\n"
                    f"Your XZ circle will now be perfectly round from -1 to +1"
                )
                msg.exec_()
                generateArduinoCode()
            except Exception as e:
                msg = QtWidgets.QMessageBox()
                msg.setWindowTitle("Mag Calibration Error")
                msg.setText(f"Error saving mag offsets: {str(e)}")
                msg.exec_()

            for key in ['magXY', 'magYZ', 'magXZ']:
                plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2))
            mag3DView.opts['distance'] = 3
            mag3DView.setCameraPosition(distance=3, elevation=30, azimuth=45)
        return

    # --- Start Calibration ---
    magCalibrating = True
    magMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}
    magMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}
    mxSamples = []
    mySamples = []
    mzSamples = []
    magRaw['x'] = np.empty(0)
    magRaw['y'] = np.empty(0)
    magRaw['z'] = np.empty(0)
    magRawMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}
    magRawMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}
    trailBuffer = []
    vectorArrow.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))
    vectorTrail.setData(pos=np.array([[0, 0, 0]]))
    xyProjection.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))
    verticalComponent.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))
    calibrateMagButton.setText("Stop Mag Calibration")
    msg = QtWidgets.QMessageBox()
    msg.setWindowTitle("Magnetometer Calibration")
    msg.setText("Rotate device in figure-8 pattern to capture min/max values for all axes.\n"
                "After calibration, XZ circle will be forced to ±1.0")
    msg.exec_()
def generateCode():
    success = generateArduinoCode(to_file=True)
    msg = QtWidgets.QMessageBox()
    msg.setWindowTitle("Generate Code")
    if success:
        msg.setText("Arduino calibration function saved to calData.txt")
    else:
        msg.setText("Error saving calData.txt.")
    msg.exec_()# --- Stop Program ---
def stopProgram():
    try:
        plotTimer.stop()
        if ser.is_open:
            ser.close()
        mainWindow.close()
        mag3DWindow.close()
        app.exit(0)
    except Exception as e:
        msg = QtWidgets.QMessageBox()
        msg.setWindowTitle("Stop Error")
        msg.setText(f"Error stopping program: {str(e)}")
        msg.exec_()# --- Update Plots ---
def updatePlots():
    global accRaw, gyroRaw, magRaw, timePoints, gxOffset, gyOffset, gzOffset
    global accCalibrating, magCalibrating, accMin, accMax, magMin, magMax
    global accOffset, accScale, magOffset, magScale, accFiltered, magFiltered
    global axSamples, aySamples, azSamples, mxSamples, mySamples, mzSamples
    global magRawMin, magRawMax, vectorArrow, vectorTrail, xyProjection, verticalComponent, trailBuffer
    global attitudeItems, attitudePlots
    if ser.in_waiting <= 0: return
    try:
        line = ser.readline().decode('utf-8').strip()
        values = line.split(',')
        if len(values) != 9: return
        ax, ay, az, gx, gy, gz, mx, my, mz = map(float, values)
        # Convert gyro data from rad/s to °/s
        gx = gx * 180.0 / math.pi
        gy = gy * 180.0 / math.pi
        gz = gz * 180.0 / math.pi
        # Apply low-pass filter to accelerometer data
        accFiltered['x'] = alpha * ax + (1 - alpha) * accFiltered['x'] if accFiltered['x'] is not None else ax
        accFiltered['y'] = alpha * ay + (1 - alpha) * accFiltered['y'] if accFiltered['y'] is not None else ay
        accFiltered['z'] = alpha * az + (1 - alpha) * accFiltered['z'] if accFiltered['z'] is not None else az
        ax = accFiltered['x']  # m/s²
        ay = accFiltered['y']
        az = accFiltered['z']
        # Apply low-pass filter to magnetometer data
        magFiltered['x'] = alpha * mx + (1 - alpha) * magFiltered['x'] if magFiltered['x'] is not None else mx
        magFiltered['y'] = alpha * my + (1 - alpha) * magFiltered['y'] if magFiltered['y'] is not None else my
        magFiltered['z'] = alpha * mz + (1 - alpha) * magFiltered['z'] if magFiltered['z'] is not None else mz
        mx = magFiltered['x']  # QMC5883L arbitrary units
        my = magFiltered['y']
        mz = magFiltered['z']
        # Collect samples during calibration
        if accCalibrating:
            axSamples.append(ax)
            aySamples.append(ay)
            azSamples.append(az)
            accMin['x'] = min(accMin['x'], ax)
            accMin['y'] = min(accMin['y'], ay)
            accMin['z'] = min(accMin['z'], az)
            accMax['x'] = max(accMax['x'], ax)
            accMax['y'] = max(accMax['y'], ay)
            accMax['z'] = max(accMax['z'], az)
        if magCalibrating:
            mxSamples.append(mx)
            mySamples.append(my)
            mzSamples.append(mz)
            magMin['x'] = min(magMin['x'], mx)
            magMin['y'] = min(magMin['y'], my)
            magMin['z'] = min(magMin['z'], mz)
            magMax['x'] = max(magMax['x'], mx)
            magMax['y'] = max(magMax['y'], my)
            magMax['z'] = max(magMax['z'], mz)
        # Apply calibration
        axCal = (ax - accOffset['x']) * accScale['x']  # m/s²
        ayCal = (ay - accOffset['y']) * accScale['y']
        azCal = (az - accOffset['z']) * accScale['z']
        gxCal = gx - gxOffset  # °/s
        gyCal = gy - gyOffset
        gzCal = gz - gzOffset
        mxCal = (mx - magOffset['x']) * magScale['x']  # µT
        myCal = (my - magOffset['y']) * magScale['y']
        mzCal = (mz - magOffset['z']) * magScale['z']
        # Determine accelerometer data to plot
        if accCalibrating:
            ax_plot = ax
            ay_plot = ay
            az_plot = az
        else:
            ax_plot = axCal
            ay_plot = ayCal
            az_plot = azCal
        # Determine magnetometer data to plot
        if magCalibrating:
            mx_plot = mx
            my_plot = my
            mz_plot = mz
        else:
            mx_plot = mxCal
            my_plot = myCal
            mz_plot = mzCal
        # Calculate roll and pitch from accelerometer (NED: x north, y east, z down)
        roll = math.atan2(ayCal, azCal + epsilon) * 180 / math.pi  # °
        pitch = math.atan2(-axCal, math.sqrt(ayCal**2 + azCal**2 + epsilon)) * 180 / math.pi  # °
        phi = math.radians(roll)
        theta = math.radians(pitch)
        # Compute tilt-compensated yaw
        mxH = mxCal * math.cos(theta) + myCal * math.sin(phi) * math.sin(theta) + mzCal * math.cos(phi) * math.sin(theta)
        myH = myCal * math.cos(phi) - mzCal * math.sin(phi)
        yaw = math.atan2(myH, mxH + epsilon) * 180 / math.pi
        if yaw < 0:
            yaw += 360  # Normalize to 0-360°
        # Update attitude readouts
        attitudeItems['rollPlot_readout'].setText(f"Roll: {-roll:+.1f}°")
        attitudeItems['pitchPlot_readout'].setText(f"Pitch: {-pitch:+.1f}°")
        attitudeItems['yawPlot_readout'].setText(f"Yaw: {yaw:.1f}°")        # Update 3D magnetic vector visualization
        vec = np.array([mx_plot, my_plot, mz_plot])
        norm = np.linalg.norm(vec)
        if norm > epsilon:
            unitVec = vec / norm
            vectorArrow.setData(pos=np.array([[0, 0, 0], unitVec]), color=(1, 0, 0, 1), width=3)
            xyProj = np.array([unitVec[0], unitVec[1], 0])
            xyProjection.setData(pos=np.array([[0, 0, 0], xyProj]), color=(0, 1, 1, 1), width=2)
            verticalComponent.setData(pos=np.array([xyProj, unitVec]), color=(1, 0, 1, 1), width=2)
            trailBuffer.append(unitVec)
            trailBuffer = trailBuffer[-trailMaxPoints:]
            vectorTrail.setData(pos=np.array(trailBuffer), color=(1, 1, 0, 0.6), width=1)
        # Update plot ranges for accelerometer if in raw mode
        if accCalibrating or (accScale['x'] == 1.0 and accOffset['x'] == 0.0):
            accRawMin = {'x': min(accRaw['x'], default=ax_plot) if len(accRaw['x']) > 0 else ax_plot,
                         'y': min(accRaw['y'], default=ay_plot) if len(accRaw['y']) > 0 else ay_plot,
                         'z': min(accRaw['z'], default=az_plot) if len(accRaw['z']) > 0 else az_plot}
            accRawMax = {'x': max(accRaw['x'], default=ax_plot) if len(accRaw['x']) > 0 else ax_plot,
                         'y': max(accRaw['y'], default=ay_plot) if len(accRaw['y']) > 0 else ay_plot,
                         'z': max(accRaw['z'], default=az_plot) if len(accRaw['z']) > 0 else az_plot}
            xMin = accRawMin['x'] * 1.1
            xMax = accRawMax['x'] * 1.1
            yMin = accRawMin['y'] * 1.1
            yMax = accRawMax['y'] * 1.1
            zMin = accRawMin['z'] * 1.1
            zMax = accRawMax['z'] * 1.1
            plots['accXY'].setRange(xRange=(xMin, xMax), yRange=(yMin, yMax))
            plots['accYZ'].setRange(xRange=(yMin, yMax), yRange=(zMin, zMax))
            plots['accXZ'].setRange(xRange=(xMin, xMax), yRange=(zMin, zMax))
        # Update plot ranges for magnetometer if in raw mode
        use_raw_mag_range = magCalibrating or (magScale['x'] == 1.0 and magOffset['x'] == 0.0)
        if use_raw_mag_range:
            magRawMin['x'] = min(magRawMin['x'], mx)
            magRawMax['x'] = max(magRawMax['x'], mx)
            magRawMin['y'] = min(magRawMin['y'], my)
            magRawMax['y'] = max(magRawMax['y'], my)
            magRawMin['z'] = min(magRawMin['z'], mz)
            magRawMax['z'] = max(magRawMax['z'], mz)
            xMin = magRawMin['x'] * 1.1
            xMax = magRawMax['x'] * 1.1
            yMin = magRawMin['y'] * 1.1
            yMax = magRawMax['y'] * 1.1
            zMin = magRawMin['z'] * 1.1
            zMax = magRawMax['z'] * 1.1
            mag3DView.opts['distance'] = max(xMax - xMin, yMax - yMin, zMax - zMin) * 1.5
            mag3DView.setCameraPosition(distance=mag3DView.opts['distance'])
            plots['magXY'].setRange(xRange=(xMin, xMax), yRange=(yMin, yMax))
            plots['magYZ'].setRange(xRange=(yMin, yMax), yRange=(zMin, zMax))
            plots['magXZ'].setRange(xRange=(xMin, xMax), yRange=(zMin, zMax))
        # Store data
        accRaw['x'] = np.append(accRaw['x'], ax_plot)[-accMagMaxPoints:]
        accRaw['y'] = np.append(accRaw['y'], ay_plot)[-accMagMaxPoints:]
        accRaw['z'] = np.append(accRaw['z'], az_plot)[-accMagMaxPoints:]
        gyroRaw['x'] = np.append(gyroRaw['x'], gxCal)[-gyroMaxPoints:]
        gyroRaw['y'] = np.append(gyroRaw['y'], gyCal)[-gyroMaxPoints:]
        gyroRaw['z'] = np.append(gyroRaw['z'], gzCal)[-gyroMaxPoints:]
        magRaw['x'] = np.append(magRaw['x'], mx_plot)[-accMagMaxPoints:]
        magRaw['y'] = np.append(magRaw['y'], my_plot)[-accMagMaxPoints:]
        magRaw['z'] = np.append(magRaw['z'], mz_plot)[-accMagMaxPoints:]
        elapsed = startTime.msecsTo(QtCore.QTime.currentTime()) / 1000.0
        timePoints = np.append(timePoints, elapsed)[-gyroMaxPoints:]
        # Update 2D scatter plots
        scatterItems['accXY'].setData(x=accRaw['x'], y=accRaw['y'])
        scatterItems['accYZ'].setData(x=accRaw['y'], y=accRaw['z'])
        scatterItems['accXZ'].setData(x=accRaw['x'], y=accRaw['z'])
        scatterItems['magXY'].setData(x=magRaw['x'], y=magRaw['y'])
        scatterItems['magYZ'].setData(x=magRaw['y'], y=magRaw['z'])
        scatterItems['magXZ'].setData(x=magRaw['x'], y=magRaw['z'])
        if len(timePoints) <= 0: return
        xMin = max(0, timePoints[-1] - gyroTimeWindow)
        xMax = timePoints[-1]
        for key in ['gyroX', 'gyroY', 'gyroZ']:
            plots[key].setRange(xRange=(xMin, xMax))
            curveItems[key].setData(x=timePoints, y=gyroRaw[key[-1].lower()])
        # Update roll horizon
        rollRad = math.radians(roll)
        x1 = -1 * math.cos(rollRad)
        y1 = 1 * math.sin(rollRad)
        x2 = 1 * math.cos(rollRad)
        y2 = -1 * math.sin(rollRad)
        attitudeItems['roll_horizon'].setData(x=[x1, x2], y=[y1, y2])
        # Update pitch ladder
        pitchOffset = -pitch / 90 * 1.2
        for idx, angle in enumerate([-90, -60, -30, -10, 0, 10, 30, 60, 90]):
            yBase = angle / 90 * 1.2
            yShifted = yBase + pitchOffset
            if angle == 0:
                attitudeItems['pitch_ladder'][idx].setData(x=[-1, 1], y=[yShifted, yShifted])
            else:
                attitudeItems['pitch_ladder'][idx].setData(x=[-0.5, -0.2, -0.2, 0.2, 0.2, 0.5], y=[yShifted, yShifted, yShifted+0.05, yShifted+0.05, yShifted, yShifted])
        # Update yaw needle
        yawRad = math.radians(yaw)
        needleX = 1 * math.sin(yawRad)
        needleY = 1 * math.cos(yawRad)
        attitudeItems['yaw_needle'].setData(x=[0, needleX], y=[0, needleY])
    except Exception:
        pass# --- Connect Buttons and Start Application ---
calibrateGyroButton.clicked.connect(calibrateGyro)
calibrateAccButton.clicked.connect(calibrateAcc)
calibrateMagButton.clicked.connect(calibrateMag)
saveCalButton.clicked.connect(saveCalibration)
generateCodeButton.clicked.connect(generateCode)
stopButton.clicked.connect(stopProgram)
ser.reset_input_buffer()
loadCalibration()
plotTimer = QtCore.QTimer()
plotTimer.timeout.connect(updatePlots)
plotTimer.start(50)
mainWindow.show()
mag3DWindow.show()
try:
    app.exec_()
except Exception as e:
    msg = QtWidgets.QMessageBox()
    msg.setWindowTitle("Application Error")
    msg.setText(f"Application error: {str(e)}")
    msg.exec_()
finally:
    if ser.is_open:
        ser.close()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

751

752

753

754

755

756

757

758

759

760

761

762

763

764

765

766

767

768

769

770

771

772

773

774

775

776

777

778

779

780

781

782

783

784

785

786

787

788

789

790

791

792

793

794

795

796

797

798

799

800

801

802

803

804

805

806

807

808

809

810

811

812

813

814

815

816

817

818

819

820

821

822

823

824

825

826

827

828

829

830

831

832

833

import serial

import numpy as np

from PyQt5 import QtWidgets, QtCore, QtGui

import pyqtgraph as pg

import pyqtgraph.opengl as gl

from pyqtgraph.opengl import GLViewWidget, GLLinePlotItem, GLMeshItem

import time

import os

import math# --- Serial Port Configuration ---

# Strategy: Set up serial communication with the IMU to receive raw sensor data:

# ax, ay, az (m/s²), gx, gy, gz (rad/s), mx, my, mz (arbitrary units from QMC5883L).

# Convert gyro data to °/s immediately after reading for consistency with plots and calibration.

serialPort = 'COM8' # Serial port for IMU communication (adjust as needed)

baudRate = 115200 # Baud rate matching IMU configuration

timeout = 0.1 # Serial read timeout in seconds

accMagMaxPoints = 1000 # Max points for accelerometer/magnetometer plots

gyroMaxPoints = 100 # Max points for gyroscope plots

trailMaxPoints = 100 # Max points for 3D magnetic trail

gyroTimeWindow = 5.0 # Time window for gyro plots in seconds

alpha = 0.3 # Low-pass filter constant for smoothing accelerometer/magnetometer data

epsilon = 1e-6 # Small constant to prevent division by zero in calculations# --- Calibration Variables ---

# Strategy: Store calibration parameters to correct sensor biases and scales.

# Gyro offsets are in °/s (converted from rad/s on input).

# Accelerometer in m/s², magnetometer in µT after calibration.

gxOffset = 0.0 # Gyroscope x-axis offset (°/s)

gyOffset = 0.0 # Gyroscope y-axis offset (°/s)

gzOffset = 0.0 # Gyroscope z-axis offset (°/s)

accCalibrating = False # Flag for accelerometer calibration mode

magCalibrating = False # Flag for magnetometer calibration mode

accMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')} # Min accelerometer readings (m/s²)

accMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')} # Max accelerometer readings (m/s²)

magMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')} # Min magnetometer readings (arb. units)

magMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')} # Max magnetometer readings (arb. units)

magRawMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')} # Min raw magnetometer readings

magRawMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')} # Max raw magnetometer readings

accOffset = {'x': 0.0, 'y': 0.0, 'z': 0.0} # Accelerometer offsets (m/s²)

accScale = {'x': 1.0, 'y': 1.0, 'z': 1.0} # Accelerometer scales

magOffset = {'x': 0.0, 'y': 0.0, 'z': 0.0} # Magnetometer offsets (arb. units)

magScale = {'x': 1.0, 'y': 1.0, 'z': 1.0} # Magnetometer scales (to µT)

accFiltered = {'x': None, 'y': None, 'z': None} # Filtered accelerometer data (m/s²)

magFiltered = {'x': None, 'y': None, 'z': None} # Filtered magnetometer data (arb. units)

axSamples = [] # Accelerometer x-axis samples during calibration

aySamples = [] # Accelerometer y-axis samples during calibration

azSamples = [] # Accelerometer z-axis samples during calibration

mxSamples = [] # Magnetometer x-axis samples during calibration

mySamples = [] # Magnetometer y-axis samples during calibration

mzSamples = [] # Magnetometer z-axis samples during calibration

trailBuffer = [] # Buffer for 3D magnetic vector trail# --- Data Storage ---

# Strategy: Store raw and calibrated sensor data in numpy arrays for efficient plotting.

# Gyro data is stored in °/s after conversion from rad/s.

accRaw = {'x': np.empty(0), 'y': np.empty(0), 'z': np.empty(0)} # Raw accelerometer data (m/s²)

gyroRaw = {'x': np.empty(0), 'y': np.empty(0), 'z': np.empty(0)} # Calibrated gyroscope data (°/s)

magRaw = {'x': np.empty(0), 'y': np.empty(0), 'z': np.empty(0)} # Calibrated magnetometer data (µT)

timePoints = np.empty(0) # Timestamps for gyro plots

startTime = QtCore.QTime.currentTime() # Start time for elapsed time calculation# --- Serial Connection Initialization ---

try:

ser = serial.Serial(serialPort, baudRate, timeout=timeout) # Open serial port

ser.reset_input_buffer() # Clear input buffer to remove stale data

except serial.SerialException as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Serial Error")

msg.setText(f"Failed to open {serialPort}: {str(e)}\nCheck: 1) Port in Device Manager, 2) Close Arduino IDE Serial Monitor, 3) Run as admin")

msg.exec_()

exit(1)# --- Generate Arduino Code ---

# Strategy: Generate Arduino code with calibration parameters for onboard processing.

# Gyro offsets are in °/s to match Python processing.

def generateArduinoCode(to_file=False):

code = [

"void calibrateSensors() {",

" const float axOffset = " + str(accOffset['x']) + ";",

" const float ayOffset = " + str(accOffset['y']) + ";",

" const float azOffset = " + str(accOffset['z']) + ";",

" const float axScale = " + str(accScale['x']) + ";",

" const float ayScale = " + str(accScale['y']) + ";",

" const float azScale = " + str(accScale['z']) + ";",

" const float gxOffset = " + str(gxOffset) + ";",

" const float gyOffset = " + str(gyOffset) + ";",

" const float gzOffset = " + str(gzOffset) + ";",

" const float mxOffset = " + str(magOffset['x']) + ";",

" const float myOffset = " + str(magOffset['y']) + ";",

" const float mzOffset = " + str(magOffset['z']) + ";",

" const float mxScale = " + str(magScale['x']) + ";",

" const float myScale = " + str(magScale['y']) + ";",

" const float mzScale = " + str(magScale['z']) + ";",

"",

" 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;",

"}"

]

if to_file:

try:

file_path = os.path.join(os.getcwd(), 'calData.txt')

with open(file_path, 'w', encoding='utf-8') as f:

f.write("\n".join(code) + "\n")

if os.path.exists(file_path) and os.path.getsize(file_path) > 0:

with open(file_path, 'r', encoding='utf-8') as f:

if f.read().strip():

return True

raise Exception("File is empty")

raise Exception("File was not created or is inaccessible")

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("File Write Error")

msg.setText("Error writing calData.txt: " + str(e))

msg.exec_()

return False

return True# --- Load Calibration Files ---

def loadCalibration():

global gxOffset, gyOffset, gzOffset, accOffset, accScale, magOffset, magScale, plots, mag3DView

loaded = []

try:

if os.path.exists('gCal.txt'):

with open('gCal.txt', 'r') as f:

values = list(map(float, f.readline().strip().split(',')))

if len(values) == 3:

gxOffset, gyOffset, gzOffset = values # Load gyro offsets (°/s)

loaded.append(f"Gyro (°/s): Gx={gxOffset:.6f}, Gy={gyOffset:.6f}, Gz={gzOffset:.6f}")

if os.path.exists('accCal.txt'):

with open('accCal.txt', 'r') as f:

values = list(map(float, f.readline().strip().split(',')))

if len(values) == 6:

accOffset['x'], accScale['x'], accOffset['y'], accScale['y'], accOffset['z'], accScale['z'] = values

for key in ['accXY', 'accYZ', 'accXZ']:

plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2)) # ±1.2 m/s²

loaded.append(f"Acc (m/s²): Offsets x={accOffset['x']:.6f}, y={accOffset['y']:.6f}, z={accOffset['z']:.6f}, " +

f"Scales x={accScale['x']:.6f}, y={accScale['y']:.6f}, z={accScale['z']:.6f}")

if os.path.exists('magCal.txt'):

with open('magCal.txt', 'r') as f:

values = list(map(float, f.readline().strip().split(',')))

if len(values) == 6:

magOffset['x'], magScale['x'], magOffset['y'], magScale['y'], magOffset['z'], magScale['z'] = values

for key in ['magXY', 'magYZ', 'magXZ']:

plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2)) # ±1.2 µT

mag3DView.opts['distance'] = 3

mag3DView.setCameraPosition(distance=3, elevation=30, azimuth=45)

loaded.append(f"Mag (µT): Offsets x={magOffset['x']:.6f}, y={magOffset['y']:.6f}, z={magOffset['z']:.6f}, " +

f"Scales x={magScale['x']:.6f}, y={magScale['y']:.6f}, z={magScale['z']:.6f}")

if loaded:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Calibration Loaded")

msg.setText("Loaded calibration:\n" + "\n".join(loaded))

msg.exec_()

generateArduinoCode()

else:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("No Calibration Found")

msg.setText("No calibration files found. Please calibrate the sensors.")

msg.exec_()

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Calibration Load Error")

msg.setText(f"Error loading calibration files: {str(e)}")

msg.exec_()# --- Save Calibration ---

def saveCalibration():

global gxOffset, gyOffset, gzOffset, accOffset, accScale, magOffset, magScale

saved = []

try:

with open('gCal.txt', 'w') as f:

f.write(f"{gxOffset},{gyOffset},{gzOffset}\n")

saved.append(f"Gyro (°/s): Gx={gxOffset:.6f}, Gy={gyOffset:.6f}, Gz={gzOffset:.6f}")

with open('accCal.txt', 'w') as f:

f.write(f"{accOffset['x']},{accScale['x']},{accOffset['y']},{accScale['y']},{accOffset['z']},{accScale['z']}\n")

saved.append(f"Acc (m/s²): Offsets x={accOffset['x']:.6f}, y={accOffset['y']:.6f}, z={accOffset['z']:.6f}, " +

f"Scales x={accScale['x']:.6f}, y={accScale['y']:.6f}, z={accScale['z']:.6f}")

with open('magCal.txt', 'w') as f:

f.write(f"{magOffset['x']},{magScale['x']},{magOffset['y']},{magScale['y']},{magOffset['z']},{magScale['z']}\n")

saved.append(f"Mag (µT): Offsets x={magOffset['x']:.6f}, y={magOffset['y']:.6f}, z={magOffset['z']:.6f}, " +

f"Scales x={magScale['x']:.6f}, y={magScale['y']:.6f}, z={magScale['z']:.6f}")

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Calibration Saved")

msg.setText("Saved calibration:\n" + "\n".join(saved))

msg.exec_()

generateArduinoCode()

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Calibration Save Error")

msg.setText(f"Error saving calibration files: {str(e)}")

msg.exec_()# --- GUI Setup - Main Window ---

app = QtWidgets.QApplication([])

mainWindow = QtWidgets.QWidget()

mainWindow.setWindowTitle("9-Axis IMU Visualization (Acc/Mag: 1000 pts, Gyro: 100 pts)")

mainWindow.setMinimumSize(1000, 800)

mainLayout = QtWidgets.QVBoxLayout()

mainWindow.setLayout(mainLayout)

plotGrid = QtWidgets.QGridLayout()

plots = {}

scatterItems = {}

curveItems = {}

attitudeItems = {}

plotTitles = [

('accXY', 'Acc XY (m/s²)', 'X', 'Y', (255, 0, 0, 255)), # Red

('accYZ', 'Acc YZ (m/s²)', 'Y', 'Z', (0, 255, 0, 255)), # Green

('accXZ', 'Acc XZ (m/s²)', 'X', 'Z', (0, 0, 255, 255)), # Blue

('magXY', 'Mag XY (µT)', 'X', 'Y', (0, 255, 255, 255)), # Cyan

('magYZ', 'Mag YZ (µT)', 'Y', 'Z', (255, 0, 255, 255)), # Magenta

('magXZ', 'Mag XZ (µT)', 'X', 'Z', (255, 255, 0, 255)), # Yellow

('gyroX', 'Gyro X (°/s)', 'Time (s)', 'X', None),

('gyroY', 'Gyro Y (°/s)', 'Time (s)', 'Y', None),

('gyroZ', 'Gyro Z (°/s)', 'Time (s)', 'Z', None)

]

for idx, (key, title, xLabel, yLabel, color) in enumerate(plotTitles):

plot = pg.PlotWidget(title=title)

plot.setMinimumSize(200, 200)

if 'acc' in key or 'mag' in key:

plot.setAspectLocked(True)

scatterItems[key] = pg.ScatterPlotItem(size=5, brush=pg.mkBrush(*color), pen=None)

plot.addItem(scatterItems[key])

if 'acc' in key:

plot.setRange(xRange=(-10, 10), yRange=(-10, 10)) # ±10 m/s²

if 'mag' in key:

plot.setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2)) # ±1.2 µT

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

plot.setLabel('bottom', xLabel)

plot.setLabel('left', yLabel)

elif 'gyro' in key:

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

plot.setLabel('bottom', xLabel)

plot.setLabel('left', yLabel)

plot.setRange(yRange=(-5, 5)) # ±0.1 °/s for calibrated gyro

curveItems[key] = plot.plot(pen=pg.mkPen('b', width=2))

plotGrid.addWidget(plot, idx // 3, idx % 3)

plots[key] = plot

mainLayout.addLayout(plotGrid)

buttonLayout = QtWidgets.QHBoxLayout()

calibrateGyroButton = QtWidgets.QPushButton("Calibrate Gyro")

calibrateGyroButton.setMinimumSize(200, 50)

calibrateGyroButton.setStyleSheet("background-color: #00FF00; color: black; font-size: 16px;")

calibrateAccButton = QtWidgets.QPushButton("Calibrate Accelerometer")

calibrateAccButton.setMinimumSize(200, 50)

calibrateAccButton.setStyleSheet("background-color: #FFFF00; color: black; font-size: 16px;")

calibrateMagButton = QtWidgets.QPushButton("Calibrate Magnetometer")

calibrateMagButton.setMinimumSize(200, 50)

calibrateMagButton.setStyleSheet("background-color: #FFFF00; color: black; font-size: 16px;")

saveCalButton = QtWidgets.QPushButton("Save Calibration")

saveCalButton.setMinimumSize(200, 50)

saveCalButton.setStyleSheet("background-color: #0000FF; color: white; font-size: 16px;")

generateCodeButton = QtWidgets.QPushButton("Generate Code")

generateCodeButton.setMinimumSize(200, 50)

generateCodeButton.setStyleSheet("background-color: #0000FF; color: white; font-size: 16px;")

stopButton = QtWidgets.QPushButton("Stop")

stopButton.setMinimumSize(200, 50)

stopButton.setStyleSheet("background-color: #FF0000; color: white; font-size: 16px;")

buttonLayout.addWidget(calibrateGyroButton)

buttonLayout.addWidget(calibrateAccButton)

buttonLayout.addWidget(calibrateMagButton)

buttonLayout.addWidget(saveCalButton)

buttonLayout.addWidget(generateCodeButton)

buttonLayout.addWidget(stopButton)

mainLayout.addLayout(buttonLayout)# --- 3D Magnetometer and Attitude Window ---

mag3DWindow = QtWidgets.QWidget()

mag3DWindow.setWindowTitle("3D Magnetic Field Vector and Attitude (µT, °)")

mag3DWindow.setMinimumSize(800, 800)

mag3DLayout = QtWidgets.QGridLayout()

mag3DWindow.setLayout(mag3DLayout)

mag3DView = GLViewWidget()

mag3DView.setCameraPosition(distance=3, elevation=30, azimuth=45)

mag3DLayout.addWidget(mag3DView, 0, 0)

sphereMesh = gl.MeshData.sphere(rows=20, cols=20, radius=1.0)

sphere = gl.GLMeshItem(meshdata=sphereMesh, smooth=True, color=(0.3, 0.3, 0.3, 0.2), shader='shaded', drawFaces=True)

mag3DView.addItem(sphere)

circleMesh = gl.MeshData.cylinder(rows=1, cols=50, radius=[1.0, 1.0], length=0.001)

xyCircle = gl.GLMeshItem(meshdata=circleMesh, smooth=True, color=(1, 0.5, 0, 0.4), shader='shaded', glOptions='additive')

xyCircle.rotate(90, 1, 0, 0)

xyCircle.setDepthValue(-5)

mag3DView.addItem(xyCircle)

for axis, color in zip([(1.2, 0, 0), (0, 1.2, 0), (0, 0, 1.2)], [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]):

line = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], axis]), color=color, width=2, antialias=True)

mag3DView.addItem(line)

vectorArrow = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], [0, 0, 0]]), color=(1, 0, 0, 1), width=3, antialias=True)

vectorArrow.setDepthValue(10)

mag3DView.addItem(vectorArrow)

xyProjection = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], [0, 0, 0]]), color=(0, 1, 1, 1), width=2, antialias=True)

xyProjection.setDepthValue(10)

mag3DView.addItem(xyProjection)

verticalComponent = gl.GLLinePlotItem(pos=np.array([[0, 0, 0], [0, 0, 0]]), color=(1, 0, 1, 1), width=2, antialias=True)

verticalComponent.setDepthValue(10)

mag3DView.addItem(verticalComponent)

vectorTrail = gl.GLLinePlotItem(pos=np.array([[0, 0, 0]]), color=(1, 1, 0, 0.6), width=1, antialias=True)

vectorTrail.setDepthValue(10)

mag3DView.addItem(vectorTrail)

attitudePlots = {}

attitudePlotTitles = [

('rollPlot', 'Roll (°)', '', ''),

('pitchPlot', 'Pitch (°)', '', ''),

('yawPlot', 'Yaw (°)', '', '')

]

for idx, (key, title, xLabel, yLabel) in enumerate(attitudePlotTitles):

plot = pg.PlotWidget(title=title)

plot.setMinimumSize(200, 200)

plot.setRange(xRange=(-1.5, 1.5), yRange=(-1.5, 1.5))

plot.setAspectLocked(True)

plot.hideAxis('bottom')

plot.hideAxis('left')

readout = pg.TextItem("0.0°", anchor=(0.5, 0.5), color=(0, 255, 0))

readout.setFont(QtGui.QFont("Courier New", 14, QtGui.QFont.Bold))

if key in ['rollPlot', 'pitchPlot']:

readout.setPos(0, -1.4)

else:

readout.setPos(0, -1.5)

readout.setZValue(15)

plot.addItem(readout)

attitudeItems[f'{key}_readout'] = readout

if key == 'rollPlot':

skyX = np.array([-1.5, 1.5, 1.5, -1.5])

skyY = np.array([0, 0, 1.5, 1.5])

groundX = np.array([-1.5, 1.5, 1.5, -1.5])

groundY = np.array([-1.5, -1.5, 0, 0])

sky = pg.PlotDataItem(x=skyX, y=skyY, pen=None, fillLevel=1.5, brush=pg.mkBrush(0, 191, 255, 255))

ground = pg.PlotDataItem(x=groundX, y=groundY, pen=None, fillLevel=-1.5, brush=pg.mkBrush(139, 69, 19, 255))

plot.addItem(sky)

plot.addItem(ground)

horizon = pg.PlotDataItem(x=[-1, 1], y=[0, 0], pen=pg.mkPen(color=(0, 100, 0), width=3))

plot.addItem(horizon)

attitudeItems['roll_horizon'] = horizon

for xPos, anchor in [(-1.2, (1, 0.5)), (1.2, (0, 0.5))]:

text = pg.TextItem("0°", anchor=anchor, color=(255, 255, 0))

text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))

text.setPos(xPos, 0)

text.setZValue(10)

plot.addItem(text)

for angle in [30, 60]:

rad = math.radians(angle)

sinRad = math.sin(rad)

cosRad = math.cos(rad)

text = pg.TextItem(f"{angle}°", anchor=(0, 0.5), color=(255, 255, 0))

text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))

text.setPos(1.2 * cosRad, 1.2 * sinRad)

text.setZValue(10)

plot.addItem(text)

text = pg.TextItem(f"{angle}°", anchor=(1, 0.5), color=(255, 255, 0))

text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))

text.setPos(-1.2 * cosRad, 1.2 * sinRad)

text.setZValue(10)

plot.addItem(text)

text = pg.TextItem(f"-{angle}°", anchor=(1, 0.5), color=(255, 255, 0))

text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))

text.setPos(-1.2 * cosRad, -1.2 * sinRad)

text.setZValue(10)

plot.addItem(text)

text = pg.TextItem(f"-{angle}°", anchor=(0, 0.5), color=(255, 255, 0))

text.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))

text.setPos(1.2 * cosRad, -1.2 * sinRad)

text.setZValue(10)

plot.addItem(text)

elif key == 'pitchPlot':

skyX = np.array([-1.5, 1.5, 1.5, -1.5])

skyY = np.array([0, 0, 1.5, 1.5])

groundX = np.array([-1.5, 1.5, 1.5, -1.5])

groundY = np.array([-1.5, -1.5, 0, 0])

sky = pg.PlotDataItem(x=skyX, y=skyY, pen=None, fillLevel=1.5, brush=pg.mkBrush(0, 191, 255, 255))

ground = pg.PlotDataItem(x=groundX, y=groundY, pen=None, fillLevel=-1.5, brush=pg.mkBrush(139, 69, 19, 255))

plot.addItem(sky)

plot.addItem(ground)

ladder = []

for angle in [-90, -60, -30, -10, 0, 10, 30, 60, 90]:

y = angle / 90 * 1.2

if angle == 0:

line = pg.PlotDataItem(x=[-1, 1], y=[y, y], pen=pg.mkPen(color=(0, 100, 0), width=3))

else:

line = pg.PlotDataItem(x=[-0.5, -0.2, -0.2, 0.2, 0.2, 0.5], y=[y, y, y+0.05, y+0.05, y, y], pen=pg.mkPen(color=(0, 100, 0), width=2))

textLeft = pg.TextItem(f"{angle}°", anchor=(1, 0.5), color=(255, 255, 0))

textLeft.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))

textLeft.setPos(-0.6, y)

textRight = pg.TextItem(f"{angle}°", anchor=(0, 0.5), color=(255, 255, 0))

textRight.setFont(QtGui.QFont("Arial", 12, QtGui.QFont.Bold))

textRight.setPos(0.6, y)

plot.addItem(textLeft)

plot.addItem(textRight)

plot.addItem(line)

ladder.append(line)

attitudeItems['pitch_ladder'] = ladder

elif key == 'yawPlot':

theta = np.linspace(0, 2 * np.pi, 100)

xCircle = 1.2 * np.sin(theta)

yCircle = 1.2 * np.cos(theta)

circle = pg.PlotDataItem(x=xCircle, y=yCircle, pen=pg.mkPen('w', width=2), fillLevel=0, brush=pg.mkBrush(0, 0, 0, 255))

plot.addItem(circle)

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

rad = math.radians(angle)

x = 1.3 * math.sin(rad)

y = 1.3 * math.cos(rad)

text = pg.TextItem(label, anchor=(0.5, 0.5), color=(255, 255, 0))

text.setFont(QtGui.QFont("Arial", 12))

text.setPos(x, y)

plot.addItem(text)

needle = pg.PlotDataItem(x=[0, 0], y=[0, 1], pen=pg.mkPen('r', width=3))

plot.addItem(needle)

attitudeItems['yaw_needle'] = needle

if key == 'rollPlot':

mag3DLayout.addWidget(plot, 0, 1)

elif key == 'pitchPlot':

mag3DLayout.addWidget(plot, 1, 0)

elif key == 'yawPlot':

mag3DLayout.addWidget(plot, 1, 1)

attitudePlots[key] = plot# --- Calibrate Gyroscope ---

def calibrateGyro():

global gxOffset, gyOffset, gzOffset

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Calibrate Gyro")

msg.setText("Keep the device still for calibration. Press OK to start.")

msg.exec_()

gxSamples = []

gySamples = []

gzSamples = []

sampleCount = 0

ser.reset_input_buffer()

while sampleCount < 200:

if ser.in_waiting <= 0: continue

try:

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

values = line.split(',')

if len(values) != 9: continue

ax, ay, az, gx, gy, gz, mx, my, mz = map(float, values)

# Convert gyro data from rad/s to °/s

gx = gx * 180.0 / math.pi

gy = gy * 180.0 / math.pi

gz = gz * 180.0 / math.pi

gxSamples.append(gx)

gySamples.append(gy)

gzSamples.append(gz)

sampleCount += 1

except Exception:

pass

time.sleep(0.01)

if not gxSamples: return

gxOffset = np.mean(gxSamples) # Offset in °/s

gyOffset = np.mean(gySamples) # Offset in °/s

gzOffset = np.mean(gzSamples) # Offset in °/s

try:

with open('gCal.txt', 'w') as f:

f.write(f"{gxOffset},{gyOffset},{gzOffset}\n")

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Gyro Calibration Complete")

msg.setText(f"Offsets (°/s): Gx={gxOffset:.6f}, Gy={gyOffset:.6f}, Gz={gzOffset:.6f}\nSaved to gCal.txt")

msg.exec_()

generateArduinoCode()

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Gyro Calibration Error")

msg.setText(f"Error saving gyro offsets: {str(e)}")

msg.exec_()# --- Calibrate Accelerometer ---

def calibrateAcc():

global accCalibrating, accMin, accMax, accOffset, accScale, accRaw, axSamples, aySamples, azSamples, plots

if accCalibrating:

calibrateAccButton.setText("Calibrate Accelerometer")

accCalibrating = False

if axSamples:

accMin['x'] = min(axSamples)

accMin['y'] = min(aySamples)

accMin['z'] = min(azSamples)

accMax['x'] = max(axSamples)

accMax['y'] = max(aySamples)

accMax['z'] = max(azSamples)

accOffset['x'] = (accMin['x'] + accMax['x']) / 2.0

accOffset['y'] = (accMin['y'] + accMax['y']) / 2.0

accOffset['z'] = (accMin['z'] + accMax['z']) / 2.0

rangeX = accMax['x'] - accMin['x']

rangeY = accMax['y'] - accMin['y']

rangeZ = accMax['z'] - accMin['z']

avgRange = (rangeX + rangeY + rangeZ) / 3.0

accScale['x'] = 2.0 / avgRange if avgRange != 0 else 1.0

accScale['y'] = 2.0 / avgRange if avgRange != 0 else 1.0

accScale['z'] = 2.0 / avgRange if avgRange != 0 else 1.0

if avgRange < 0.5 or avgRange > 20.0:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Calibration Warning")

msg.setText(f"Average range {avgRange:.6f} is unusual.\nEnsure full 3D rotation during calibration.")

msg.exec_()

accRaw['x'] = np.empty(0)

accRaw['y'] = np.empty(0)

accRaw['z'] = np.empty(0)

try:

with open('accCal.txt', 'w') as f:

f.write(f"{accOffset['x']},{accScale['x']},{accOffset['y']},{accScale['y']},{accOffset['z']},{accScale['z']}\n")

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Acc Calibration Complete")

msg.setText(f"Offsets (m/s²): Ax={accOffset['x']:.6f}, Ay={accOffset['y']:.6f}, Az={accOffset['z']:.6f}\n" +

f"Scales: Ax={accScale['x']:.6f}, Ay={accScale['y']:.6f}, Az={accScale['z']:.6f}\nSaved to accCal.txt")

msg.exec_()

generateArduinoCode()

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Acc Calibration Error")

msg.setText(f"Error saving acc offsets: {str(e)}")

msg.exec_()

for key in ['accXY', 'accYZ', 'accXZ']:

plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2))

return

accCalibrating = True

accMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}

accMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}

axSamples = []

aySamples = []

azSamples = []

accRaw['x'] = np.empty(0)

accRaw['y'] = np.empty(0)

accRaw['z'] = np.empty(0)

calibrateAccButton.setText("Stop Acc Calibration")

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Accelerometer Calibration")

msg.setText("Rotate device in all directions (full 3D rotation) to capture min/max values for all axes.")

msg.exec_()# --- Calibrate Magnetometer ---

def calibrateMag():

global magCalibrating, magMin, magMax, magOffset, magScale, magRaw, mxSamples, mySamples, mzSamples, magRawMin, magRawMax, plots, mag3DView, vectorArrow, vectorTrail, xyProjection, verticalComponent, trailBuffer

if magCalibrating:

calibrateMagButton.setText("Calibrate Magnetometer")

magCalibrating = False

if mxSamples:

magMin['x'] = min(mxSamples)

magMin['y'] = min(mySamples)

magMin['z'] = min(mzSamples)

magMax['x'] = max(mxSamples)

magMax['y'] = max(mySamples)

magMax['z'] = max(mzSamples)

# --- CENTER OFFSET (unchanged) ---

magOffset['x'] = (magMin['x'] + magMax['x']) / 2.0

magOffset['y'] = (magMin['y'] + magMax['y']) / 2.0

magOffset['z'] = (magMin['z'] + magMax['z']) / 2.0

# --- PER-AXIS SCALING TO ±1.0 (NEW) ---

rangeX = magMax['x'] - magMin['x']

rangeY = magMax['y'] - magMin['y']

rangeZ = magMax['z'] - magMin['z']

magScale['x'] = 2.0 / rangeX if rangeX > 1e-6 else 1.0

magScale['y'] = 2.0 / rangeY if rangeY > 1e-6 else 1.0

magScale['z'] = 2.0 / rangeZ if rangeZ > 1e-6 else 1.0

# Clear buffers

magRaw['x'] = np.empty(0)

magRaw['y'] = np.empty(0)

magRaw['z'] = np.empty(0)

magRawMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}

magRawMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}

trailBuffer = []

vectorArrow.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))

vectorTrail.setData(pos=np.array([[0, 0, 0]]))

xyProjection.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))

verticalComponent.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))

try:

with open('magCal.txt', 'w') as f:

f.write(f"{magOffset['x']},{magScale['x']},{magOffset['y']},{magScale['y']},{magOffset['z']},{magScale['z']}\n")

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Mag Calibration Complete")

msg.setText(

f"Offsets (arb. units): Mx={magOffset['x']:.6f}, My={magOffset['y']:.6f}, Mz={magOffset['z']:.6f}\n"

f"Scales (per-axis to ±1): X={magScale['x']:.6f}, Y={magScale['y']:.6f}, Z={magScale['z']:.6f}\n"

f"Saved to magCal.txt\n"

f"Your XZ circle will now be perfectly round from -1 to +1"

)

msg.exec_()

generateArduinoCode()

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Mag Calibration Error")

msg.setText(f"Error saving mag offsets: {str(e)}")

msg.exec_()

for key in ['magXY', 'magYZ', 'magXZ']:

plots[key].setRange(xRange=(-1.2, 1.2), yRange=(-1.2, 1.2))

mag3DView.opts['distance'] = 3

mag3DView.setCameraPosition(distance=3, elevation=30, azimuth=45)

return

# --- Start Calibration ---

magCalibrating = True

magMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}

magMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}

mxSamples = []

mySamples = []

mzSamples = []

magRaw['x'] = np.empty(0)

magRaw['y'] = np.empty(0)

magRaw['z'] = np.empty(0)

magRawMin = {'x': float('inf'), 'y': float('inf'), 'z': float('inf')}

magRawMax = {'x': float('-inf'), 'y': float('-inf'), 'z': float('-inf')}

trailBuffer = []

vectorArrow.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))

vectorTrail.setData(pos=np.array([[0, 0, 0]]))

xyProjection.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))

verticalComponent.setData(pos=np.array([[0, 0, 0], [0, 0, 0]]))

calibrateMagButton.setText("Stop Mag Calibration")

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Magnetometer Calibration")

msg.setText("Rotate device in figure-8 pattern to capture min/max values for all axes.\n"

"After calibration, XZ circle will be forced to ±1.0")

msg.exec_()

def generateCode():

success = generateArduinoCode(to_file=True)

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Generate Code")

if success:

msg.setText("Arduino calibration function saved to calData.txt")

else:

msg.setText("Error saving calData.txt.")

msg.exec_()# --- Stop Program ---

def stopProgram():

try:

plotTimer.stop()

if ser.is_open:

ser.close()

mainWindow.close()

mag3DWindow.close()

app.exit(0)

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Stop Error")

msg.setText(f"Error stopping program: {str(e)}")

msg.exec_()# --- Update Plots ---

def updatePlots():

global accRaw, gyroRaw, magRaw, timePoints, gxOffset, gyOffset, gzOffset

global accCalibrating, magCalibrating, accMin, accMax, magMin, magMax

global accOffset, accScale, magOffset, magScale, accFiltered, magFiltered

global axSamples, aySamples, azSamples, mxSamples, mySamples, mzSamples

global magRawMin, magRawMax, vectorArrow, vectorTrail, xyProjection, verticalComponent, trailBuffer

global attitudeItems, attitudePlots

if ser.in_waiting <= 0: return

try:

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

values = line.split(',')

if len(values) != 9: return

ax, ay, az, gx, gy, gz, mx, my, mz = map(float, values)

# Convert gyro data from rad/s to °/s

gx = gx * 180.0 / math.pi

gy = gy * 180.0 / math.pi

gz = gz * 180.0 / math.pi

# Apply low-pass filter to accelerometer data

accFiltered['x'] = alpha * ax + (1 - alpha) * accFiltered['x'] if accFiltered['x'] is not None else ax

accFiltered['y'] = alpha * ay + (1 - alpha) * accFiltered['y'] if accFiltered['y'] is not None else ay

accFiltered['z'] = alpha * az + (1 - alpha) * accFiltered['z'] if accFiltered['z'] is not None else az

ax = accFiltered['x'] # m/s²

ay = accFiltered['y']

az = accFiltered['z']

# Apply low-pass filter to magnetometer data

magFiltered['x'] = alpha * mx + (1 - alpha) * magFiltered['x'] if magFiltered['x'] is not None else mx

magFiltered['y'] = alpha * my + (1 - alpha) * magFiltered['y'] if magFiltered['y'] is not None else my

magFiltered['z'] = alpha * mz + (1 - alpha) * magFiltered['z'] if magFiltered['z'] is not None else mz

mx = magFiltered['x'] # QMC5883L arbitrary units

my = magFiltered['y']

mz = magFiltered['z']

# Collect samples during calibration

if accCalibrating:

axSamples.append(ax)

aySamples.append(ay)

azSamples.append(az)

accMin['x'] = min(accMin['x'], ax)

accMin['y'] = min(accMin['y'], ay)

accMin['z'] = min(accMin['z'], az)

accMax['x'] = max(accMax['x'], ax)

accMax['y'] = max(accMax['y'], ay)

accMax['z'] = max(accMax['z'], az)

if magCalibrating:

mxSamples.append(mx)

mySamples.append(my)

mzSamples.append(mz)

magMin['x'] = min(magMin['x'], mx)

magMin['y'] = min(magMin['y'], my)

magMin['z'] = min(magMin['z'], mz)

magMax['x'] = max(magMax['x'], mx)

magMax['y'] = max(magMax['y'], my)

magMax['z'] = max(magMax['z'], mz)

# Apply calibration

axCal = (ax - accOffset['x']) * accScale['x'] # m/s²

ayCal = (ay - accOffset['y']) * accScale['y']

azCal = (az - accOffset['z']) * accScale['z']

gxCal = gx - gxOffset # °/s

gyCal = gy - gyOffset

gzCal = gz - gzOffset

mxCal = (mx - magOffset['x']) * magScale['x'] # µT

myCal = (my - magOffset['y']) * magScale['y']

mzCal = (mz - magOffset['z']) * magScale['z']

# Determine accelerometer data to plot

if accCalibrating:

ax_plot = ax

ay_plot = ay

az_plot = az

else:

ax_plot = axCal

ay_plot = ayCal

az_plot = azCal

# Determine magnetometer data to plot

if magCalibrating:

mx_plot = mx

my_plot = my

mz_plot = mz

else:

mx_plot = mxCal

my_plot = myCal

mz_plot = mzCal

# Calculate roll and pitch from accelerometer (NED: x north, y east, z down)

roll = math.atan2(ayCal, azCal + epsilon) * 180 / math.pi # °

pitch = math.atan2(-axCal, math.sqrt(ayCal**2 + azCal**2 + epsilon)) * 180 / math.pi # °

phi = math.radians(roll)

theta = math.radians(pitch)

# Compute tilt-compensated yaw

mxH = mxCal * math.cos(theta) + myCal * math.sin(phi) * math.sin(theta) + mzCal * math.cos(phi) * math.sin(theta)

myH = myCal * math.cos(phi) - mzCal * math.sin(phi)

yaw = math.atan2(myH, mxH + epsilon) * 180 / math.pi

if yaw < 0:

yaw += 360 # Normalize to 0-360°

# Update attitude readouts

attitudeItems['rollPlot_readout'].setText(f"Roll: {-roll:+.1f}°")

attitudeItems['pitchPlot_readout'].setText(f"Pitch: {-pitch:+.1f}°")

attitudeItems['yawPlot_readout'].setText(f"Yaw: {yaw:.1f}°") # Update 3D magnetic vector visualization

vec = np.array([mx_plot, my_plot, mz_plot])

norm = np.linalg.norm(vec)

if norm > epsilon:

unitVec = vec / norm

vectorArrow.setData(pos=np.array([[0, 0, 0], unitVec]), color=(1, 0, 0, 1), width=3)

xyProj = np.array([unitVec[0], unitVec[1], 0])

xyProjection.setData(pos=np.array([[0, 0, 0], xyProj]), color=(0, 1, 1, 1), width=2)

verticalComponent.setData(pos=np.array([xyProj, unitVec]), color=(1, 0, 1, 1), width=2)

trailBuffer.append(unitVec)

trailBuffer = trailBuffer[-trailMaxPoints:]

vectorTrail.setData(pos=np.array(trailBuffer), color=(1, 1, 0, 0.6), width=1)

# Update plot ranges for accelerometer if in raw mode

if accCalibrating or (accScale['x'] == 1.0 and accOffset['x'] == 0.0):

accRawMin = {'x': min(accRaw['x'], default=ax_plot) if len(accRaw['x']) > 0 else ax_plot,

'y': min(accRaw['y'], default=ay_plot) if len(accRaw['y']) > 0 else ay_plot,

'z': min(accRaw['z'], default=az_plot) if len(accRaw['z']) > 0 else az_plot}

accRawMax = {'x': max(accRaw['x'], default=ax_plot) if len(accRaw['x']) > 0 else ax_plot,

'y': max(accRaw['y'], default=ay_plot) if len(accRaw['y']) > 0 else ay_plot,

'z': max(accRaw['z'], default=az_plot) if len(accRaw['z']) > 0 else az_plot}

xMin = accRawMin['x'] * 1.1

xMax = accRawMax['x'] * 1.1

yMin = accRawMin['y'] * 1.1

yMax = accRawMax['y'] * 1.1

zMin = accRawMin['z'] * 1.1

zMax = accRawMax['z'] * 1.1

plots['accXY'].setRange(xRange=(xMin, xMax), yRange=(yMin, yMax))

plots['accYZ'].setRange(xRange=(yMin, yMax), yRange=(zMin, zMax))

plots['accXZ'].setRange(xRange=(xMin, xMax), yRange=(zMin, zMax))

# Update plot ranges for magnetometer if in raw mode

use_raw_mag_range = magCalibrating or (magScale['x'] == 1.0 and magOffset['x'] == 0.0)

if use_raw_mag_range:

magRawMin['x'] = min(magRawMin['x'], mx)

magRawMax['x'] = max(magRawMax['x'], mx)

magRawMin['y'] = min(magRawMin['y'], my)

magRawMax['y'] = max(magRawMax['y'], my)

magRawMin['z'] = min(magRawMin['z'], mz)

magRawMax['z'] = max(magRawMax['z'], mz)

xMin = magRawMin['x'] * 1.1

xMax = magRawMax['x'] * 1.1

yMin = magRawMin['y'] * 1.1

yMax = magRawMax['y'] * 1.1

zMin = magRawMin['z'] * 1.1

zMax = magRawMax['z'] * 1.1

mag3DView.opts['distance'] = max(xMax - xMin, yMax - yMin, zMax - zMin) * 1.5

mag3DView.setCameraPosition(distance=mag3DView.opts['distance'])

plots['magXY'].setRange(xRange=(xMin, xMax), yRange=(yMin, yMax))

plots['magYZ'].setRange(xRange=(yMin, yMax), yRange=(zMin, zMax))

plots['magXZ'].setRange(xRange=(xMin, xMax), yRange=(zMin, zMax))

# Store data

accRaw['x'] = np.append(accRaw['x'], ax_plot)[-accMagMaxPoints:]

accRaw['y'] = np.append(accRaw['y'], ay_plot)[-accMagMaxPoints:]

accRaw['z'] = np.append(accRaw['z'], az_plot)[-accMagMaxPoints:]

gyroRaw['x'] = np.append(gyroRaw['x'], gxCal)[-gyroMaxPoints:]

gyroRaw['y'] = np.append(gyroRaw['y'], gyCal)[-gyroMaxPoints:]

gyroRaw['z'] = np.append(gyroRaw['z'], gzCal)[-gyroMaxPoints:]

magRaw['x'] = np.append(magRaw['x'], mx_plot)[-accMagMaxPoints:]

magRaw['y'] = np.append(magRaw['y'], my_plot)[-accMagMaxPoints:]

magRaw['z'] = np.append(magRaw['z'], mz_plot)[-accMagMaxPoints:]

elapsed = startTime.msecsTo(QtCore.QTime.currentTime()) / 1000.0

timePoints = np.append(timePoints, elapsed)[-gyroMaxPoints:]

# Update 2D scatter plots

scatterItems['accXY'].setData(x=accRaw['x'], y=accRaw['y'])

scatterItems['accYZ'].setData(x=accRaw['y'], y=accRaw['z'])

scatterItems['accXZ'].setData(x=accRaw['x'], y=accRaw['z'])

scatterItems['magXY'].setData(x=magRaw['x'], y=magRaw['y'])

scatterItems['magYZ'].setData(x=magRaw['y'], y=magRaw['z'])

scatterItems['magXZ'].setData(x=magRaw['x'], y=magRaw['z'])

if len(timePoints) <= 0: return

xMin = max(0, timePoints[-1] - gyroTimeWindow)

xMax = timePoints[-1]

for key in ['gyroX', 'gyroY', 'gyroZ']:

plots[key].setRange(xRange=(xMin, xMax))

curveItems[key].setData(x=timePoints, y=gyroRaw[key[-1].lower()])

# Update roll horizon

rollRad = math.radians(roll)

x1 = -1 * math.cos(rollRad)

y1 = 1 * math.sin(rollRad)

x2 = 1 * math.cos(rollRad)

y2 = -1 * math.sin(rollRad)

attitudeItems['roll_horizon'].setData(x=[x1, x2], y=[y1, y2])

# Update pitch ladder

pitchOffset = -pitch / 90 * 1.2

for idx, angle in enumerate([-90, -60, -30, -10, 0, 10, 30, 60, 90]):

yBase = angle / 90 * 1.2

yShifted = yBase + pitchOffset

if angle == 0:

attitudeItems['pitch_ladder'][idx].setData(x=[-1, 1], y=[yShifted, yShifted])

else:

attitudeItems['pitch_ladder'][idx].setData(x=[-0.5, -0.2, -0.2, 0.2, 0.2, 0.5], y=[yShifted, yShifted, yShifted+0.05, yShifted+0.05, yShifted, yShifted])

# Update yaw needle

yawRad = math.radians(yaw)

needleX = 1 * math.sin(yawRad)

needleY = 1 * math.cos(yawRad)

attitudeItems['yaw_needle'].setData(x=[0, needleX], y=[0, needleY])

except Exception:

pass# --- Connect Buttons and Start Application ---

calibrateGyroButton.clicked.connect(calibrateGyro)

calibrateAccButton.clicked.connect(calibrateAcc)

calibrateMagButton.clicked.connect(calibrateMag)

saveCalButton.clicked.connect(saveCalibration)

generateCodeButton.clicked.connect(generateCode)

stopButton.clicked.connect(stopProgram)

ser.reset_input_buffer()

loadCalibration()

plotTimer = QtCore.QTimer()

plotTimer.timeout.connect(updatePlots)

plotTimer.start(50)

mainWindow.show()

mag3DWindow.show()

try:

app.exec_()

except Exception as e:

msg = QtWidgets.QMessageBox()

msg.setWindowTitle("Application Error")

msg.setText(f"Application error: {str(e)}")

msg.exec_()

finally:

if ser.is_open:

ser.close()

Then finally we take the calibration parameters from the python program, and incorporate them on the Arduino side to allow reading the data from the sensors, and reporting calibrated numbers. Remember, in the program below, you should use your calibration parameters instead of mine. That is, edit the program below for your specific calibration numbers.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;
float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
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 = -125.54461152573242;
  const float myOffset = 124.52589039569995;
  const float mzOffset = 965.8106108557855;
  const float mxScale = 0.0010185143396312232;
  const float myScale = 0.0010185143396312232;
  const float mzScale = 0.0010185143396312232;
    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();

Serial.print(AxCal);
Serial.print(',');
Serial.print(AyCal);
Serial.print(',');
Serial.print(AzCal);
Serial.print(',');
Serial.print(GxCal);
Serial.print(',');
Serial.print(GyCal);
Serial.print(',');
Serial.print(GzCal);
Serial.print(',');
Serial.print(MxCal);
Serial.print(',');
Serial.print(MyCal);
Serial.print(',');
Serial.println(MzCal);
delay(100);
}

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

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

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

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

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

Serial.begin(115200);

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 = -125.54461152573242;

const float myOffset = 124.52589039569995;

const float mzOffset = 965.8106108557855;

const float mxScale = 0.0010185143396312232;

const float myScale = 0.0010185143396312232;

const float mzScale = 0.0010185143396312232;

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

Serial.print(AxCal);

Serial.print(',');

Serial.print(AyCal);

Serial.print(',');

Serial.print(AzCal);

Serial.print(',');

Serial.print(GxCal);

Serial.print(',');

Serial.print(GyCal);

Serial.print(',');

Serial.print(GzCal);

Serial.print(',');

Serial.print(MxCal);

Serial.print(',');

Serial.print(MyCal);

Serial.print(',');

Serial.println(MzCal);

delay(100);

}

Python

Python Program to Convert GPS Log file into a KML File for Display on Google Earth

October 22, 2025 admin

In this video lesson we show you how you can transfer data from your Raspberry Pi Pico W GPS Project to your PC for display on Google Earth. This program transfers the Pi Pico GPS Log file to your PC, and as the transfer is done, the program converts the log.txt file into a .kml file for display on Google Earth.

import serial
import time
serialPort = 'COM7'
baudRate = 115200
ser = serial.Serial(serialPort, baudRate, timeout = .1)
time.sleep(1)
ser.write(b"f=open('log.txt','r')\r\n")
line = ser.readline().decode('utf-8').strip()
print("Initial Line: "+line)
time.sleep(.1)
kmlHeader = """<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2">
  <Document>
    <name>Path from Pico</name>
    <Style id="pointStyle">
      <IconStyle>
        <Icon><href>http://maps.google.com/mapfiles/kml/shapes/placemark_circle.png</href></Icon>
        <scale>1.0</scale>
      </IconStyle>
    </Style>
    <Style id="lineStyle">
      <LineStyle><color>ff0000ff</color><width>3</width></LineStyle>
    </Style>
    <Placemark>
      <styleUrl>#pointStyle</styleUrl>
      <MultiGeometry> """
pointStart = "<Point><coordinates>"
pointEnd = "</coordinates></Point>"
lineStringStart = """ </MultiGeometry>
    </Placemark>
    <Placemark>
      <name>Path</name>
      <styleUrl> #lineStyle</styleUrl>
      <LineString>
        <tessellate>1</tessellate>
        <altitudeMode>clampToGround</altitudeMode>
        <coordinates> """
lineStringEnd = """ </coordinates>
      </LineString>
    </Placemark>
  </Document>
</kml> """
with open('path.kml','w') as kml:
    kml.write(kmlHeader)
with open('lon-lat.txt', 'w') as lonLat:
    pass
with open('path.kml','a') as kml:
    with open('lon-lat.txt','a') as lonLat:
        while line != "''":
            ser.write(b"f.readline()\r\n")
            line = ser.readline().decode('utf-8').strip()
            line = ser.readline().decode('utf-8').strip()
            print("LINE: "+line)
            if line.startswith("'") and line !="''":
                line = line[1:-3]
                myData = line.split(',')
                line = myData[1]+','+myData[0]
                print("New Line: ",line)
                kml.write(pointStart+'\n'+line+ '\n'+pointEnd+'\n')              
                lonLat.write(line+ " "+'\n')
with open('lon-lat.txt','r') as lonLat:
    coordinates = lonLat.read()
print(coordinates)
with open('path.kml','a') as kml:
    kml.write(lineStringStart+'\n'+ coordinates+lineStringEnd)
ser.write(b"f.close()\r\n")
ser.close()
print("KML File Saved as "+ 'path.kml')

import serial

import time

serialPort = 'COM7'

baudRate = 115200

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

time.sleep(1)

ser.write(b"f=open('log.txt','r')\r\n")

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

print("Initial Line: "+line)

time.sleep(.1)

kmlHeader = """<?xml version="1.0" encoding="UTF-8"?>

<Icon><href>http://maps.google.com/mapfiles/kml/shapes/placemark_circle.png</href></Icon>

</IconStyle>

</Style>

</Style>

<styleUrl>#pointStyle</styleUrl>

<MultiGeometry> """

pointStart = "<Point><coordinates>"

pointEnd = "</coordinates></Point>"

lineStringStart = """ </MultiGeometry>

</Placemark>

<styleUrl> #lineStyle</styleUrl>

<altitudeMode>clampToGround</altitudeMode>

<coordinates> """

lineStringEnd = """ </coordinates>

</LineString>

</Placemark>

</Document>

</kml> """

with open('path.kml','w') as kml:

kml.write(kmlHeader)

with open('lon-lat.txt', 'w') as lonLat:

pass

with open('path.kml','a') as kml:

with open('lon-lat.txt','a') as lonLat:

while line != "''":

ser.write(b"f.readline()\r\n")

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

print("LINE: "+line)

if line.startswith("'") and line !="''":

line = line[1:-3]

myData = line.split(',')

line = myData[1]+','+myData[0]

print("New Line: ",line)

kml.write(pointStart+'\n'+line+ '\n'+pointEnd+'\n')

lonLat.write(line+ " "+'\n')

with open('lon-lat.txt','r') as lonLat:

coordinates = lonLat.read()

print(coordinates)

with open('path.kml','a') as kml:

kml.write(lineStringStart+'\n'+ coordinates+lineStringEnd)

ser.write(b"f.close()\r\n")

ser.close()

print("KML File Saved as "+ 'path.kml')

Arduino, Python

Improving Digital Compass Accuracy With a Low Pass Filter

October 22, 2025 admin

In this video lesson we add a low pass filter to our calibration and digital compass program. This allows more precise calibration, and removes the jitter from the digital compass display. We continue to use the QMC5883L 3-Axis Magnetometer, which is on our GY-87 IMU module. We are using the following schematic for our project:

We collect the data using the GY-87 connected to an Arduino. Below is the simple program which takes the magnetometer data and sends it to the serial port.

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

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

int x,y,z;

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

Serial.begin(115200);

mpu.begin();

mpu.setI2CBypass(true);

compass.init();

}

void loop() {

compass.read();

x = compass.getX();

y = compass.getY();

z = compass.getZ();

Serial.print(x);

Serial.print(',');

Serial.print(y);

Serial.print(',');

Serial.println(z);

delay(100);

}

The code for the Python side to do the calibration and display a Digital Compass is:

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

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


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

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

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

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

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


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

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

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

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

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

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

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


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

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

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

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

import serial

import numpy as np

from PyQt5 import QtWidgets, QtCore, QtGui

import pyqtgraph as pg

import math

serialPort = 'COM8'

baudRate =115200

to =.1

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

maxPoints = 2000

xRaw = np.empty(0)

yRaw = np.empty(0)

zRaw = np.empty(0)

xCal = np.empty(0)

yCal = np.empty(0)

zCal = np.empty(0)

x=0

y=0

z=0

calibrated = False

offsets = np.zeros(3)

scales = np.ones(3)

app = QtWidgets.QApplication([])

mainWindow = QtWidgets.QWidget()

mainWindow.setWindowTitle("Magnetometer Calibratioin")

mainLayout =QtWidgets.QVBoxLayout()

mainWindow.setLayout(mainLayout)

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

statusLabel.setAlignment(QtCore.Qt.AlignCenter)

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

mainLayout.addWidget(statusLabel)

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

minMaxLabel.setAlignment(QtCore.Qt.AlignCenter)

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

mainLayout.addWidget(minMaxLabel)

calibrateButton = QtWidgets.QPushButton("Start Calibration")

calibrateButton.setCheckable(True)

mainLayout.addWidget(calibrateButton)

plotLayout =QtWidgets.QGridLayout()

mainLayout.addLayout(plotLayout)

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

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

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

for plot in [xyPlot, yzPlot, xzPlot]:

plot.setAspectLocked(True)

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

plot.setMinimumSize(300,300)

plotLayout.addWidget(xyPlot, 0,0)

plotLayout.addWidget(yzPlot, 0,1)

plotLayout.addWidget(xzPlot, 1,0)

xyScatter = pg.ScatterPlotItem(size=5)

yzScatter = pg.ScatterPlotItem(size=5)

xzScatter = pg.ScatterPlotItem(size=5)

xyPlot.addItem(xyScatter)

yzPlot.addItem(yzScatter)

xzPlot.addItem(xzScatter)

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

headingPlot.setAspectLocked(True)

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

headingPlot.setMinimumSize(300,300)

headingPlot.hideAxis('left')

headingPlot.hideAxis('bottom')

plotLayout.addWidget(headingPlot,1,1)

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

headingPlot.addItem(compassCircle)

compassCircle.removeHandle(0)

compassCircle.setZValue(0)

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

headingPlot.addItem(arrowShaft)

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

dot.setZValue(1)

headingPlot.addItem(dot)

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

rad = angle/360*2*math.pi

xPos = np.cos(rad)*1.8

yPos = np.sin(rad)*1.8

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

font=QtGui.QFont()

font.setBold(True)

font.setPointSize(12)

text.setFont(font)

text.setPos(xPos,yPos)

headingPlot.addItem(text)

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

liveHeading.setFont(font)

liveHeading.setPos(0,-2.2)

liveHeading.setZValue(1)

headingPlot.addItem(liveHeading)

def toggleCalibration():

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

if calibrateButton.isChecked():

calibrateButton.setText("Stop Calibration")

statusLabel.setText("Mode: Calibration")

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

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

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

calibrated = False

xRaw = np.empty(0)

yRaw = np.empty(0)

zRaw = np.empty(0)

if not calibrateButton.isChecked():

calibrateButton.setText("Start Calibration")

statusLabel.setText("Mode: Calibrated")

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

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

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

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

offsets = (maxVals + minVals)/2

scales = 2.0/(maxVals-minVals)

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

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

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

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

print("Calibration Completed.")

print("Offsets: ", offsets)

print("Scales: ", scales)

xCal = np.empty(0)

yCal = np.empty(0)

zCal = np.empty(0)

calibrated = True

calibrateButton.clicked.connect(toggleCalibration)

def updatePlot():

global xRaw, yRaw, zRaw, xCal, yCal, zCal, x, y ,z

if ser.in_waiting > 0:

try:

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

values = line.split(',')

if len(values)==3:

x = x*.75 + float(values[0])*.25

y = y*.75 + float(values[1])*.25

z = z*.75 + float(values[2])*.25

if not calibrated:

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

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

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

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

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

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

if len(xRaw)>10:

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

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

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

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

+str(maxVals.round(2)))

if calibrated:

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

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

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

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

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

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

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

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

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

headRad= math.atan2(yC,xC)

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

compassRad= -headRad+ math.pi/2

compassDeg = (-headDeg + 90)%360

xDot=1.5*math.cos(compassRad)

yDot=1.5*math.sin(compassRad)

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

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

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

except Exception as e:

print("Parse Error: ", e)

plotTimer =QtCore.QTimer()

plotTimer.timeout.connect(updatePlot)

plotTimer.start(50)

mainWindow.show()

try:

app.exec_()

finally:

ser.close()

Technology Tutorials

All posts by admin

Tilt Compensated Digital Compass

High Performance Roll, Pitch and Yaw values from the GY-87 IMU Module

Ultimate 9-axis Program for Easily and Accurately Calibrating a 9-axis IMU on Arduino

Python Program to Convert GPS Log file into a KML File for Display on Google Earth

Improving Digital Compass Accuracy With a Low Pass Filter

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