42 lines
1.5 KiB
Python
42 lines
1.5 KiB
Python
import numpy as np
|
|
from scipy.spatial.transform import Rotation as R
|
|
|
|
# Dados de entrada
|
|
acc_x, acc_y, acc_z = 0.030, -0.197, 0.980
|
|
gyro_x, gyro_y, gyro_z = 0.000, 0.000, 0.000
|
|
angle_x, angle_y, angle_z = -11.426, -1.725, 21.127
|
|
mag_x, mag_y, mag_z = -13.708, 250.175, 100.842
|
|
quat_0, quat_1, quat_2, quat_3 = 0.97827, -0.09509, -0.03299, 0.18091
|
|
|
|
# Função para calcular a orientação magnética
|
|
def calculate_magnetic_orientation(acc, gyro, mag, quat):
|
|
# Converter quaternions para ângulos de Euler
|
|
r = R.from_quat([quat[1], quat[2], quat[3], quat[0]])
|
|
euler = r.as_euler('xyz', degrees=True)
|
|
|
|
# Calcular a direção do norte magnético
|
|
pitch = np.radians(euler[0])
|
|
roll = np.radians(euler[1])
|
|
|
|
# Corrigir os dados do magnetômetro com base na inclinação e rotação
|
|
mag_x_corr = mag[0] * np.cos(pitch) + mag[2] * np.sin(pitch)
|
|
mag_y_corr = mag[0] * np.sin(roll) * np.sin(pitch) + mag[1] * np.cos(roll) - mag[2] * np.sin(roll) * np.cos(pitch)
|
|
|
|
# Calcular o ângulo de direção (yaw) em radianos e converter para graus
|
|
yaw = np.arctan2(-mag_y_corr, mag_x_corr) * 180.0 / np.pi
|
|
|
|
# Ajustar yaw para o intervalo [0, 360)
|
|
if yaw < 0:
|
|
yaw += 360
|
|
|
|
return yaw
|
|
|
|
# Dados de entrada organizados
|
|
acc = [acc_x, acc_y, acc_z]
|
|
gyro = [gyro_x, gyro_y, gyro_z]
|
|
mag = [mag_x, mag_y, mag_z]
|
|
quat = [quat_0, quat_1, quat_2, quat_3]
|
|
|
|
# Calcular a orientação magnética
|
|
yaw = calculate_magnetic_orientation(acc, gyro, mag, quat)
|
|
print(f"Orientação Magnética (Yaw): {yaw:.2f} graus") |