Fix the orientation of the kite in KiteViewer (#80)

* use fix_view branch

* less pre-compilation

* change sign of x and y in calc_orient_quat

* add param old to calc_orient_quat

* fix orientation in viewer

* fix test for heading

* precompile KPS3

examples_3d/parking_4p.jl

@@ -4,7 +4,7 @@ using Pkg, Timers
 tic()
 if ! ("KiteViewers" ∈ keys(Pkg.project().dependencies))
     Pkg.activate("examples_3d")
-    pkg"add KiteModels#main"
+    pkg"add KiteModels#fix_view"
 end
 using KiteModels, KitePodModels, KiteUtils, Rotations, StaticArrays
 using ControlPlots, KiteViewers
@@ -65,7 +65,8 @@ function simulate(integrator, steps, plot=PLOT)
             end
             sleep(0.05)           
         end
-    sys_state = SysState(kps4)
+        sys_state = SysState(kps4)
+        sys_state.orient = calc_orient_quat(kps4; old=true)
         KiteViewers.update_system(viewer, sys_state; scale = 0.08, kite_scale=3)
     end
     iter / steps

src/KiteModels.jl

@@ -251,13 +251,21 @@ function orient_euler(s::AKM)
     SVector(roll, pitch, yaw)
 end
 
-function calc_orient_quat(s::AKM)
-    x, y, z = kite_ref_frame(s)
-    # reference: NED
-    ax = [0, 1, 0]
-    ay = [1, 0, 0]
-    az = [0, 0, -1]
-    rotation = rot3d(ax, ay, az, x, y, z)
+function calc_orient_quat(s::AKM; old=false)
+    if old
+        x, _, z = kite_ref_frame(s)
+        pos_kite_ = pos_kite(s)
+        pos_before = pos_kite_ .+ z
+
+        rotation = rot(pos_kite_, pos_before, -x)
+    else
+        x, y, z = kite_ref_frame(s)
+        # reference: NED
+        ax = [0, -1, 0]
+        ay = [-1, 0, 0]
+        az = [0, 0, -1]
+        rotation = rot3d(ax, ay, az, x, y, z)
+    end
     q = QuatRotation(rotation)
     return Rotations.params(q)
 end

test/test_kps3.jl

@@ -286,7 +286,7 @@ end
     @test all(pos_kite(kps) .≈ [134.97402018366216, 0.0, 366.8418273480761])
     @test calc_elevation(kps) .≈ 1.2182337959242815 # 69.8 deg
     @test calc_azimuth(kps) ≈ 0
-    @test_broken calc_heading(kps) ≈ 0
+    @test calc_heading(kps) ≈ 0
     calc_course(kps) # the course for vel_kite=zero is undefined, so we cannot test it
 end
 

test/test_kps4.jl

@@ -457,7 +457,7 @@ end
     @test all(pos_kite(kps4) .≈ [78.56500000036361, 0.0, 136.07857169877312])
     @test calc_elevation(kps4) ≈ 1.0471975512013534 # 60 degrees
     @test calc_azimuth(kps4) ≈ 0
-    @test_broken calc_heading(kps4) ≈ 0
+    @test calc_heading(kps4) ≈ 0
     calc_course(kps4) # the course for vel_kite = zero is undefined
 end