Dual-loop parking controller

.gitignore

@@ -16,3 +16,4 @@ examples_3d/Manifest.toml
 bin/kps-image-1.10-fix_yaw.so
 Manifest-v1.11.toml
 examples_3d/Manifest.toml
+bin/kps-image-1.10-parking_controller.so

CHANGELOG.md

@@ -1,4 +1,14 @@
 # Changelog
+### Unreleased
+#### Changed
+- use the latest version of KiteModels, which defines the azimuth angle and the orientation differently and make the controllers and examples work with the new definitions
+- the constructor `SystemStateControl()`now needs the additional parameter `v_wind`
+- the constructors `WCSettings()`, `FPCSettings()` and `FPPSettings()` now have the new argument `update`. If true, then the settings are loaded from the corresponding `yaml` file.
+
+#### Added
+- add a script that tests the parking controller when the wind direction is changing
+- add the script `test/menu.jl` which allows to execute the manual tests, that display plots and fix the tests
+
 
 ### 0.2.10 - 2024-09-07
 #### Changed

Project.toml

@@ -4,6 +4,7 @@ authors = ["Uwe Fechner <[email protected]> and contributors"]
 version = "0.2.10"
 
 [deps]
+DiscretePIDs = "c1363496-6848-4723-8758-079b737f6baf"
 JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
 KiteModels = "b94af626-7959-4878-9336-2adc27959007"
 KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533"

examples/parking_4p.jl

@@ -6,10 +6,13 @@ end
 using Timers; tic()
 
 using KiteControllers, KiteViewers, KiteModels, ControlPlots, Rotations
-set = deepcopy(load_settings("system.yaml"))
+set = deepcopy(load_settings("system_v9.yaml"))
 set.abs_tol=0.00006
 set.rel_tol=0.0001
-set.v_wind = 8.5 # v_min1 7.7; v_min2 8.5
+set.v_wind = 12.0 # v_min1 6-25; v_min2 5.5-30
+
+include("parking_controller.jl")
+pcs = ParkingControllerSettings(dt=0.05)
 
 kcu::KCU = KCU(set)
 kps4::KPS4 = KPS4(kcu)
@@ -24,28 +27,31 @@ dt::Float64 = wcs.dt
 
 if KiteUtils.PROJECT == "system.yaml"
     # result of tuning
-    fcs.p=1.2
-    fcs.i=0.04
-    fcs.d=13.25*0.95
+    pcs.kp_tr=0.06
+    pcs.ki_tr=0.0012
+    pcs.kp = 15
+    pcs.ki = 0.5
     MIN_DEPOWER       = 0.22
-    fcs.use_chi = false
-    @assert fcs.gain == 0.04
+    DISTURBANCE      = 0.1
+    pcs.c1 = 0.048
+    pcs.c2 = 0 # has no big effect, can also be set to zero
 else
     # result of tuning
     println("not system.yaml")
-    fcs.p=1.05
-    fcs.i=0.012
-    fcs.d=13.25*2.0
+    pcs.kp_tr=0.05
+    pcs.ki_tr=0.0024
+    pcs.kp = 30
+    pcs.ki = 1.0
     MIN_DEPOWER       = 0.4
-    fcs.use_chi = false
-    fcs.gain = 0.04
-    fcs.c1 = 0.048
-    fcs.c2 = 5.5
+    DISTURBANCE      = 0.4
+    pcs.c1 = 0.048
+    pcs.c2 = 0    # has no big effect, can also be set to zero
 end
-println("fcs.p=$(fcs.p), fcs.i=$(fcs.i), fcs.d=$(fcs.d), fcs.gain=$(fcs.gain)")
+println("pcs.kp_tr=$(pcs.kp_tr), pcs.ki_tr=$(pcs.ki_tr), pcs.kp=$(pcs.kp), pcs.ki=$(pcs.ki), MIN_DEPOWER=$(MIN_DEPOWER)")
+pc = ParkingController(pcs)
 
 # the following values can be changed to match your interest
-MAX_TIME::Float64 = 60 # was 60
+MAX_TIME::Float64 = 120 # was 60
 TIME_LAPSE_RATIO  =  6
 SHOW_KITE         = true
 # end of user parameter section #
@@ -59,6 +65,10 @@ HEADING::Vector{Float64}       = zeros(Int64(MAX_TIME/dt))
 SET_STEERING::Vector{Float64}  = zeros(Int64(MAX_TIME/dt))
 STEERING::Vector{Float64}      = zeros(Int64(MAX_TIME/dt))
 AoA::Vector{Float64}           = zeros(Int64(MAX_TIME/dt))
+PSI_DOT::Vector{Float64}       = zeros(Int64(MAX_TIME/dt))
+PSI_DOT_SET::Vector{Float64}   = zeros(Int64(MAX_TIME/dt))
+NDI_GAIN::Vector{Float64}      = zeros(Int64(MAX_TIME/dt))
+V_APP::Vector{Float64}         = zeros(Int64(MAX_TIME/dt))
 
 function simulate(integrator)
     global sys_state
