plotjuggler_yaw_function.lua

function yawAngleFromQ(time, value, v1, v2, v3)
    M_PI = 3.14159265358979323846
    a = v1
    b = v2
    c = v3
    d = value

    aa = a * a
    ab = a * b
    ac = a * c
    ad = a * d
    bb = b * b
    bc = b * c
    bd = b * d
    cc = c * c
    cd = c * d
    dd = d * d

    dcm00 = aa + bb - cc - dd
    dcm01 = 2.0 * (bc - ad)
    dcm02 = 2.0 * (ac + bd)
    dcm10 = 2.0 * (bc + ad)
    dcm11 = aa - bb + cc - dd
    dcm12 = 2.0 * (cd - ab)
    dcm20 = 2.0 * (bd - ac)
    dcm21 = 2.0 * (ab + cd)
    dcm22 = aa - bb - cc + dd

    phi_val = math.atan(dcm21, dcm22)
    theta_val = math.asin(-dcm20)
    psi_val = math.atan(dcm10, dcm00)
    pi = M_PI

    if (math.abs(theta_val - pi / 2) < 1.0e-3) then
        phi_val = 0
        psi_val = math.atan(dcm12, dcm02)
    elseif (math.abs(theta_val + pi / 2) < 1.0e-3) then
        phi_val = 0
        psi_val = math.atan(-dcm12, -dcm02)
    end

    return psi_val * (180 / M_PI)
end

print(yawAngleFromQ(0.0, 0.0, 0.0, 0.0, 1.0))


Taken from: https://www.programmersought.com/article/22117129352/
Add in plotjuggler as:
M_PI = 3.14159265358979323846
a = v1
b = v2
c = v3
d = value

aa = a * a
ab = a * b
ac = a * c
ad = a * d
bb = b * b
bc = b * c
bd = b * d
cc = c * c
cd = c * d
dd = d * d

dcm00 = aa + bb - cc - dd
dcm01 = 2.0 * (bc - ad)
dcm02 = 2.0 * (ac + bd)
dcm10 = 2.0 * (bc + ad)
dcm11 = aa - bb + cc - dd
dcm12 = 2.0 * (cd - ab)
dcm20 = 2.0 * (bd - ac)
dcm21 = 2.0 * (ab + cd)
dcm22 = aa - bb - cc + dd

phi_val = math.atan(dcm21, dcm22)
theta_val = math.asin(-dcm20)
psi_val = math.atan(dcm10, dcm00)
pi = M_PI

if (math.abs(theta_val - pi / 2) < 1.0e-3) then
    phi_val = 0
    psi_val = math.atan(dcm12, dcm02)
elseif (math.abs(theta_val + pi / 2) < 1.0e-3) then
    phi_val = 0
    psi_val = math.atan(-dcm12, -dcm02)
end

return psi_val * (180 / M_PI)