improved test case

test/test_flightpathcontroller3.jl

@@ -4,6 +4,7 @@ if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
     using TestEnv; TestEnv.activate()
 end
 using KiteControllers, Timers, ControlPlots; tic()
+if false; include("../src/flightpathcontroller.jl"); end
 
 # Test the flight path controller against the real 4point kite
 # 1. park
@@ -12,20 +13,21 @@ using KiteControllers, Timers, ControlPlots; tic()
 using KiteUtils
 using KiteControllers, KiteModels, KiteViewers, ControlPlots
 
-kcu::KCU   = KCU(se())
+set = deepcopy(se())
+kcu::KCU   = KCU(set)
 kps4::KPS4 = KPS4(kcu)
-wcs::WCSettings   = WCSettings();  wcs.dt = 1/se().sample_freq
+wcs::WCSettings   = WCSettings();  wcs.dt = 1/set.sample_freq
 fcs::FPCSettings  = FPCSettings(); fcs.dt = wcs.dt
 fpps::FPPSettings = FPPSettings()
-# ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps)
 dt::Float64 = wcs.dt
+attractor=[55.73, 56.95]
 
 fpc = FlightPathController(fcs)
 fpca = FlightPathCalculator(fpc, fpps)
 kite = KiteModel(fcs)
 kite.omega = 0.08
 u_d = 0.2
-on_control_command(fpc, psi_dot_set=51.566)
+on_control_command(fpc; attractor=deg2rad.(attractor))
 
 
 # the following values can be changed to match your interest
@@ -49,14 +51,18 @@ function simulate(integrator)
                 steering = 0.0
             # then steer towards P1
             else
+                if time == 20.0
+                    println("on_control_command...")
+                    on_control_command(fpc; attractor=deg2rad.(attractor), intermediate=false)
+                end
                 phi  = sys_state.azimuth
                 v_a = sys_state.v_app
                 chi = sys_state.course
                 u_d = sys_state.depower
                 beta = sys_state.elevation
                 KiteControllers.set_azimuth_elevation(fpca, phi, beta)
                 omega = fpca._omega
-                # on_est_sysstate(fpc::FlightPathController, phi, beta, psi, chi, omega, va; u_d=nothing, u_d_prime=nothing)
+                # println("omega: $omega")
                 on_est_sysstate(fpc, phi, kite.beta, kite.psi, chi, omega, v_a; u_d=u_d)
                 steering = calc_steering(fpc, false)
             end
@@ -65,7 +71,6 @@ function simulate(integrator)
         v_ro = 0.0
         KiteModels.next_step!(kps4, integrator, v_ro=v_ro, dt=dt)
         sys_state = SysState(kps4)
-        # on_new_systate(ssc, sys_state)
         KiteViewers.update_system(viewer, sys_state; scale = 0.04/1.1, kite_scale=6.6)
         sleep(dt/4)
         if i*dt >= MAX_TIME break end
@@ -77,7 +82,7 @@ end
 function test_parking(suppress_overshoot_factor = 3.0)
     global LAST_RES
     clear!(kps4)
-    init_kcu(kcu, se())
+    init_kcu(kcu, set)
     integrator = KiteModels.init_sim!(kps4, stiffness_factor=0.04)
     simulate(integrator)
 end