eulerAnglesOpenlog.py

# Usage
# python3 eulerAngles.py --logfile dataLog00017.TXT --outfile dataLog00017.csv

from math import atan2, asin, sqrt, pi
import pandas as pd
import argparse


# Parse arguments
ap = argparse.ArgumentParser()
ap.add_argument("-f", "--logfile", required=True,
	help="path to the OpenLog output file")
ap.add_argument("-o", "--outfile", required=True,
	help="path to the output CSV file")
args = vars(ap.parse_args())

# Load csv (openlog output) and read quaternion values
olog = pd.read_csv(args["logfile"])

def eulerVals(q1, q2, q3):
    """
    Convert quaternion values to Euler angles in degrees:
    pitch
    roll
    yaw
    """
    q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)))
    q2sqr = q2 * q2

    t0 = 2 * (q0 * q1 + q2 * q3)
    t1 = 1 - 2 * (q1 * q1 + q2sqr)
    roll = atan2(t0, t1) * 180.0 / pi

    t2 = +2.0 * (q0 * q2 - q3 * q1)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch = asin(t2) * 180.0 / pi

    t3 = +2.0 * (q0 * q3 + q1 * q2)
    t4 = +1.0 - 2.0 * (q2sqr + q3 * q3)
    yaw = atan2(t3, t4) * 180.0 / pi
    
    return pitch, roll, yaw

olog['pitch'], olog['roll'], olog['yaw'] = zip(*map(eulerVals, olog['Q9_1'], olog['Q9_2'], olog['Q9_3']))

olog.to_csv(args["outfile"])