Add Compass

src/exports.jl

@@ -85,6 +85,7 @@ combine,
 floordist,
 slew,
 Orientations,
+Compass,
 
 # Reconstruction
 RecStageRange,

src/root/calibration.jl

@@ -64,3 +64,34 @@ function Base.read(filename::AbstractString, T::Type{Orientations})
     end
     T(module_ids, times, quaternions)
 end
+
+"""
+A compass with yaw, pitch and roll.
+"""
+struct Compass
+    yaw::Float64
+    pitch::Float64
+    roll::Float64
+end
+
+"""
+
+Initialises a [`Compass`](@ref) from a [`Quaternion`](@ref).
+
+"""
+function Compass(q::Quaternion)
+    yaw = -atan(2.0 * (q.q0 * q.qz + q.qx * q.qy), 1.0 - 2.0 * (q.qy * q.qy + q.qz * q.qz))
+    sp = 2.0 * (q.q0 * q.qy - q.qz * q.qx)
+
+    if (sp >= +1.0)
+        pitch = asin(+1.0)
+    elseif (sp <= -1.0)
+        pitch = asin(-1.0)
+    else
+        pitch = asin(sp)
+    end
+
+    roll = -atan(2.0 * (q.q0 * q.qx + q.qy * q.qz), 1.0 - 2.0 * (q.qx * q.qx + q.qy * q.qy))
+
+    Compass(yaw, pitch, roll)
+end

test/calibration.jl

@@ -41,3 +41,12 @@ end
     @test 1.693407821152e9 == qdata.t[1]
     @test [0.8260205110995139, 0.003912907129683348, -0.004395551387888641, -0.5636093359133512] == qdata.q[1]
 end
+
+@testset "Compass" begin
+    q = Quaternion(0.8260205110995139, 0.003912907129683348, -0.004395551387888641, -0.5636093359133512)
+    c = Compass(q)
+    @test c.yaw == 1.1975374212207646
+    @test c.pitch == -0.0028509330922497
+    @test c.roll == -0.011419325278029469
+
+end