Вращение отношения с смартфонами от эталонной рамы, самолета ⇐ Javascript

[Anonymous]

Я работаю над мобильным приложением, используя магнитометр, гироскоп и акселерометр, чтобы получить информацию о отношении к устройству в качестве рыскания, шага и броска. «Ноль», чтобы вести себя как экран EFIS в самолете. Уголы путем преобразования углов Эйлера в кватернионы и «умножьте на отношение к отношению», полный пример в JS: < /p>
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/Conversio ... ler_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/Conversio ... ler_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,
};
};
< /code>
Таким образом, углы являются родственниками моего эталонного кадра.
он работает, если мое устройство идеально выровнен с плоскостью и горизонтальными. обратный.>

Подробнее здесь: https://stackoverflow.com/questions/795 ... plane-like