I am working on a mobile application using magnetometer, gyroscope and accelerometer to get information about device attitude as yaw, pitch and roll.
I am using Expo which has a DeviceMotion API giving me directly euler angles from IOS / Android attitude system APIs.
I try to manage to convert those angles relatives to “earth” after a user calibration or “zero”, to act like an EFIS screen in an airplane.
The smartphone will be installed in an arbitrary position in an airplane cockpit to give audio information about the airplane attitude (roll, pitch and yaw).
Once the device installed and fixed, the user will “calibrate” to have a reference zero for the fly.
I manage to rotate the angles by converting euler angles to quaternions and “multiply by inverse of attitude”, full example in js :
import { DeviceMotion, DeviceMotionMeasurement } from "expo-sensors";
import { quat } from "gl-matrix";
import { EventSubscription } from "react-native";
// Euler data in rad
export type EulerData = {
roll: number;
pitch: number;
yaw: number;
};
export class Fusion {
motion_sub?: EventSubscription;
raw_euler_data?: EulerData;
euler_data?: EulerData;
reference_quaternion?: quat; // reference quaternion frame
start() {
// @ts-ignore
this.motion_sub = DeviceMotion.addListener((data) => {
this.raw_euler_data = motionRotationDataToEuler(data);
this.tick();
});
DeviceMotion.setUpdateInterval(200); // 5Hz
}
stop() {
this.motion_sub?.remove();
this.motion_sub = undefined;
}
tick() {
if (this.raw_euler_data) {
if (this.reference_quaternion) {
const currentQuat = eulerToQuat(this.raw_euler_data);
const relativeQuat = quat.multiply(quat.create(), this.reference_quaternion, currentQuat);
const euler = quatToEuler(relativeQuat);
this.euler_data = euler;
} else {
this.euler_data = this.raw_euler_data;
}
let roll = radToDeg(this.euler_data.roll);
let pitch = radToDeg(this.euler_data.pitch);
let yaw = radToDeg(this.euler_data.pitch);
console.log(roll, pitch, yaw);
}
}
reference() {
if (this.raw_euler_data) {
const ref = JSON.parse(JSON.stringify(this.raw_euler_data)) as EulerData;
const q = eulerToQuat(ref);
this.reference_quaternion = quat.invert(quat.create(), q);
this.tick();
}
}
}
// const degToRad = (deg: number) => deg * (Math.PI / 180);
const radToDeg = (rad: number) => rad * (180 / Math.PI);
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
const eulerToQuat = (data: EulerData) => {
const { roll, pitch, yaw } = data;
const cr = Math.cos(roll * 0.5);
const sr = Math.sin(roll * 0.5);
const cp = Math.cos(pitch * 0.5);
const sp = Math.sin(pitch * 0.5);
const cy = Math.cos(yaw * 0.5);
const sy = Math.sin(yaw * 0.5);
const q = quat.fromValues(
sr * cp * cy - cr * sp * sy, // x
cr * sp * cy + sr * cp * sy, // y
cr * cp * sy - sr * sp * cy, // z
cr * cp * cy + sr * sp * sy, // w
);
return q;
};
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
const quatToEuler = (q: quat): EulerData => {
const [x, y, z, w] = q;
// roll (x-axis rotation)
const sinr_cosp = 2 * (w * x + y * z);
const cosr_cosp = 1 - 2 * (x * x + y * y);
const roll = Math.atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
const sinp = Math.sqrt(1 + 2 * (w * y - x * z));
const cosp = Math.sqrt(1 - 2 * (w * y - x * z));
const pitch = 2 * Math.atan2(sinp, cosp) - Math.PI / 2;
// yaw (z-axis rotation)
const siny_cosp = 2 * (w * z + x * y);
const cosy_cosp = 1 - 2 * (y * y + z * z);
const yaw = Math.atan2(siny_cosp, cosy_cosp);
return {
roll,
pitch,
yaw,
};
};
const motionRotationDataToEuler = (motionData: DeviceMotionMeasurement): EulerData => {
return {
roll: motionData.rotation.gamma,
pitch: motionData.rotation.beta,
yaw: motionData.rotation.alpha,
};
};
In this way, the angles are relatives to my reference frame.
It works if my device is perfectly aligned with the plane and horizontal.
I try to manage to make this angles relatives to the “world” and not to the device itself.
If my device is straight and vertical for example, the “roll” should be replaced by the “yaw” and inverse.
Could someone help me to understand those references, conversions and try to fix that ?