@@ -72,13 +82,23 @@ function simulate(integrator)
     on_new_systate(ssc, sys_state)
     while true
         steering = 0.0
-        if i > 100
+        if i >= 100
             heading = calc_heading(kps4; neg_azimuth=true, one_point=false)
-            steering = calc_steering(ssc, 0; heading)
+            if i == 100
+                pc.last_heading = heading
+            end
+            elevation = sys_state.elevation
+            # println("heading: $(rad2deg(heading)), elevation: $(rad2deg(elevation))")
+            chi_set = -navigate(pc, sys_state.azimuth, elevation)
+            steering, ndi_gain, psi_dot, psi_dot_set = calc_steering(pc, heading, chi_set; elevation, v_app = sys_state.v_app)
+            PSI_DOT[i] = psi_dot
+            PSI_DOT_SET[i] = psi_dot_set
+            NDI_GAIN[i] = ndi_gain
+            V_APP[i] = sys_state.v_app
             time = i * dt
             # disturbance
             if time > 20 && time < 21
-                steering = 0.1
+                steering = DISTURBANCE
             end            
             set_depower_steering(kps4.kcu, MIN_DEPOWER, steering)
         end
@@ -158,9 +178,10 @@ on(viewer.btn_PARKING.clicks) do c; parking(); end
 
 play()
 stop(viewer)
-p = plotx(T, rad2deg.(AZIMUTH), rad2deg.(HEADING), [100*(SET_STEERING), 100*(STEERING)], AoA; 
+p = plotx(T, rad2deg.(AZIMUTH), rad2deg.(HEADING), [100*(SET_STEERING), 100*(STEERING)],
+             [rad2deg.(PSI_DOT), rad2deg.(PSI_DOT_SET)], NDI_GAIN, V_APP; 
           xlabel="Time [s]", 
-          ylabels=["Azimuth [°]", "Heading [°]", "steering [%]", "AoA [°]"],
-          labels=["azimuth", "heading", ["set_steering", "steering", "AoA"]], 
-          fig="Azimuth, heading, steering and AoA",)
+          ylabels=["Azimuth [°]", "Heading [°]", "steering [%]", "psi_dot [°/s]", "NDI_GAIN", "v_app [m/s]"],   
+          labels=["azimuth", "heading", ["set_steering", "steering"], ["psi_dot", "psi_dot_set"], "NDI_GAIN", "v_app"],  
+          fig="Azimuth, heading, steering and more",)
 display(p)

examples/parking_controller.jl

