Now working with 7% wind turbulence

Co-authored-by: Copilot <copilot@github.com>

Author: ufechner7

examples/parking_4p.jl

@@ -39,22 +39,30 @@ dt::Float64 = 1/set.sample_freq
 
 MIN_DEPOWER, DISTURBANCE = if KiteUtils.PROJECT == "system.yaml"
     # result of tuning
-    pcs.kp_tr=0.07
-    pcs.ki_tr=0.0012
-    pcs.kp = 15
-    pcs.ki = 0
-    pcs.c1 = 0.149
-    pcs.c2 = 0 # has no big effect, can also be set to zero c1 = 0.149 c2 = 5.428
+    pcs.kp_tr=0.04
+    pcs.ki_tr=0.0008
+    pcs.kp = 8.0
+    pcs.ki = 0.2
+    pcs.c1 = 0.048
+    pcs.c2 = 0 # has no big effect, can also be set to zero
+    pcs.max_turn_rate_set = 0.45
+    pcs.max_turn_rate_cmd = 0.75
+    pcs.max_steering = 0.45
+    pcs.max_steering_rate = 1.0
     0.22, 0.1
-else 
+else
     # result of tuning
     println("not system.yaml")
-    pcs.kp_tr=0.05
-    pcs.ki_tr=0.0024
-    pcs.kp = 30
-    pcs.ki = 1.0
+    pcs.kp_tr=0.035
+    pcs.ki_tr=0.0015
+    pcs.kp = 10.0
+    pcs.ki = 0.3
     pcs.c1 = 0.048
     pcs.c2 = 0    # has no big effect, can also be set to zero
+    pcs.max_turn_rate_set = 0.45
+    pcs.max_turn_rate_cmd = 0.75
+    pcs.max_steering = 0.45
+    pcs.max_steering_rate = 1.0
     0.4, 0.4
 end
 @info "pcs.kp_tr=$(pcs.kp_tr), pcs.ki_tr=$(pcs.ki_tr), pcs.kp=$(pcs.kp), pcs.ki=$(pcs.ki), MIN_DEPOWER=$(MIN_DEPOWER)"

examples/parking_controller.jl

@@ -19,13 +19,18 @@ import KiteUtils: wrap2pi
     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
+    max_turn_rate_set::Float64 = 100.0 # clamp outer-loop desired turn-rate [rad/s]
+    max_turn_rate_cmd::Float64 = 100.0 # clamp inner-loop commanded turn-rate [rad/s]
+    max_steering::Float64 = 100.0      # clamp final steering command to physical range
+    max_steering_rate::Float64 = 0.0   # optional rate limit [1/s], 0 disables limiting
 end
 
 mutable struct ParkingController
     pcs::ParkingControllerSettings
     pid_tr::DiscretePID
     pid_outer::DiscretePID
     last_heading::Float64
+    last_steering::Float64
     chi_set::Float64
     last_ndi_gain::Float64
 end
@@ -34,7 +39,7 @@ 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, 0)
+    return ParkingController(pcs, pid_tr, pid_outer, last_heading, 0.0, 0.0, 0.0)
 end
 
 """
@@ -104,12 +109,22 @@ function calc_steering(pc::ParkingController, heading, chi_set; elevation=0.0, v
     # 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
+    psi_dot_set = clamp(psi_dot_set, -pc.pcs.max_turn_rate_set, pc.pcs.max_turn_rate_set)
+    # Use shortest-angle difference to avoid artificial spikes at wrap boundaries.
+    dpsi = atan(sin(heading - pc.last_heading), cos(heading - pc.last_heading))
+    psi_dot = dpsi / pc.pcs.dt
     pc.last_heading = heading
     psi_dot_in = pc.pid_tr(psi_dot_set, psi_dot)
+    psi_dot_in = clamp(psi_dot_in, -pc.pcs.max_turn_rate_cmd, pc.pcs.max_turn_rate_cmd)
     # linearize the NDI block
     u_s, ndi_gain = linearize(pc, psi_dot_in, heading, elevation, v_app; ud_prime)
-    u_s, ndi_gain, psi_dot, psi_dot_set
+    u_cmd = clamp(u_s, -pc.pcs.max_steering, pc.pcs.max_steering)
+    if pc.pcs.max_steering_rate > 0.0
+        max_delta = pc.pcs.max_steering_rate * pc.pcs.dt
+        u_cmd = clamp(u_cmd, pc.last_steering - max_delta, pc.last_steering + max_delta)
+    end
+    pc.last_steering = u_cmd
+    u_cmd, ndi_gain, psi_dot, psi_dot_set
 end
 
 function test_linearize()

examples/parking_wind_dir.jl

@@ -47,22 +47,30 @@ dt::Float64 = wcs.dt
 
 MIN_DEPOWER = if KiteUtils.PROJECT == "system.yaml"
     # result of tuning
-    pcs.kp_tr=0.06
-    pcs.ki_tr=0.0012
-    pcs.kp = 15
-    pcs.ki = 0.5
+    pcs.kp_tr=0.04
+    pcs.ki_tr=0.0008
+    pcs.kp = 8.0
+    pcs.ki = 0.2
     pcs.c1 = 0.048
     pcs.c2 = 0 # has no big effect, can also be set to zero
+    pcs.max_turn_rate_set = 0.45
+    pcs.max_turn_rate_cmd = 0.75
+    pcs.max_steering = 0.45
+    pcs.max_steering_rate = 1.0
     0.22
 else
     # result of tuning
     println("not system.yaml")
-    pcs.kp_tr=0.05
-    pcs.ki_tr=0.0024
-    pcs.kp = 30
-    pcs.ki = 1.0
+    pcs.kp_tr=0.035
+    pcs.ki_tr=0.0015
+    pcs.kp = 10.0
+    pcs.ki = 0.3
     pcs.c1 = 0.048
     pcs.c2 = 0    # has no big effect, can also be set to zero
+    pcs.max_turn_rate_set = 0.45
+    pcs.max_turn_rate_cmd = 0.75
+    pcs.max_steering = 0.45
+    pcs.max_steering_rate = 1.0
     0.4
 end
 pc = pcm.ParkingController(pcs)
@@ -201,10 +209,10 @@ end
 play_parking()
 stop(viewer)
 p=plotx(T, rad2deg.(AZIMUTH), rad2deg.(AZIMUTH_EAST),[rad2deg.(UPWIND_DIR_), rad2deg.(AV_UPWIND_DIR)],
-         rad2deg.(HEADING), [100*(SET_STEERING), 100*(STEERING)]; 
+         rad2deg.(HEADING), [100*(SET_STEERING), 100*(STEERING)], V_WIND_KITE; 
          xlabel="Time [s]", 
-         ylabels=["Azimuth [°]", "azimuth_east [°]", "upwind_dir [°]", "Heading [°]", "Steering [%]"],
-         labels=["azimuth", "azimuth_east", ["upwind_dir", "filtered_upwind_dir"], "heading", ["set_steering", "steering"]])
+         ylabels=["Azimuth [°]", "azimuth_east [°]", "upwind_dir [°]", "Heading [°]", "Steering [%]", "v_wind_kite [m/s]"],
+         labels=["azimuth", "azimuth_east", ["upwind_dir", "filtered_upwind_dir"], "heading", ["set_steering", "steering"], "v_wind_kite"])
 display(p)
 reactivate_host_app()