@@ -0,0 +1,184 @@
+# Prototype of a parking controller.
+# Components: PID controller, NDI block, and great circle navigation
+using DiscretePIDs, Parameters, Test, ControlPlots
+import KiteControllers: calc_steering, wrap2pi, navigate
+
+@with_kw mutable struct ParkingControllerSettings @deftype Float64
+    dt
+    # turn rate controller settings
+    kp_tr=0.06
+    ki_tr=0.0012
+    # outer controller (heading/ course) settings
+    kp=15
+    ki=0.5
+    # NDI block settings
+    va_min = 5.0   # minimum apparent wind speed
+    va_max = 100.0 # maximum apparent wind speed
+    k_ds = 2.0 # influence of the depower settings on the steering sensitivity
+    c1 = 0.048 # v9 kite model
+    c2 = 0     # a value other than zero creates more problems than it solves
+    last_ndi_gain = 0.0
+end
+
+mutable struct ParkingController
+    pcs::ParkingControllerSettings
+    pid_tr::DiscretePID
+    pid_outer::DiscretePID
+    last_heading::Float64
+    chi_set::Float64
+end
+
+function ParkingController(pcs::ParkingControllerSettings; last_heading = 0.0)
+    Ts = pcs.dt
+    pid_tr    = DiscretePID(;K=pcs.kp_tr, Ti=pcs.kp_tr/ pcs.ki_tr, Ts)
+    pid_outer = DiscretePID(;K=pcs.kp,    Ti=pcs.kp/ pcs.ki,       Ts)
+    return ParkingController(pcs, pid_tr, pid_outer, last_heading, 0)
+end
+
+"""
+    linearize(pcs::ParkingControllerSettings, psi_dot, psi, elevation, v_app; ud_prime=0)
+
+Nonlinear, dynamic inversion block (NDI) according to Eq. 6.4 and Eq. 6.12.
+
+Parameters:
+- psi_dot: desired turn rate in radians per second
+- psi: heading in radians
+- elevation: elevation angle in radians
+- v_app: apparent wind speed in m/s
+- ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered
+"""
+function linearize(pcs::ParkingControllerSettings, psi_dot, psi, elevation, v_app; ud_prime=0)
+    # Eq. 6.13: calculate va_hat
+    va_hat = clamp(v_app, pcs.va_min, pcs.va_max)
+    # Eq. 6.12: calculate the steering from the desired turn rate
+    u_s = (1.0 + pcs.k_ds * ud_prime) / (pcs.c1 * va_hat) * (psi_dot - pcs.c2 / va_hat * sin(psi) * cos(elevation))
+    if abs(psi_dot) < 1e-6
+        ndi_gain = pcs.last_ndi_gain
+    else
+        ndi_gain = clamp(u_s / psi_dot, -20, 20.0)
+    end
+    pcs.last_ndi_gain = ndi_gain
+    return u_s, ndi_gain
+end
+
+"""
+    navigate(fpc, limit=50.0)
+
+Calculate the desired flight direction chi_set using great circle navigation.
+Limit delta_beta to the value of the parameter limit (in degrees).
+"""
+function navigate(pc::ParkingController, azimuth, elevation; limit=50.0)
+    phi_set  = 0.0         # azimuth
+    beta_set = deg2rad(77) # zenith
+    beta = elevation
+    phi = azimuth
+    # println("beta: $(rad2deg(beta)), phi: $(rad2deg(phi))")
+    r_limit = deg2rad(limit)
+    if beta_set - beta > r_limit
+        beta_set = beta + r_limit
+    elseif beta_set - beta < -r_limit
+        beta_set = beta - r_limit
+    end
+    y = sin(phi_set - phi) * cos(beta_set)
+    x = cos(beta) * sin(beta_set) - sin(beta) * cos(beta_set) * cos(phi_set - phi)
+    pc.chi_set = atan(-y, x)
+end
+
+"""
+    calc_steering(pc::ParkingController, heading; elevation=0.0, v_app=10.0, ud_prime=0.0)
+
+Calculate rel_steering and ndi_gain from the actual heading, elevation, and apparent wind speed.
+
+Parameters:
+- pc: parking controller
+- heading: actual heading in radians
+- elevation: elevation angle in radians
+- v_app: apparent wind speed in m/s
+- ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered
+
+"""
+function calc_steering(pc::ParkingController, heading, chi_set; elevation=0.0, v_app=10.0, ud_prime=0.0)
+    # calculate the desired turn rate
+    heading = wrap2pi(heading) # a different wrap2pi function is needed that avoids any jumps
+    psi_dot_set = pc.pid_outer(wrap2pi(chi_set), heading)
+    psi_dot = (wrap2pi(heading - pc.last_heading)) / pc.pcs.dt
+    pc.last_heading = heading
+    psi_dot_in = pc.pid_tr(psi_dot_set, psi_dot)
+    # linearize the NDI block
+    u_s, ndi_gain = linearize(pc.pcs, psi_dot_in, heading, elevation, v_app; ud_prime)
+    u_s, ndi_gain, psi_dot, psi_dot_set
+end
+
+function test_linearize()
+    @testset "test_linearize" begin
+        # set the parameters of the parking controller
+        pcs = ParkingControllerSettings(dt=0.05)
+        pcs.kp_tr=1.05
+        pcs.ki_tr=0.012
+        # create the parking controller
+        pc = ParkingController(pcs)
+        # set the desired turn rate
+        psi_dot = 0.1
+        # set the heading
+        psi = 0.0
+        # set the elevation angle
+        elevation = 0.0
+        # set the apparent wind speed
+        v_app = 10.0
+        # set the depower setting
+        ud_prime = 0.5
+        # linearize the NDI block
+        u_s, ndi_gain = linearize(pc.pcs, psi_dot, psi, elevation, v_app; ud_prime)
+        @test u_s ≈ 0.41666666666666674
+        @test ndi_gain ≈ 4.166666666666667
+    end
+    nothing
+end
+
+function test_calc_steering()
+    @testset "test_calc_steering" begin
+        # set the parameters of the parking controller
+        pcs = ParkingControllerSettings(dt=0.05)
+        pcs.kp_tr=1.05
+        pcs.ki_tr=0.012
+        # create the parking controller
+        pc = ParkingController(pcs)
+        # set the heading
+        heading = deg2rad(1.0)
+        elevation = deg2rad(70.0)
+        chi_set = deg2rad(34.0)
+        u_s, ndi_gain, psi_dot, psi_dot_set = calc_steering(pc, heading, chi_set; elevation)
+        @test u_s ≈ 18.135061759003584
+        @test ndi_gain ≈ 2.0833333333333335
+        @test psi_dot ≈ 0.3490658503988659
+        @test psi_dot_set ≈ 8.639379797371932
+    end
+    nothing
+end
+
+function test_navigate()
+    @testset "test_navigate" begin
+        # set the parameters of the parking controller
+        pcs = ParkingControllerSettings(dt=0.05)
+        pcs.kp=1.05
+        pcs.ki=0.012
+        # create the parking controller
+        pc = ParkingController(pcs)
+        # set the azimuth
+        azimuth = deg2rad(90.0)
+        # set the elevation
+        elevation = deg2rad(70.0)
+        chi_set = navigate(pc, azimuth, elevation)
+        @test chi_set ≈ deg2rad(34.019878734151234)
+    end
+    nothing
+end
+
+function test_parking_controller()
+    @testset verbose=true "test_parking_controller" begin
+        test_calc_steering()
+        test_linearize()
+        test_navigate()
+    end
+    nothing
+end