From ecb94ad4785c63d3f29fe8b02fe3b856f46f7169 Mon Sep 17 00:00:00 2001 From: Tomaso Maria Luigi De Ponti <48210579+tmldeponti@users.noreply.github.com> Date: Tue, 16 Jul 2024 13:28:31 +0200 Subject: [PATCH] ONELOOP controller updates (#3333) --- .../tudelft/bebop_one_indi_generated_ekf2.xml | 9 +- .../tudelft/rot_wing_v3c_oneloop.xml | 189 ++- .../rot_wing_v3c_oneloop_optitrack.xml | 338 ++++ ...ot_wing_v3c_oneloop_optitrack_ext_pose.xml | 179 +- .../rot_wing_v3c_oneloop_simulation.xml | 312 ++++ .../rotorcraft_oneloop_andi_indi.xml | 236 ++- conf/autopilot/rotorcraft_oneloop_switch.xml | 258 +++ .../flight_plans/tudelft/oneloop_cyberzoo.xml | 2 + .../tudelft/oneloop_valkenburg.xml | 210 ++- conf/modules/oneloop_andi.xml | 6 +- .../jsbsim/aircraft/rotwingv3c_SI.xml | 822 +++++++-- conf/userconf/tudelft/RW_control_panel.xml | 1503 +++++++++++++++++ conf/userconf/tudelft/conf.xml | 58 +- data/pictures/gcs_icons/transition_left.png | Bin 0 -> 47394 bytes data/pictures/gcs_icons/transition_right.png | Bin 0 -> 85111 bytes .../rotorcraft/guidance/guidance_oneloop.c | 13 +- .../rotorcraft/oneloop/oneloop_andi.c | 790 ++++++--- .../rotorcraft/oneloop/oneloop_andi.h | 8 +- 18 files changed, 4204 insertions(+), 729 deletions(-) create mode 100644 conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml create mode 100644 conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml create mode 100644 conf/autopilot/rotorcraft_oneloop_switch.xml create mode 100644 conf/userconf/tudelft/RW_control_panel.xml create mode 100644 data/pictures/gcs_icons/transition_left.png create mode 100644 data/pictures/gcs_icons/transition_right.png diff --git a/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml b/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml index 8114035179..2cb4e949a7 100644 --- a/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml +++ b/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml @@ -20,8 +20,8 @@ - - + + @@ -33,9 +33,10 @@ - + + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml index f6e5f0a414..6576ba46d6 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml @@ -7,10 +7,10 @@ --> - RotatingWingV3C for outdoor flight and simulation with INS EKF2 + RotatingWingV3C for outdoor flight with INS EKF2 - + @@ -28,18 +28,23 @@ - + - - - + + + + + + + + - - + + @@ -55,8 +60,8 @@ - - + + @@ -88,7 +93,7 @@ - + @@ -107,36 +112,35 @@ - + - + + - - - + + - + - + + + + @@ -155,8 +159,8 @@ - - + + @@ -176,42 +180,47 @@ - - - - + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + - - - - + + + +
@@ -223,8 +232,9 @@ + - + @@ -233,37 +243,40 @@ - + + +
+
- +
- + - - - + + + +
- - - - - - - + @@ -272,18 +285,30 @@ - - - - - - - - - - - - + + + + + + + + + + +
+ +
+ + + + + + + +
@@ -291,7 +316,7 @@ - +
@@ -304,11 +329,9 @@
- - - +
diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml new file mode 100644 index 0000000000..d3407db036 --- /dev/null +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml @@ -0,0 +1,338 @@ + + + + RotatingWingV3C with optitrack and INS EKF2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + +
+
diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml index 859d9ea913..c44df9c52b 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml @@ -10,7 +10,7 @@ RotatingWingV3C with optitrack and INS ext pose - + @@ -28,7 +28,7 @@ - + @@ -38,8 +38,8 @@ - - + + @@ -55,8 +55,8 @@ - - + + @@ -109,32 +109,31 @@ - + + - + - - - + + - + - + + + + @@ -153,7 +152,7 @@ - + @@ -174,42 +173,47 @@ - - - - + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + - - - - + + + +
@@ -221,8 +225,9 @@ + - + @@ -231,23 +236,24 @@ - + +
- +
- + @@ -255,15 +261,9 @@
- - - - - - - + @@ -272,21 +272,52 @@ - - - - - - - - - - - - - + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
@@ -305,11 +336,9 @@
- - + - - +
diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml new file mode 100644 index 0000000000..8e92a1a627 --- /dev/null +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml @@ -0,0 +1,312 @@ + + + + RotatingWingV3C for simulation + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + +
+ +
+ + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + +
+
diff --git a/conf/autopilot/rotorcraft_oneloop_andi_indi.xml b/conf/autopilot/rotorcraft_oneloop_andi_indi.xml index b60ae736da..cc1080a8a3 100644 --- a/conf/autopilot/rotorcraft_oneloop_andi_indi.xml +++ b/conf/autopilot/rotorcraft_oneloop_andi_indi.xml @@ -32,24 +32,22 @@ - + - - - + + + - - - - + + + + - - - - + + @@ -57,6 +55,60 @@ + + + @@ -66,47 +118,35 @@ - - + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + - + - - - - - - - - - - - - - - - - - + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/tudelft/oneloop_cyberzoo.xml b/conf/flight_plans/tudelft/oneloop_cyberzoo.xml index 29e4e9f5d1..51faba79da 100644 --- a/conf/flight_plans/tudelft/oneloop_cyberzoo.xml +++ b/conf/flight_plans/tudelft/oneloop_cyberzoo.xml @@ -2,10 +2,12 @@
+ #include "autopilot.h" #include "modules/datalink/datalink.h" #include "modules/energy/electrical.h" #include "modules/radio_control/radio_control.h" #include "modules/ahrs/ahrs.h" + #include "modules/gps/gps.h"
diff --git a/conf/flight_plans/tudelft/oneloop_valkenburg.xml b/conf/flight_plans/tudelft/oneloop_valkenburg.xml index 65ce2e0a34..194c95e9eb 100644 --- a/conf/flight_plans/tudelft/oneloop_valkenburg.xml +++ b/conf/flight_plans/tudelft/oneloop_valkenburg.xml @@ -1,38 +1,45 @@ - +
#include "autopilot.h" #include "modules/datalink/datalink.h" #include "modules/energy/electrical.h" #include "modules/radio_control/radio_control.h" #include "modules/ahrs/ahrs.h" + #include "modules/gps/gps.h"
- - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -54,18 +61,17 @@ - - - - + - - = IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="Landed"/> @@ -92,69 +98,139 @@ + - + + - + + + - - - + + + - - + + + + + + + + + + + - - - + + + + - - + + + + - - + + + + - - - + + + + + - - - - + + + + + + + + - - - + + + + - - - - - - + + + + - - - - - + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/oneloop_andi.xml b/conf/modules/oneloop_andi.xml index c69da1a44c..5ba8d08d38 100644 --- a/conf/modules/oneloop_andi.xml +++ b/conf/modules/oneloop_andi.xml @@ -6,6 +6,10 @@ + + + + @@ -14,7 +18,7 @@ - + diff --git a/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml b/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml index 90ba41f02c..a58f7e8e90 100644 --- a/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml +++ b/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml @@ -35,13 +35,13 @@ - 0.126323 - 1.064109 - 1.00000 + 0.129 + 0.950 + 0.975 0.0 0.0 0.0 - 7.97 + 6.670 0 0 @@ -119,89 +119,339 @@ - - fcs/front_motor + + fcs/MOTOR_FRONT 10.1 - fcs/front_motor_lag + fcs/MOTOR_FRONT_lag - - fcs/right_motor + + fcs/MOTOR_RIGHT 10.1 - fcs/right_motor_lag + fcs/MOTOR_RIGHT_lag - - fcs/back_motor + + fcs/MOTOR_BACK 10.1 - fcs/back_motor_lag + fcs/MOTOR_BACK_lag - - fcs/left_motor + + fcs/MOTOR_LEFT 10.1 - fcs/left_motor_lag + fcs/MOTOR_LEFT_lag - - fcs/pusher + + fcs/MOTOR_PUSHER 24.07 - fcs/pusher_lag + fcs/MOTOR_PUSHER_lag + + + fcs/ELEVATOR + 50.00 + fcs/ELEVATOR_lag + + + fcs/RUDDER + 50.00 + fcs/RUDDER_lag + + + fcs/AILERONS + 50.00 + fcs/AILERONS_lag + + + fcs/FLAPS + 50.00 + fcs/FLAPS_lag - - - fcs/front_motor + + fcs/MOTOR_FRONT 10.1 - fcs/front_motor_d + fcs/MOTOR_FRONT_d - - fcs/right_motor + + fcs/MOTOR_RIGHT 10.1 - fcs/right_motor_d + fcs/MOTOR_RIGHT_d - - fcs/back_motor + + fcs/MOTOR_BACK 10.1 - fcs/back_motor_d + fcs/MOTOR_BACK_d - - fcs/left_motor + + fcs/MOTOR_LEFT 10.1 - fcs/left_motor_d + fcs/MOTOR_LEFT_d + + + + + + + fcs/ROT_MECH + 0.5 + + + + + fcs/cosskew + + + + + +

fcs/cosskew

+ 2.0 +
+
+ fcs/cos2skew +
+ + + + +

fcs/cosskew

+ 3.0 +
+
+ fcs/cos3skew +
+ + + + + + fcs/ROT_MECH + 0.5 + + + + + fcs/sinskew + + + + + +

fcs/sinskew

+ 2.0 +
+
+ fcs/sin2skew +
+ + + + +

fcs/sinskew

+ 3.0 +
+
+ fcs/sin3skew +
+ + + + + fcs/cosskew + + fcs/cos3skew + -1 + + + + fcs/coscos3skew + +
+ + + + + + aero/qbar-psf + 47.880259 + 1.6327 + + + fcs/airspeed2_ms + + + + + + fcs/airspeed2_ms + + + fcs/airspeed_ms + + + + + + + + fcs/airspeed_ms + + 0.0 0.0 + 8.8 0.0 + 16.0 -0.8333 + 25.0 -0.8333 + +
+ fcs/ELEVATOR_lag +
+
+ fcs/de_lag +
+ + + + 0.129 + + fcs/Ixx_quad + + + + + 0.950 + + fcs/Iyy_quad + + + + + 0.0478 + + fcs/Ixx_body + + + + + 0.7546 + + fcs/Iyy_body + + + + + 0.08099 + + fcs/Ixx_wing + + + + + 0.1949 + + fcs/Iyy_wing + + + + + + fcs/Ixx_body + + fcs/Ixx_wing + fcs/cos2skew + + + fcs/Iyy_wing + fcs/sin2skew + + + + fcs/Ixx_sched + + + + + + fcs/Iyy_body + + fcs/Ixx_wing + fcs/sin2skew + + + fcs/Iyy_wing + fcs/cos2skew + + + + fcs/Iyy_sched + + + + + + fcs/Ixx_quad + fcs/Ixx_sched + + + fcs/Ixx_ratio_quad_sched + + + + + + fcs/Iyy_quad + fcs/Iyy_sched + + + fcs/Iyy_ratio_quad_sched + +
- fcs/front_motor - fcs/front_motor_lag - fcs/front_motor_d - fcs/right_motor - fcs/right_motor_lag - fcs/right_motor_d - fcs/back_motor - fcs/back_motor_lag - fcs/back_motor_d - fcs/left_motor - fcs/left_motor_lag - fcs/left_motor_d - - fcs/pusher - fcs/pusher_lag - + fcs/MOTOR_FRONT + fcs/MOTOR_FRONT_lag + fcs/MOTOR_FRONT_d + fcs/MOTOR_RIGHT + fcs/MOTOR_RIGHT_lag + fcs/MOTOR_RIGHT_d + fcs/MOTOR_BACK + fcs/MOTOR_BACK_lag + fcs/MOTOR_BACK_d + fcs/MOTOR_LEFT + fcs/MOTOR_LEFT_lag + fcs/MOTOR_LEFT_d + fcs/MOTOR_PUSHER + fcs/MOTOR_PUSHER_lag + fcs/ELEVATOR + fcs/RUDDER + fcs/AILERONS + fcs/FLAPS + fcs/ELEVATOR_lag + fcs/RUDDER_lag + fcs/AILERONS_lag + fcs/FLAPS_lag + fcs/ROLL + fcs/PITCH + fcs/YAW + fcs/THRUST + fcs/ROT_MECH + fcs/de_lag + fcs/airspeed2_ms + - - - + + + + - fcs/front_motor_lag - 8.69 + fcs/MOTOR_FRONT_lag + 36.82 + 0.2248 - -0.423 + 0 0 0 @@ -212,34 +462,16 @@ - + - fcs/right_motor_lag - 8.69 + fcs/MOTOR_RIGHT_lag + 36.82 + 0.2248 0 - 0.408 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/back_motor_lag - 8.69 - - - - 0.423 0 0 @@ -250,16 +482,17 @@ - + - fcs/left_motor_lag - 8.69 + fcs/MOTOR_BACK_lag + 36.82 + 0.2248 0 - -0.408 + 0 0 @@ -269,11 +502,32 @@ - + - fcs/pusher_lag - 36.96 + fcs/MOTOR_LEFT_lag + 36.82 + 0.2248 + + + + 0 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/MOTOR_PUSHER_lag + 33.30 + 0.2248 @@ -288,13 +542,301 @@ - - + + - fcs/front_motor_lag - 10.0 - 0.192 + fcs/MOTOR_FRONT_lag + 36.82 + 0.423 + 0.738 + fcs/Iyy_ratio_quad_sched + + + + 0 + 0 + 0 + + + 0 + 1 + 0 + + + + + + + fcs/MOTOR_BACK_lag + 36.82 + 0.423 + 0.738 + fcs/Iyy_ratio_quad_sched + + + + 0 + 0 + 0 + + + 0 + -1 + 0 + + + + + + + fcs/MOTOR_RIGHT_lag + fcs/cosskew + 36.82 + 0.408 + 0.738 + fcs/Ixx_ratio_quad_sched + + + + 0 + 0 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/MOTOR_RIGHT_lag + fcs/sinskew + 36.82 + 0.408 + 0.738 + fcs/Iyy_ratio_quad_sched + + + + 0 + 0 + 0 + + + 0 + 1 + 0 + + + + + + + fcs/MOTOR_LEFT_lag + fcs/cosskew + 36.82 + 0.408 + 0.738 + fcs/Ixx_ratio_quad_sched + + + + 0 + 0 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/MOTOR_LEFT_lag + fcs/sinskew + 36.82 + 0.408 + 0.738 + fcs/Iyy_ratio_quad_sched + + + + 0 + 0 + 0 + + + 0 + -1 + 0 + + + + + + + + fcs/airspeed2_ms + + 0.0 0.0 + 77.44 0.0 + 256 1.0 + +
+ fcs/airspeed2_ms + fcs/de_lag + + + 0.2112 + 0.850 + 0.738 + fcs/Iyy_ratio_quad_sched +
+
+ + 0 + 0 + 0 + + + 0 + 1 + 0 + +
+ + + + + fcs/airspeed2_ms + fcs/RUDDER_lag + 0.01159 + 0.880 + 0.738 + + + + 0 + 0 + 0 + + + 0 + 0 + 1 + + + + + + + fcs/AILERONS_lag + fcs/sin3skew + fcs/airspeed2_ms + 0.03920 + 0.68 + 0.738 + fcs/Ixx_ratio_quad_sched + + + + 0 + 0 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/airspeed2_ms + fcs/FLAPS_lag + fcs/sin3skew + 0.05527 + 0.355 + 0.738 + fcs/Ixx_ratio_quad_sched + + + + 0 + 0 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/airspeed2_ms + fcs/AILERONS_lag + fcs/coscos3skew + 0.03920 + 0.68 + 0.738 + fcs/Iyy_ratio_quad_sched + + + + 0 + 0 + 0 + + + 0 + 1 + 0 + + + + + + + fcs/airspeed2_ms + fcs/FLAPS_lag + fcs/coscos3skew + 0.05527 + 0.355 + 0.738 + fcs/Iyy_ratio_quad_sched + + + + 0 + 0 + 0 + + + 0 + 1 + 0 + + + + + + + + fcs/MOTOR_FRONT_lag + 3.74 + 0.738 @@ -309,12 +851,12 @@ - + - fcs/right_motor_lag - 10.0 - 0.192 + fcs/MOTOR_RIGHT_lag + 3.74 + 0.738 @@ -329,12 +871,12 @@ - + - fcs/front_motor_lag - 10.0 - 0.192 + fcs/MOTOR_FRONT_lag + 3.74 + 0.738 @@ -349,12 +891,12 @@ - + - fcs/right_motor_lag - 10.0 - 0.192 + fcs/MOTOR_LEFT_lag + 3.74 + 0.738 @@ -370,12 +912,12 @@ - + - fcs/front_motor_d - 10.0 - 0.0096 + fcs/MOTOR_FRONT_d + 0.187 + 0.738 @@ -390,12 +932,12 @@ - + - fcs/right_motor_lag - 10.0 - 0.0096 + fcs/MOTOR_RIGHT_lag + 0.187 + 0.738 @@ -410,12 +952,12 @@ - + - fcs/front_motor_lag - 10.0 - 0.0096 + fcs/MOTOR_FRONT_lag + 0.187 + 0.738 @@ -430,12 +972,12 @@ - + - fcs/right_motor_lag - 10.0 - 0.0096 + fcs/MOTOR_RIGHT_lag + 0.187 + 0.738 @@ -460,26 +1002,40 @@ Drag - aero/qbar-psf - 47.9 - 0.0151 + fcs/airspeed2_ms + 0.0576 0.224808943 - - - Side force due to beta - - aero/qbar-psf - metrics/Sw-sqft - aero/beta-rad - -4 - - + + + + + + 0.4887 + aero/alpha-rad + fcs/airspeed2_ms + + + 0.616 + fcs/sin2skew + aero/alpha-rad + fcs/airspeed2_ms + + + 0.120 + fcs/sin2skew + fcs/airspeed2_ms + + + 0.2248 + + + + - \ No newline at end of file diff --git a/conf/userconf/tudelft/RW_control_panel.xml b/conf/userconf/tudelft/RW_control_panel.xml new file mode 100644 index 0000000000..247d5858ca --- /dev/null +++ b/conf/userconf/tudelft/RW_control_panel.xml @@ -0,0 +1,1503 @@ + +
+ + + + + + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 21251420f1..2577541010 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -582,6 +582,50 @@ settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" gui_color="red" /> + + + + diff --git a/data/pictures/gcs_icons/transition_left.png b/data/pictures/gcs_icons/transition_left.png new file mode 100644 index 0000000000000000000000000000000000000000..2a4a57c1a1a50a00b86313f26ed9e4c99052b4b0 GIT binary patch literal 47394 zcmeGENv`bLvMq?cs0cy;(E&l#3q%zD1?f)Gokiw(p0gq_&od|KB)ta$)D{&~QVUd6 zKhz09#(mCx|NV%-M+hqFZ>)3N7o9#i6PsqkhK;%Y&!#T?5C8rD;lKa4fBU!pK@wRT zUjN7YzyJOJ4*vgt`G5c42YC7KR$HL|?f?4!{6GG`{_WrXZ~u>t3%B7co4)wBe>|DL z-u>f&)YH2CN!UHBD6y54@XPvjpD$nK$&WE9vyfi_c$|V6=~aJh^`$6x@3MJej{|7JeBM==Gm( znlM@a>F=*Mf6uTGAvQ-ipVf4;&+zudD_Q>Mt^KRb;S@f}Tx(Na^w4mA{@o$$iT~B7 z{QB$4`$hhBq5AymIe$O)zTh;!e+}hN#~(_vrqnHod3m?-dBSr-|D|d@p%2!m;1U{G*$htC!@sQ z;~SEH-Te1R);h_jtm|KE@Si*VXB_|anfuT4Cls)2x6cKS`@2OPCI3nOhkx(Cp76Kr zKJdRgar|dX!2Z5e{|6)Z*R|wt+pGTlIrqr^x2zA`^Y`-p*T-^u&;OYfJpbzQ_rEgr zV2OVmNi+O0e)^u%225T}(xNw~wb}l1QR%d8(*R4--yhIP*4>LX=FHSjw|mKpGTD2u z)PHY*>YM6zZrk*~y0m{DNd0TU!57hgcY)25Edl@i*RKzL9xL#)jZYi)fha;XQSeW9 zyqfbWcz6FtNfbkA@XE4t<4X4x>Zvo1Z%GW*rWp84{?C0(NJ~KocQQN%_tXU?5Zlv| z?=J|U%?G0GrjHoOAN&QRnpBoT4ECgKuGT!>jzWR% z65%{@v3^&QBegQ)4f0jZO0%x2gMBrYyR@H$>TQM@?({NQ2pF`9(6rU*obAP;!}W@T zoVweQmgmc^&-;Oo)^X5pS?J?sB=Al2u={w(rzDs^sepnHqt9&Akvxo_h1e6t^CZg= z8m6j9VQVQ6YOIl)Y1i6_FgsarI^6O5xrl~%Nk{onj096pi_y(VGA_!nEX1{a7HHvE zQ{}g*!DD+h`2N}7-xT07e`q3eyj}iWOXQ8$bKa#bP&GgVb4IlcLJjh@Eyg_z_ z6>5tc;Q(NfZf9cuuCL>EoD&YTtH=;<2`{M z*TWj9*7QpHOONq4Zh|t~AZkc;D02=q^-JP9Q(`k@p4B$tJ6tp?-qLh9b;|XD^-h9i zw$m9=kccP zl&C~fO(Xa+e@bkyl)sebn?XiM^%I3~)mG7DaerId*S+k9pYAJ^id?)~$UjzY3eLcN z%kNpA*M{*^WhOPqYxt?3zsfcoR}(EmTs9^uu1I$*!-Mhn+}#xEPumLW7(|mY#Hvkr ziM!upblP!5-s=%4Uix((kjn6gGeev7unEM^Y4lf?mO{DcZr}!RS&wAT!$;*{zL;Al zQx-GWylUr;e}XNqaGUS2CEO$}bGKme+0gyT`nzv)mjBF{cd&8$cGV>Z`n}e$=2h%= zsCi0Efl{Y9v#vf(?|#*l7pVpxm61E+7sel(MT5b2*Aub5F$&8RQ59%D=tThfG{MnB z%uA(V$JUEHXwY?^n&n6^#Ie*JeLbP%&24gySjsH2kE$+lAc8s#$22kswgQFtfUo6&;z3V)K!JK_u9Xfi(34=H&fU~ zRQ{L()RRz@dsB~Z?&_RrnAz0FS}1)1bkmVTe7aNMyFUB%kGECUMKOyi5-L=AW{(4{ zJdZnggydO#I$I{KYuJ%A+0ct&VuRZ07Fqdl1v8ZSGN{J3!h>JrbXWpxcEUy8_0{+? zri_cII21)_(+}-2yqX|eXDi7sY2D@_iGD3l+9+Du{Eo(b2>T*d*Xv`f%BDWUat~QW z3#P~MSW2&VIi(06p4QKmOaDZ{E97%+mezV-?qwJ?J*c5Fy_iAtuGuN6<-QSjr|(+cL2lu9{w&9qi7bRZ9eN z0|R6peS31!=hg5i`w+_CDP?Z4qI2D_-0L-{lg%h(N(k#Ut#Jo!d?p@q17@E{*oKXA ze&|GtCf;Dso_hv4$44lk<&-TAG^uF9u}p3rZ%(F9Ofx9Qlg1aalYf@I_!H^b?u56b z5zdX$T8O8Y=XprCQO@F$*g2)4MeCWlCnN?)3}?=1*qj?cb`h1)kvUF9Od4uW%|Ve{GFHkG?KRVsrq6?$hBVqX-W!LX?Zb9DTf(Hll+OW1Arjxn znmkCNUm66jX1)2--fC?|Gx6qSLO%Uxg4wE%%_HSqLkbS#p}3s9PXqOAf~om3a~h=A z`f}d9Vq)!sJ5J3D3uepg#HVo2l0P0yLSSvOL!>YoxoTeZcj5PhHfXo$C#gdwaS<%E z{$xAPy(^KOEmPVu5jJM#{+I*rm7h&y-oS-9L>!*t2h!Szm2tP8b(6`(?Axsj>LPa9 z{^P+i*v*^gMtCZ)bg-s)l}L;`^hfuZ#7NS_h5g2qlHXmUqf%OO->@D(MVE%Ar;3}e;iPc=2>{RB#d-|@KNHMNaz%SB*F#3*MBx1zn`VxGO@ zMD7?NRvf$hXom7=Vf@8ppq0+~=_ZMxiQU$1dirIoZ2L9j(0k)Ts$oX5S&f<4?kG)U zx|r)G6#QYX{=GFc?d)DNszlAyi=p=m-Aa6F^RLS=*(8Oh^OP|3QW9t(zJ}fQiLdv| zVyvLvR)+2)A7gwN{zj>)@ZWFo>rhD2eKrc7Gq`JbjbWENPkucbQ>|^X^UF84&xB+j zffu`C5|rEYz&L5BxV-!t$1wJa9o3>Z+uLOoMH%#jq#g@bp6Yns8TPa(_~JDvUxnOjJLq*b2Fu1YjC~KA%_8*SiLQN#P(Q@wXqrTl%dU0D{r) z8SvhOH=1)kHyH6Ys|R*03THK8an;0e0|Y_vEyJHgKV0&OMIUHITDrYe`n=dPUDn>d_Q`*Cq#L|?aL9oO%6Tz>k3xb zt?(@P5yM2921YI${>Dqx7a6@a5_zImwJI%Ne|C}}`WR!Q5n?@^JT*Zi~iO0koniGxSPFI~_&5z5y-KcX)f z9%EBqauF5%nkr%?ZJiIo`d4~kjx-ME=Fs5}ejWZ6H#1p%+-Y3s>qC{KqtH=RYO$qr z5{xHZ=-op3C~0lpfZuJWWOP)#2i$@?@2ifRkx3kx4-41SqJrQbLS9@=QNtqf zPCyJPC2jg7;Gd)f6ue7CVB|cpM_30r2~F^{X1iZxG8%)5=6x;{f{z`d8z{fbBhp2W zA|N_5f2W^AkG_r7x5Hlu`ImW?B@{|oH6w9Vf)q0>dx_U)|betC(W?#(laMe*v z!ksB=djPcZkrfQk^oz?Wuc;?VMzcXrR{KG@%eit(ipm)qz6UQYW|}#jsMgUNoUtZq z+ux)4v=A7ngNMWfuJ%k|qRP+fMymS|bvo}WS3jFcJme=Q=eLs)4_iJ%)OgA?7!%DZ z?!-3+&^)AN|OaoRJWop(|#sNJ;Z6ib8cI##NnG2Dot{ zwCZ}BgXFl{D9(G2oD-3D2w3{@&GWeYbdS;6nan1i%CWYlD=lgiiC01NU5F)&n8btf zRyPfsFGa6flzC{tU(ju%5hD&b6ZZdCE(pwO>CE^%{0=IXw}CF!pq)hBpqun`v?*W* z<#*D%iW*2waleIL4F(Kek^HyrE#s@hxR>`@kb12{!khe+Vvc*?S$g7qE-f=Q1FU3gbBF)EwaA%$!xOhhcn_1}D z`f(KXyAA)3`Fcmyv)CP=?_V=Vp#&Qoy)rWi(Y(YceuG6bQCgV8j2w9PT%MgVajgPT zWu}@1`m#jq-1dX_DIN@^#!iP0RHvfi=zov7$x-8kS~CsC?b(hXGgtema%PK_My8ax zm>2?out`Q*`6A7f(<`g3)P$#ID-}iFTvi5FX2a=t2fkmd&#H@rxUHh#oTWW}i&*b< z*F6iO?sPAwd{ogatX*Q1!V5Oh_Q-AA$StkY2KUcA(q>Mbi$X}$M63s!FYXqLMCzGH zpXY}g+z9jDV6~ERe)HM4mYv*b5jhbD&L?FSqqrxEbm)$Cxu%{eDgdZtyhb*~_ha{KdKvC|C~?Oo~^8@!Xo(;>8!KTXG_Rw5{u@95MNzfYGp z1CJIvI{B9G$HkVmESC|pxhYYw;Ekn3cG(rfeA+qWuO^1c_< zrGIkdN#@B}%d zxzm%KR5yO-y)>;Wp~y|!HWH!uj;Hj~Kc=uE6&s_ffY9b$fscPvl+BOS`6j-iQ!dXU zw#XN%=kGBPcae{kP3hT8iA`yg$>Ps2tJ7Fj>1aZ)JJ}b5?&2x%;MieUDolzFj6lO}!J+dU8C=H0a<#JfkKdKits0FB#=`2}$nHPVx)Pv?NwO6m zf-Y^WpLgs&=c?&_C5S2w2)s?wW&WmEzj4LH;SaZfGE}Z59oVz|8IpMz(hslj&nnZ- zgjISOw-N=!9iN2X=_|1+lMww;px|K(IKxb|-wy)E#Mr&U;^M5HOy>5CU#Jfu>9kl6e*7hTLt;F6 z$<&*1xZR5F<6-HpN|iXqZa%oG^>N$zO1!$yX;t!F^Nu$FoHYGWx6O~wH=Ec~;{yXG zH`3?FZ{fTNhZ*0`;KM&;nCQGa-I< zgD7OJ5P6Sm)shb{XSLVO&QHqGG|YO>d#nG*SCOo~!+=+BK0oJ$hR#T2@Ok7jYWO|K$MyE8ov*V^va`St2Z zMe@N#T)O&%Vn64NAMvxogmo`A+_ttzSp#1CmXB4Z^3Qq3?zN&A-KdN@mM2gTzawFJ zT(wSu|JXz?1*@ndH1_0Pr-`_RZ$rX7T#K1XM|}iv@21BSfAKBuuz~oKR^M;r>!%1& zJ#zDNz@NgwrWEuo3iQvKF2E~(K&jP9-0Z7A(0Qp*F&=LM}@>Jijo177Mrs{IZ3LbI<-7B5E*$9U2hasnr*Yr%iq!?M`i6(aW8|z ztreV`9vcnFLry;PrH2r8rym@)9VkQ8to+sNb@Nob@lYSnQ}yiRvCE%^_UypN?k%`# zd685vTAzyPVQnu`5vK-dZg4$3_js8cfBH~`g1{RB2b-c`<`2t<-17%3L__FSBsSF{ zLvPykZE5JZD$*)EZ-+(suUz2)pu~8yNGh}y>iKFVK56w*v?aZ$)f|MP&0$n;c1wwu z1Z1>XH9qEE_z7Nw{>fP7{s^&{r+$JO#6xS1sV-KTu=UK9CncDBA3+z-u8f=feqqnUZebh*+RUOJ%f+)qOzaJ;kDDl)FUTv2LjhUY52%Hy~ z$ilQXu-Peg^NsJX!c8?+-824tB%E}LU4qO3zeSmiHBEAykPER5d-1&r(d0~JaA5%e z!C^BF;D;J8+&$KtW`*rpN6mY?#^pFM*bM#ih^RzY4uKV!9`KD#F17h~qvmsvyieAx z(D|F73M{=pC@uwFbmfW=au~@}c2LmJWL-;{y=wwju;(gB56Fp+8vA+?;u$rn)zypgMyXExF1W}vZgSP> zu1CzwyqMQ5*LmB6xhnxs8;0sEkT9*6MqE8Mw|B74N9K)zDAB{m!5)l>=XQ5yT0lSr zt`0jt{SbuFDQH#C&&aNA6IOd;+^(B#!S!&cfr*iuAxf>x!vkMGhYBXUrK|}4P+5%* z7y7;HzvWlKB&!wZspT|yZY(y+ZeM&Bm>L+@X*t}qw`lr6S|TO_R@N5peKDp_bP1*Z zLJbhS|2u3Jf&5UoC*dJV-RZQQwgR7**O!TS>?KF-^d(B1#>7jK-fhMA(={(;@PK-2Uj& zl!8*$q$}4PSX~}=k0TgnZ2O?pT59(tvUJC|LBY%IkbKq5YsoDjP<=T$T(d{`Xx<2$ffwLf&G& zuh=c$?*%2HRPlDrC7Bpc-{-heRp8#!$*XmzgNN5uCx4=3i(Gs-@E?f^ml&o*r%=6X zv^Rx%!d= zL+hTDyz%0q{H}?>k31Q4?7TDYhG#c^VIGx2>Fg+yU;)CwtbXa%wNnog{oeCQGZ`_Z zlHi>^+uRnk-&HG+H^LXe`S_N%Kr`{zDpl>4U^r>v+bu45ABWar4aQ;)=Jb&@CDKo? z?Iz?GLgLRSKk6pUv~Lr|Rz54gJJC6Qm%NygXMIKeRX!J{N{5gQUQ1PzPw(sSTAI5* zDIWAS+@Qtp89$E$>PPSQO0vd}n_o}UE-3}Pc}$zT2F#TtdHrMt(#AaL&i>}gWo%4S zk+n;Ark<7U`<`12BW>(jnTQ1r&gKb!f4{M#QdTkN2+9%gMv$_XsYz#oNI~Mi=9;Xj z5X%vFqMy6;1Ej9g%>(F_yMnbr;f9^=37)R#CvKSYIe({eY)5QPWN$N9aCJo$J`*Ur z2)uvl^hN%(7vu!1Z6Rv}+!iPyk`{2!H#)e-_sMTshre1vg0OW{DR<%vP(e?ClBREa2zf=sKD(yH=%7fByYyU^SA6uE zT~g!bf48~$FPqC$j^)3LKGd4=5UO5}yB+G{JwkrdK%#T*fh|wSB6R9X?2XIkr!x*z z!rH(^sQL>ZePlh<*(KrPM@k5p_qHWpbVk=V7jr5#m@VP*fb0jJfeL3b39P ztb-M1W3}c8lu#KBg>8u{#wYI;Sekgleqs+{V)ZPus0(dc`f;sTdgoP#aM)za4oNMooK72uR zOgm0mpvD&bqU31#9`dE4d#{(`d^zCD!fBq7tKDl4Byi3KfzU)bwhv(kfP$H*3aYaG z#AOHOsR7lcE!}y$be860yY>39UxnPL83vfXXnI9={1kXWvohBMOQ)Ml55FI3QA1?4 zV}VQza1rlfnFPSErp=krVtqI&@9Gtl21GM;N7B|Ls1R)LYJ)E+K^;7fES}@^ZKHqw zE0RG)GW6cD)IPaMvi(I3w>zBlrT7ClPWpHN+QU*+F^2WT1llRD_5=jNTa$e*=*j^< zIS$7I>cp)>yH4zeRMX-uV;U~Ic;FRRu7g^A4(Ash{O$MeS|AO22|6+N_+ZQuuc^${C5xySL;xUzvFE35YJ}hg+VohdLAt6P+@#`Z7wcxTqe^s5;G(wP zma>XDRp*-HOroZ&Dh{Arcri9W`mPm~&ijCO-FoKdiO~zmmnR1qMv-}bKzo4r=@iJ# z*uSl}()6ys?qou1Uu;oKHNI-I+ZaFv4qWt2qV6D?M1k6qThwkFrKG5&A4=|#iC%@H zD6NadpqNB*vA%L1(rdC@i*Ac@F9YWDy<_8T2VAJpOpI!-_^;F6nc8U#LZqE=j25K!t=(HuA3?!_vg ziWhvm8V5==Zy@sbiLTh;RY4-%@kl^jb{IxDB`7rrri|+YJYUuFELz=|WCBW(gWjnV zWg1x??1Kb5Udag>Y8TGpbzfRDi}_V?lef&zjN;#bC``$pGDhb^yakB|G!x$-)`{(o z;H0hBf7TR{Copqw0n>B01MSW6Cx4qTuF!g3SXd1jTW+43 z-ex9({g$9}5^XH^g*r2EP5tfGKTRhtz)Y+p(JvA@buz90Y$eL6fh>4H47Olm+|@4B zx0SzA$vwo(wr}q9hw+^r7Dd~8bA#0KXQerZZ=r5kd;M3#pi}KVjyJp!5H7Mr?$wIs z!$6MWwHI6#-en5UT3YO;j(+X0pw>YIk&i^EtSC*2W@IjW@)ci;vbzy4znU5Hj%5o} zyM!zXG}18;sMa`ogCfuBK=L190|@K|HN~;SC4X z-P&XlYDE|8)!mR+P_ST&D@7~GU!_{X%)RZUIV}x($h>zW`hLFFvFbK#5RCnzv4vd# zJ(+Qtf(ZORuY7_8(7??&8EdKXxG1S%TS&Am-AtKg4IkwRGE0iiTP*l&%$tSG465IA zJ@SHCO;7WMy3UWPEb^;jUyFr#u7cWb2WhwV_@-HO$26c5Wr?*oD9Pyy1UA7*+@8&* z((zAR`7LZ!gkT{wZ?iNaJ>Z@UbbWR5B`7)il)Au;%!?&o9yTLv&=&MoYxoN$rBR;l zozc?Aeq?W1KLbHFi3$fP?KuEZYx7P2+RYM(+T$g8tU9LEMq8%tt+u}I^W_X2avu5! z&i0p4*LNJCgnM5#^{8`9=2l#Cs*GZHdR9S}aN8oW&86^Eb0^CgUJH#89J^Uwqf5d8 z9QCc0Q-aI&WUa7yXu{;;Q@hSb3e36tqYm<=0)z0x219<=9Gvu92kB<|QiiGCtO4(M z0NHxI#_KG0CUP?vj2^2A`)vF(t=^`^gW|1m1>$x_!e{X8v_el|13@5Z+3v=(nAvs2CEfvZ zq@=@4>vl|gDB%&@?FGkF2Eme5Mzw1ur(~|<(~lCYur(-7`qSEryzW`?^dhJy3`dGP zw&(1$7ku`?9_Gh5=>c_azR+rIeAuPL=2(R-3{a|(8)wJcaKq;fd*G5~*_j2ua0s-k z^U}MuZVDaZB77a{Z~THRtxHYc2sR5nXZQNdY}DYQpbRIi6TR}T-qnm61lpGFxIyVU z1j3g9^v4S*Ir-M^yrFg!A`Xl^3q2#z$KhaJ3zRQ~1d;a59#k41F2Mww?x~J3 zp~cy5^>B`eUZpnPi33s#k1V?CJNvVNtP)4~5bZlES0gSW-PYMw zu}6*ro%kLs1|qXsO%Nw-)^lTp1L=9|H?t89hWN`*+vU+-yvz%m~ zYLn0!C5kF~B*omSAT26Kd`$PsjS`9>cY~xk13wW2qBH+s>Kf#cFCiC4vxrVcjESDd z-e$lj?j4s&QWA2y6--dmlaP89Lh4sDi511(v3F|lu^J*9U%d6Hp541DTDJ2t2z`oFdgHE#maIP(1QB?aOV^zG8=eQ6J`p5GZJG?}uSNtg zi+dBK;gnJ<1M@T@zmyM8#WW%B;Q;f4kgr@JClOxigxg+3QVSXLm|46ZO^d$mHc4Io zDx#ZCP%atBL_auxop2_x?We`!IZ2NHhmHE0ZgwkJZ?j~cmQB@!{XLEtF!dqs~A zcM6w+ysYc8LS|>+^vnG)6&Qe1jlU_B{^sAN3EjOqkU$|jsA9hHmqCu-kbYBN?1grbkDo=2AGIxk$mrQbv}1^8O4vwwt+K2n7sM=&`xZe zhYyNJ=2KxMpkS$d6I zg;gD(d7#cqk-UoEe%)HU&{Z3`fHg?G|0oT`VV1Lqy?k~7>+kwQ4ujz z9&yrF=}KifOWurX!*zOr`aLF5Svd~M;gQ^zrhn*jAEq|Y?7Zr%wAciYw|>0EW%uXr zm&f1(-mX$3EjO7FYx5rBj9l%Oltl2pky5oFy#j?r52&w2+A9vAH^A5gBb02sRP%)i--M`2I+i=F|-=x z+4V)0TNntexO6$&6GecJ!Bk9>bySrXs;_|AesRhJ?wOE^e$$Q*b%5$M?qeN+)LM7!9QxXS+JbDO*gX8HHu}*ykESxS15bbLW z-&*{|Ph?%Hsb#8%G-+V4zn!aKQ^3P;dypBd%04%E&_OretoFy5?VlEw%}!KWT@5=O zyAMATX4GBZ9uykTwyJ8LF9`;Mt$#ZEgS<1d_z+5b=!w&!LbWn-F$1&9->g8 zmeId&KnCR`DB8K8gyQ1)8I0NIxLl@v@_4|5SlB7wq2y*Kh*=b^xe0Q)pe!!M;B|>!7$;cl_STA0H@@aA2AvtLYLXvm4)@B!Dxpog%LWF1c1o|46O3G4Vh7sJUc@vZ1- zUqMMV4O3b&@;i}kMT3d-Y#_Q9{Kg4k^krMDIhF`eD2S(LJz-+2=UgP)eUOqbjl_Fo zorsJBdTix1D)6&rsXA+sy%vhDwvs)ogIUbvjYbt-=QJY@Aju9@S5>&1&Z2m ztWAw0kP%Pnslq(_}O5C4;uO}I*1BY8(;fxo8NNRyeSN96Y(Ji<292OD?>>7wO> zC^*zjj%gb<0N1PnMXsw_j|o6Ve`gH@6hgiyQ6aT8u#4#cck!4_>dvsb`j1%}A-GK% zVkPZ-(gvaN(U1dMHB?0>=uZnBqPFU+O=SHdx9FEYrVLJ=sq1gD5d@BV2_Xj1AKEa) zZS(eIKW2-Yu&w;*#yr z0sK$8`UiilzYy{o-K$KZiG*k}y6sH;Q7pjBO$!vv4UeRu?u|3&RVJ9P6?F z1mX(?`vHX`Cg?JvJ$gf47>%27r{%(Zxl@cAJl5^c1-0ynEa1gy!@9`KlWnmld*jEv zI`pTA4g==^QWDfAzwe%T95UyU8Rd@Y^qPde5J=n1&&wks1cP1JncW6?Cd@`uKY(rm z^$JLA%gd%^%@McR1k6vbFRMJM)y?E6HW0{;Xi047^@(VA=&w(?@;0di0M%@SdOdwMN3bLJXCyzNS~p^Jh)xnj8NBd zUNy-1tQy~zzzwVC(A$sDI`J&TPV%Fu3C1zWuG;qJeP_)CSOnd*5o*r^pMAmJQqU`e zXJ$zGfJ|X07ah^9*(FK7lMq9kkIoomS-tzg$qgH-;A~QQgE80PPRPDZarjSKI$R8$ z)804(0Uf|ehXZVceSor{i`Oi1ooGab!1{yd(a;`Fu2^s&Zb;qp%27^+%}V&(`4(xzOZ)0d%4`MTYJB_CEB+%X5feHCOOz$Wca%}^dbnhE(RKkr4 zP;^kE#)gK4b7%%K_I~BU!C_A=iyu*Y)2{}ktK5NVg;U*IXd@B|N2lCAVcXx$nzJO` zvl-2ka7M2$xlT~aCsk5-BWg0J|ILULU}fm4cTWnpMr@z@hEmj$v~BZ1%S=TfNF0qb z{$2t+MGsiK0KEM!3b%`-=VD&YdBWKp^fVLblx>wq^cFxQmq;McwI9)8;(EPjqvpO- z2?4;ATocX~tZ1KbHn9Bgu`m3XFY$H-|0hKbP{%7y-t#>Q%^8695Z>bH8PX0nZ}O-B zkPBeKvKzunh6Wun2HpYYvZ!sy(g^s9P6kmpAy0Nt7m zY96+_Q|0~y@US+u@;PtL$Ql8qtiD6sKz00;f+7%#{8juC%J$L{H&kePFGmbd(t@f7 zbLl|b)bH=%OJ5`+{g+K$=mCQDH>6m=i%Dmi*64$x`Ko2 zzCplYa&00SE#S=<4r!a$pFgCa^e>uw_og`L8xjZ=k^R08pOx0P6WoavoB?T}BF{7$ zWWgp7p7QC|i~sr1!V;H2(jc*_I@)uSOSSSpxe_>e^}0YGoupBvhbRMbMhGO(OF2~h zw)=?rwdkwBJRYsO7ddbU@D~M0yRd(7X7GE`fy%;J-r$n*j~x5*s{L>{;f2%~oZ-NG z)NGDllg-KvbW|GzgI&~VW>W!H!M5L`I&^M>5{5J$@|$Oze8V4O0e`~*dHABV+am@C zR2%xCGaA;09cWy63w1~V`?!xZ*K10 zYJJ{vx=0t*+dnu@L7dqst^H=>mjraL!1SE&K$N^=!grCT+OE$^Uf^KpJ5|~d1zC`1 zVz~kvhqk{ml>0bl4M1sk4{d`3H=6>=t!L;opRe>Jd5|(dXFj4^fDhb#G7d}SK0!zRU+|++;_?(WDrT2G1zWS zft(q`I93ll*~lx7ΜVH+KNCS@7NYC0Mm^E7-zVSm6LNpT(j{>| z)YGW9V^p^Fls-%dZX|B+He?*kou0pvOQnkyHF|3|g)~EDdeU&f9`{&IW)Ah*W+U%( za*o{=&pQRmpWQ1Ms1o!3c%SKDTm_B`<}1L`Qd60)RP??Npo^vpDL}&Bw4vhnST3yQ z*>cH-U&JxJewyF~-wYf(ibGQ7HO98r=eJ>kpP~R52eKbvBh)_OkBlt#r5rPyLXd)J z9aP>xt?7nab17**P;7W+SJ01McsG1~8SkJ~bQYpSjU4B&3%KXTgif`n3a3b02Tt+5 znJp2oMS#C>Vfd!!^ek|><08>fH(bC-^NL*m1juDFE`2pnxtEXS(a^K04Js(SpvFAW z#KU(J%=!URn;)iKAE4#h8X%9o*`X=Vm{Ldu1!u$I`L(7{+8Gy;ngEqEWl$O2r&g2V zkbU4CNHWs5*0J{f-_Yi|NkNH&9+tLmTti}I*md=E~!xzM3KTfYDJ_5k*Ci- z=Gy^u;;T~i$(e}r zH8ccE{Tz&wefS_K7N{UdwWm~i#4gk*Dy4bhc)Ug!m{4MN6}*L`5pqLXyH?NFnLzZlfK01kANpj zpUjGpA7O*NHUL`zx+m?s_;q1cBZC6W5eQcJTU@)5&IJn$OsSC9|2U_W{9#JMem&OM zM`dsJ^F}h~F0}UGbAKC4{z%N}njHEqN@LYO@b~NfI~1VRl$%;5hvD(ke1r!FMRhWG zHBrUAnP%Uey~hCX4m(dK75ZUQY6$W8YWrNx_(_nFuAZIZhwdflvnjdKtBz z(T4}{x4iqj4uO8tc4dwnl$ZSbRN7?gcN>AMMkZO@(npBZ&xxzLl)UN1{; zpEj@`GLMXpU)2VuZf19R4FEF@>O>aGJIcT>5H@EzBGRI-mSllfL_JL)1Kvji#!=Z1 z?)`Bdk!RdL(IlU(cFfKS3v+o@d_0&G5FB;G1ra4Th` zuS|C%Y^UFA|N1GZy6Elh=?k}Kew8KY`q-QXs;hS!fVnH0j)bCv557uKM&F?Nw1or- zp|#_C^J{D8FJYR#>Z*2-qnnr4`gHVZWE(#fKyQ^sn3eWMfqFefeLSe8b^yn+4pP_U z_~%%S`Zuahcy2$MGE*nGna17EqiAj_a2XxR?hR5)g7uys!n=sH&Exk`>km#-&Z+Wy zoy$e$@7`(~r_j5)DzaeEYlx@F)hZo>O1~QE-zgL(ep{51o4~Ic4C^Jm{fagaK{9#p zr=6(I6t%h!n&O#lfGHa0?QobSz%xj^ZKLUv=_ch) z7WRkte!HG(f`-CvxJ@eIN*DpWf}g%SFz|Dsy;~12MCAdpKbO1zQxJB#M=zKxf`!TC z9AYRCFpmHlEzH|G^x*c)vGje)*I$q1J5!H88Sonn0<|D$9-L|{iVbnxPRI3v&_Gy*F8o+w;p;55v}ZxU2ysgaRhxL{%MgD6acz&IFuqF-*4U`s{`wUj;8(nwVd5? z=I#|akdRPn2I+0cwB!WqB`QViG_$;WrtE5RzO7Z{ob3uHlYSPDlhQ*zDZD@J7#=v* z+M@CX{fOhRGhH=S+sb~1HM;(!D(-LC-Cod{6Km}Or2E|VuM>03BX>{#0)>RhPLKN$ zS9rbkvymkgAb-ijfg~M2P+kDkki6OnIgIhW(b)3o-}>yMo%b3DC55c?WNx||^Yf>6 zImCmBjL%!T(~yupOI={W-1)WhmyM?T^vq=)=7w>IHrLB7YzceqM$_tXE+uCEDJM`4 zzw2e+&a-}|kVAX2khKHW-jM-4N$YA0c_^T0UANNh{uDw~16Un_p58_B_qWv+r|)wp z?z~r4CnfnT%=c&0_j|uc*P68bYrpW!KPWF0j%t(qo~y3cDyOOmU!7aXg}Cq4OLu2b zZ=ie-^bgMAu1Ul_qn^+F=Q|zS>wH1`z%#2~HKwU}3=HqR%$<11e4!89r)R6yt_vfW zU$hUp-<{37+5&0IQ2dulx zcaY^aM4ManVA0FR`@GJ(`|tN$>vL4Un;iMkZ34{nH$2{f6@Q;xTRXb5zp>gn*p7p| zV*Pb0p&Rg_O7>3jaKguqI8FMgklm+v4RWX-oIb(5cpvnkw_9|P?&vmjs}8aLd*XB{ zA*Cd1{c}G^FrB+$3?94sXBmll3;nADF5baeMh)rqWk+BD@P;6iK+^7YlW^d5aW5eJ zNc=CP44+NIZ8{P{%S?NG-o^^TQtlA&R~KoX-Qf|qV|Q~>V|$FR+wa5~<2$6X$GH9k z7Q7z4OK|=|e{1*1*k;+C*b9e$pEEN(!WWMP&vXyZx-&<&f5bq^ooCf+@+F(n10`1W zbs)YAlFB)Ie>?oBXuLxf<@;{-BZ2-IGr#@b?$E(p!_lfQ#!+Y?T-<%?#XO8 zYnbB%jiM>Kv5UUsQ#_;^M4|K@c9p4Rya&h!=_fyq zyjec?^*4UIsbPU*u)iM8;`68jCy0*36F%JN+rx0ntp}C=3SPPq`(-6*cc2^kMY)@w z1)Oy0d>`zWMO5984vEm6%81@K5vcfD$q8cnh5Uqo*nPgN)sv5X#+tBNW$2BaPfiX& zFz-R7yGzrL5rTVn`O`s~YJY7Odzg0Tp4%nwr~L-CjWg@V1(1ia?yorZ#{*QE#49AG zOw5q1JCogOE6uOs(jhq-L!Qx2Ul1?X*X#Y=iI5!QE%H5QZewZ&*eRh2i~+q zcW-@{>@7fS$697VJ|D&T23b?7Ru`>a5j{zD@mXJJ-x1H0Met3JWld7BKvya1C~iS{ zJMg9y9V$L)rBhLxmzMPt0zh)#wD6bFbddPz{U|x(nRkG2(_m$hg7k=w*(b^p74dip zF9iSx*&kmBs$5b)ljcod1+bx&w;|Hk0`pX%t^DGC?cc&OSLg%AK@RE;4md7y%ze#( zpUKYSDT&=KI1!*NH$-BWl=WBixa>6?LIvhcp+jz^l~Uia=x?}3j#p1&V~CqPV+~M? z>fbm7$MDK*t;$8aZ#m=&u28_7l?^PSyd8VvA^UBQK*q7#pRynhpZx&!9qRNfGid`! z735}X6Y9eg-Ox=IrMvluy`7Y@Iq~}gzE`|K*yU1y*!I}u^v-6S^h7U(?Kx=rp9*Ii z#Jh})pkaY&&+v_m45>-KB$cx4&vV~>A8(nCp>HGz2u6Nf^RYXe9{ObwcQ+D{#1B~X zyXXjwD$ad`DYSGe)(8yYJLY7C+vDS1oc!Zyo)}GvQ{!zzYCo6DfK?=>K6OGO}2^nFQ?T@OI)D4$NMzItjmG`Fh-Ph&n`+z7Jd-^-T_R)lV$SsZV;4? z@~__{-pSSVf;>|G`5oXB_}>d|37_>isMnP0cH>YLpLQDk%X6FD8>N1IdvW)E$aOIe zrxod&pb>@73#Xsm=rIb3_`D6xbN7(X)Dg4JzB}2ju{LNX9ky2_RPSilRp0b^pew=b zL#rgpG~ex--#CEA-?o(CxATSqGq1Rh_W9Y~cE7u!p6$3F6L+)!cv}vnQ53=X{ciR? z)TLD2oo-UOg34{u^<$si4uLSgMdj=>RjT5XajQ~Kohc28Z`3%45Me^u`ctp3OQpQc zt(yKoQ2zCs=(9xI-F4MG>2}pt-C;_70;`Rze;3^Y`Cl4;31Q}P8TKw6=hWOS8lhq%u}j8zs9n>zuwHTI~>LMO1Gbz{<>DK zpGb2LKZJadnp$7!ttYigxqj#C;;YqEs*>vN^Ybn*5MY@pkxD(E;ACKw%crw%r+U7= z`cOai9f&iU8GIs3XbjDIXN7WqSX_=kz@J=AP@ixMWP!)~v`hP|Ihrn%Y{;y-#eFFA z5B>K!cNG*Z5M>f>n;u&?7YkCp*8OMglh$Nh{rb>|sfFPG`+8Fnq&nOw_MB3$-j;95 z8XBd;-JFJ5na)+Tw@DT79lVrOZ}35fr7fCW>g&vAVrOmO+2Cej}M3^z*(X zCqo&uEoP+R$JC6m%FgSTSdZituGkA|>;>bDeSQavn3yfsE}%VEx-akWO1i0OZ=o@q z57Gzxqn}Vv3P=#Z5!#~L4}Vg+Ret=qH_$Y-W;IF+i^Q|`8TV*aW*iKLxr47i+FN2B zA5!c#`ZR6oR@-&3j>B}wVpO>y$wa5uR=w-kcLZepjwX5p( zYnY26G~XMDdDpiyBhg@UGvs?W*ALWOqd7jH(azW4O@P80^F}^bdDv3xnH&(4g{}zg zk=Wl--pl9S%s&ZDdw-vKy)cj9GI*dUo)2yJDrSB+xMuC^N0?{<2=9RUKlP_&kLwM> z%hgF7A01ejw5z`uNa|6B_$e>>u!qU3zigl!WnP>0gh*)nXoxGqMTp!(xAgtIUyjG_ z2vVxKd0XyS+f1#~dkxt@P1h-zi;HX8;l*<1alPYE%gg&#@B7!DI)57giG`;apVNhd zghzz)5?0yk1*Zxc(X;4V#p;4r2 zFusoJ7MgjJb|AC7N?WGPReAblzv?l-IRQ;*nK*|eDEQpiu(gmMUnW~+xP)q@D^sl~xR_n;5nca_~pn}SO%DwzQ zm&L)k^S@SsVz77pU;U7RGXLkYIO6}HcRQZu*!|z1giwh)QvWH7gOWKM54lK?GsvHN zY`o;%|5hZ2q5p?K|L4PhxAY`W?By<%hE?@Opd!OmlJIcCjzL7YiYto%mwulap+I)|F-yRO2khW5( zyt+k}wK+tR;Q3s}cB$#o3$hu5qHFg1wrbDAxh4w>tU(>IBE1OOQs+eK^5WU4*a2IE8cpK`wn(z!;}3Y>!URo zAn^v;@ZvuO$cu3P5rlcmcJ6%IqbGpC)vb3VCwyG|Ue(dh{SNWnTEoOgBE;Gc`6(Uz zA1;>f#EQOgct`i}pDLav0yKHc@IYA45AI+ap&##K3w8U%T0Zk2@_bm(`(S^!Gx7bd z4njrnCmmL13f$lT2O`=Z4$t%C z2Qo26RA~sTz0en@ESz8aPb~`Ou98Yw8eA}$H!+%^KA-eo@ppmRv(SL&mLYQ{=#?4-JKkxsq|3%&{GQL{kf8!Zn9SLu zii*E6>bMIu!PsvAx}d`b!xwrf@-F6wM?nRw&p+4gB*n`2O=d~mFnpf!{!kRuC^GN; zmpB&$L3$OYPpgjAIR$Qw+mf7+C?L!w$6q;_X@13JiGRq;g#3JUe0zF zS&KT1$8(CmZCtnnQC0Br#T?RDy6M-y^zvcd7on&>AB-oG^p9sgqz->hhaFF;h{u6% zU3C<5F&C`(M5LUg`rU9HJ?&hFb#~`;D*9|B)jDC*3SL$obbgYhD8q60=up3{UllC zGuy0)ESA4q(6AU>nfr+eJLp(@+*4kQs;XpZf-Mm~3eqcHxEKwf`=B^Xo%?5@zO$fR@KmP_n|6mp%R%@cmn@ z7?uk$)4DUZ{2_5SVJP8T(*`kp2jX7TUPBjooy@19NM`#+*pc!}Wz{q}nAu9cIHWE) z(-!)@Xm~CiNVXt;TJq`1N3BKVLnt9WroZ!hgug;@fwF^SLP2hq2#1vctf@0v);J>y z#$1LN$>&IlhLQOe-(&;9+wI}6%?enx>TN)uvK?(RRdIc>9zSj;bDo6GV795i7C?4y zxp1ucQI=9HUs#L0cFJrC3_sjVU`D=UCZYICUz9I=j6*`=#9wYy;jJiqp6z}gmM1o- zJCFU zFN0VK8Q8d}7S|^0uP?;o=wPb$!If<*AUspa|FY=CYzfs?hos1JLGc{^_eBLAdZDJJ zLzSJ2n$XMBeh%}D1GFRu(Sc8o{75?jS%qTzfwe@m*9(2$89OdTs@Gusip}Q{|H%5r zsE`laqV&c2eF-9mUQob*x+{_mX|_;c#UY7pNWdyzSaZ5Z9?&NsAd;aTtef_|Gb1ZYY|YM|R4#?K{%Ci;a=b1$5lMt~3icWLFxM0EU8Y;!*HYGqAomypRX3?;5x#^r30T0t8#~a2$IT zBt<^NSiFIobJ;sJscTA$5Tqdwb6@U0!bsvBDF5>ckJNW z$;_WP$WZ3wj1I^6R%CO1fxcAuz=7_n{Om0Cq6_zn^+Q=Mntw|W>1T2AypYG~=z^>| zzl)m8&e;N=Rloyzd0+0@3~7dpofFs8CTcHea%(2Ro#9!v7N%FpAI8mV9F#G*e=qLs zKDA>wOJ;k{E`g`7T$=S1Etrq&qlOPNNVfe4%cbQkedYHLo2i+uSZQ4Y^M4 z3hlD2cX*3~7C>$yLg}HlE_7%T9%|ie)g7jr6aP5c61~a>S?+#^gV+g3N;Si zYl?3j_SJm@loa@JF8QWz!r22T-?Yyj%7K*%N>_EieJs7Spxae3P9-YhvhG`#T9=hefwed#aVW(UvG%TC#Ij z-}v}@F2Q9Y5s3+(fS66=NlRA(eMItD60*X(<>c*Gla1MVOP#$vNYyKu5xY;Xy~8y) zGwbzpW+d~KI6Gt|jZm@B-7EdzhufX=^5b|L0SM?KR-XLGY1L1?2_-L<>1(>0GBr6LpysXisx>F9b9E zYb3N;Cx|+_F9`w8#wW(ZB@uI&LHLb4JP!(u(w`}h8;c~le8_a3{`23pVcyJBz zSJ3nRr?%20x7i0-Sx}YoBm+v{NSa}p28RaGuEYUD&KR#pYvi^0KBN;}pU4t^s&p$r zx!x1(`7T!eTLP1_nplk2GF29B+B6*|VM@6!_A5Q|)rsNq0@H_PON7IGY{9blhUrK4 z(dLm?rz=xs$txHf`o@dN)|#a$p@Lo1U-ol^=t{Yp7T$4X>J?7YR|zpO0Ir$)#Xkt_ z{VN@?Ea|Q|p5E5*tx}{k)n}5#Ok$UvtufI#EfWGy5- zSdx0*P^_)d{^V{Ms=JHK09`OCa?cDWid=!+Tn69u4&>>R`Qi`EL07%k#q1%s5A9hk z@wmO<3e?niL~&O-L0`!|L~w-kR(s*awWHS4)KXG4nHjDZOoV1X8IZp2_m^p3xH3Hs zoKSy+X*M8`(Kz>mcp_CbP}YW=lN!H@GXs7A6{6(aEtF}=6lNOZ@)i;s_!sDqD1>kV zya_V_`jBO8KobO7Mh5dR?#a(Pk%?Y@wj@;Rrh6cBcuyZ|<_^Jkl`1c9)qJHxuJ&tO zB)d;(2;7C)!6{+NM|{K~bM-1}9k4*= zuphRgEhercB1QJt+6PbOn_RvRFTh#t%v>*zn5WMiZa)w+N`zyAaQ*iTX)lydEd9D= z2SlSs#=`Z)kcI%jRBgkN`Fg~^`}O>*-`kNRAFi+WX~=Em6N7LrjNj$^mKcbW&18*Q z-pmYTl@5ztUW}8PUSWyJI0%;AU5Rj*$@*02l;X4IgtYWh2p{NCEFal-Bi53n_zQWS zzY0)XZQcs&N>ZD3ZXT6hvtA8`FJ{s~6Ys*0OCbjOV1I=i4vwvV;wrb2-B{|6=`qvq zYYfF;oqg?hH-t8%F|Mf01;r%i7V{3&EKVNCLOznm*!TQ zMQdj~?^kM%vU2+aW%Wg%P7(V7q0kD)&lD-85eH-%pch~MU#XvqQy4zd#UGNAleSB| zk|jcGB~CNIJ)UpBItLTf7r_i!hdog_?b_=tSZL$n3iTNzO!uAt>e^`e| zr+ZKOECe(e`}Xb5t_GsF^~ zU9{d#Wq$P&By4H-^R@py!}yi)ym}(@H6tTTnQ@wB3ami==&G(dey*=A*~VH!`2yqT zhqNsj!Pqgs=l<|5Tr1nN%+OzRrvTrH?1M)*mWre;f)(eJ(QYr4iW1;=pwe@BLP5|C zPy>!v+WSoRD?uEt&+*9bDRRSrxLkSG2w%5;|LFP!gO0KW z~EFZbu2BRIWm;xh#MIY$QqcRL+>+^X#i&JRy#-q81}EsB#)3v~liwq4~VV#JNv_h8eMW@?JZFh?xv7Ja&>OXPbTP)#!;^SWd&VK@(HGl(K=kKXhY(|x4&M8~ z3~z;}jOLfd7qS1lN1HTmJaZhVL96(AXSjv?mctAkraKeY?clymC{vpV|DIA^ksgXs z8-2%S|Ft4&mJzLJ&Zn!Wucvc$yE@LYXGm|$!k{vrIP=8)#T~U z_A=t>@V`Sc+~P&%T4Z+N+UMVxQU=1>;|O0%|3GW!IR+(Ju^uFcN$+MTANX@=+!4|R zU~Gr%#nwI{))=drW0)a&mF_rDMt-9e`BIL=luKIMfZg~&7T!k;3x_^`_G2UlVA=)VJ1-#qofa zDcW&sQuOkJN?ym<#T{g&d}JX6QEU718sm9KEu^2^=GOf^L8FoL89%sIk)?eE+WcO$ zbj4rdMY8^l_0t{{yz=hf+AB@&Z5OyLR`yM8bJ92QvGR8qrf4g+Bl#vQ zXcV@L_-7INOZ{ z3{Kn>2I`~Rk4$wvYpLqoM!c#%Z!N!m$mb27{jbl1yM*S4)wUF>GDaxgy`1JFwWv1alFq+wK24CK#uG>+y5FInN!w*6s4N`|Fsm z86s#;2H;o(?%(XAwY$Kfha57pyNFnqks4wWS5T_k?Q!veiaX3IT!WG{0^8x2vy9s4 z(R<9NqXL|gb;~9+t)u=s4d2!y`^H2lMBN(*1F=XWtjKqO7jxKTbqj*1zaE`j+LouDg z@O}}j#VHn+4{_H{GRO#55n^lnHGAOTmb-7rWUrzU&G$q`ygzfckJ=VUZ;}+wUpieZ zh>xfjLqJb@LtQF9S9#cTNrtp2b_}b8`!oZ`#M@rOVHfD?OeZ6_tz_DMC*=B07Z$7; z9?Y2CX;|vZbV^X=Nnmaf^$-pzTCO_Ur)>xN1KA!>`o2#LqQjhiSlC(&sNIXJcjf< zT~zFcu5h;%o}S`0wuu25JrL$$$<@W}GfD6=jlu{A_u)Z{2mJIa%!0*74^sV#%M<-Z z{oT9a5_s>px1k5+?;Rt;$O^4I&uJ2OpX4eIyj#r_g-rkw^JH@ZexC*0_kq=Dp`h9Y zFNq1LT4$=l{yx5fb>_vZdsXtLOBY^as)xmKZDp77yBZBwEK~BKe!qqU*zU~uJdrD=KPS|35Nv$wU9@ikge6iJOuL@_bA_ zz2~P-Pdq74oHzeX+2$U$SLQEnxm!Dk>EbPS7mNHDJ4#qsBRJz%TFYW zOoIg-qMwoYemnP1{-{A8O)YEdTZanfbu$SnWlD_u^Uua}|#vIm1sgz2yZ}-1bS&31)A-$ep z!<+l9x9ucs<>;K?6^nu-3du&?lgZhU$gg_8=~O7i4p|D=H|MK(LD3uekI0-a|1QN_ z`?p>qK&{yR>3byDO+r+C;T8%NCN29sb zE5Ni~3^@)!)X6;=acTeE^AcWL4IIqzL|Uk^rs(y+Dkz~1mdm1|W~*eqe6ku5lfr6z z@xwYp^P^zU{0);ED6QJp)OlNeMpZKS)9=p zgKkhDUyn*AG3m~cMxKbQxh&(0h@8FVfE={p!o>gc`P~(lf3@PwghS5pvff-9mFyBH zvG)2P+ysJu5$n0F!w6#FJd!y@+LjBO{VT8Z-)7e_==HDZ4XNlGDUywGmy{<<=u5B2 zIckf5Yd`<`@;<#^txaD|L@siU7C7ux`Z1{sNNK;x`r_|1Rm`f;rH6Bn20`GSPTpN8)=Nta2M}9^@TKqpp0y>XJTlq&xVli6pD0wMC-glfSQ!Hijeqx}3tulU?P()$(-1C@ zXv|iRDrr(=Mt_z?gfegd2b+F3NdTwg3zv^=cDogJ*reHge?dopZN8%ghGC9RvO8Sj zp?ZB)TKnq=pKZ2~x^py$iLgIMe#hFu?HfoyVA`!`6kRIXq85_B@f>D;r4g%n?US?k zagrA~bFQ?2i2gy%$@4u(drh3A15PE~W&z@k12YD+|BBR601zWddF>@Swp@d_fBFDD zes2A10LVL2+P~Rv{$RKA&ofm(2L)^GX)yrr;X$O^et%lAjBKT1l_Qed10;SgwEWjE zv|xUO?{8Z_A+}{bk5K!4FOi*%*O43jHGAc+^_aUE?|XRQm|8y%eM4PyzIZ9`_ZdS+ z#(K0)-_sowJhC3f99x@{+VIx;YTx6Y%75<3k$X!2dgO<4K6k8=xcsW_JnB_X|*!eyEp<|3TjZyGAbG>z82&`3BCFx~na9tcKTvS34;2=8OQa3#f>I&r$N2?A$rxfos!hqzg;Q;!X;7sI$c27F7AC%pY|_i$OgOJ^silA zSg0PL!I;TNxTK3;k+Xac?}AQ=%$>a4n*)^Gh*u~cFExpBaM;Fmtr67E@6Z~_yqNxv z0x_~*JpFbiJ9~RvdS>}1_TWL)5*2UD{Ie5hiM$8-CYlVy-M~3&RIre^-TZ_2(8UQ} zQIDR6gQTCP54-O_L1QyFcx#7u!BE&zF@wLsn@o104$i+m7XM(V<61XcHbO}hL`@WQ zJ(Ib0kdfr-?Q!Vq0y0gf$E33!HE7veMyT@+g9g>{um?%b*9&pckzIn~%)SJaB1)qP zwQyLs4^EPqe%!zNL5)p*lzXn&fh5N~w7W`nqOfv)AvX_qxEse88QD*aeUy5<-*kV@ zr$>FJnt;p!^)W>Zr}?Mr7j9<8LU97vV70|QPTO;5Rf7|IwobtY6Mo1u`F@I$5?*9x zU1XI>5T8-s0o+6S=Lx5A9n9Yq&8mR`r@j&SNH@JO7`LED^k4ZkNWFe90SGYF$Hu) z2@{sQoS$-PDl1p&6}#J91LLCEKuC_Sk&X*6Tb50Tdr9a}o5X_3W8(iIQN|xSVcp-X zAjRGZ#nJ!Tkr?(A(Fw|I{IZG|2|q|MOV#5um-l72$E>MUJ5qE1*o%79`xxxqqs3_; zKN3c4N24mh#Kk{RH*5r`tuMQaWFg@8>6ASjdF-x~O?ss#9^rDwZ~8^w7!-%?Z)*HI zz|y_p9V7r0xpl$YIr_IBjHA)XTzX|iyz~w64f?URWXwPL8SGK=sfpMZGc!h=vulbv zJv#eImA^FHMH%RL8&30bXqrm;b{-i-pvvTljZuU+Fe20+&x`cRBqBh4>tSF%x8>jAvp0hq|~;F#gwPLlnFCheH4y@_|h>SCaRlGdo~p?g5N$ zXyQ&ld_I&9IV0ATu)Dbj=D#qPo%uz3B%)BRUk3h!!fTn18BEYBlQbIW9NrQT6FE!R z>g$bu+6%0@M;{rd+5EeohR_NsNRF}n3;u(%AXRqpcLblg5|Wzv7ePd2{*NwgE?{=qo+6JZ^th^9t0*-y&j0k{zQ zW*KlxAj<^dc3TNhJ%PladHnC`th-=n|LHOIw1-IZ>mSkwpccUwu(FPiSA5$2m^UHg zxvf1TNd|XF7F{K{3(AThs|5%O>V8AVe5d+*70;vr1Mx=%+7)QS_z+<{lNKnMW&S<8 zoJt-alE+laEe6g9OnesJf8Q;2?mnviS1jmmTG~J45!ndg%N+>Lsaadu4cv#0y$#5b z@Gt{>hXgMt`kyEtSp*KQRb-8!%YW@WZ$5MMMTNA}lHcurP`@MZ{zV@0vX4_k4&V!W&^&d2PijfS-_}Nyu~Yvcs%&PHv$H%&z`B}0>>uR|PdoMmUK`e%cVQ$l z1AxXe4tpaO8Wic*BZ)f^+rygwjS7UDW7107EK#56Z#F-kD^s9aZ`MxSmPOsQFZ5&-W|p5dVdjw2z2= zBH!N;p~4q>*k=$}Y5&Hcm{}LAoWK8z)PUS)EboErhT4u6uYe)`B1{Mzv7<<$S1>j8 zMx+98ArZ&?0184o+*7H(iH{0rpTt2R>McSCX#);i9M?gpjdtXy5jKk=9nTq=PYa4q zTmPay?wbnX-0mq7+Cl^OD2IZZAAZ&YCi{OPRr8dqf^tM+FCO;vyYnPY1dQ@w`=yBe znXC#$2SfR9w&Lz*+@0*G?wjn;f)=2JOkS^KPejCfToGIAZ;OoB`wjD;)RnP+;Rr?1 zs(~EkC@^$wdgTv)Rpp)^8|Y<|d5~NKXa1yr7rAs4LhG zIbQpVEMkq%*I3b3tEc`(cTB$I&^K=XqPzr+KDSL(OforPD(#7wC@b9JTtwc_kyzq$ zEs|c4D-z#{NeucL%cmcr4$F?%1(0wFg99Vpu!wWr$G-nBeybc~t6iReZZv!(LP?S9 zkokMUebyp{qW4Z6M5wAsUtde`TAnk;qybmyy6Qz zSD(Jt>I&tT0I_SjqQEGlq+~s^w>0-rfph;{A3Pc3GzxyfY@a0Ady&IU-$P%=UbGeZ zugDuh+z@qzO8LGfEV=wx%!YxA!}LbYITEZE;Urz5yqQy|p$$?(XYQ_d`3J6rhw4al z+i?+VP+21G9%hK4v-5@9rLtulE8(sHqM8CKE`xg23`!YTX5XxIrGL`UI6XsmEDsh# zO9J~G{7R3A04Yjw9gX@fx5v`XidPgp0nkHN8t|Ci&I?~=>5{lqDC8|;2J(25BhX|Cwf74CzvAT z@$x6oG6wiM(De_#ldU?G#)j`G)g#CtJVVU?Bcf?oNmgf^#|Au}8XCX!4Yh0D>KWr~ z$#;~=kZ2s=`nz+cCSK3JGx>UWZT5gbicj`fcjJ#rh#{4WRxByquNpv}kC5*ki0x6N zxWS#uo!pZElD*@-?slv|a`rJwdT}m~)7c(kKJ7Gzfsa?}h3AZr&wmAez%x}T*6jz3 z)hF#;m*FN^!-t)Sk2579z6G>{9gzJCGu z&%bOmz{uJ`63&&>Z`Sq9(Lu(v@#vxC_zZv$$n2|lUcNVGYEJUv&(!s?qPib-attg3 zU0n%IJVtjV?(O?R9^1^$e@rUps^Kw4Lqxf5{(u7zi7B9zF2qD;L9oE3&j<1uQD3nG ze`ZusYKevJ;CmB0`A6MoRvoq3a=(>^He14IO!|0w8__=9h=9h{1udS>grC2 zpLNSym0s-L(&0Ki4?76YG9pWbbl1P<<@eU&(VO^VC?=^0WsMlMLo+dV<>w743+T!= zFwtD$Ky+>8%OLSD51AWg6IB0LZ8HZTCI5)7d2a+LuQL8ZW-ouK$_v}iykJyd?pc99 zS(iM?S!uXm=@T6rwh@fASkIaD5LfE*Gu;?@mUW>Zb0zCP2Cr~W^e)_ium<|N5Z#0v3JxY$i2hkp zbd=<{Yc=8&`ZKkaVf3DxhFn)kfW8$PkJq8KTM02lbGNP|7RXUNbBIq4Y@G_CzM^IO zy02>J<`zu)Rohmt1eR*7!dQw#33>~>Q}9XuP*CItG=M5MtsJ8 z4c*%DJcdB|AznH>>d(pn84!S0T>tVRkx7hy{pvf5Cs0a4RM!Bv{CeC%Y>6P^-6aysMT; z0BK(ps*sJ0sYWB=_xOpnNa~R~%X#(C@bCUJB2pRf^xC*mq*WBVuPyGDMi3tF7v4zQ z)0-Ukw~s8Pvv{4OPYi5B23>W4;^buF_Z*AM9vKJqT_;p(@b4jgq^?)A<)TxUk5mRe zjnNQrvt};+{K(h~;9FpfwlQ z`x(i|HOj%c9=eRZ_|BtiC&L25C2L3OFf;dQKUztBLlxQ+tCmx53=vSjchqS(?wv$cDvxuZx zHmd^qH_SO(Y{Z9)d`!FY!|jexZOhp9!4>3;FoS8i(18dUcLAw}sklfbxW{X^?pRMr z=++$BhfZP?P};SX;vmkDXRL4AOaZl<(lEw> zrb}~b9wvf|jh&=IY6*i~qRJ)&uh=ExH{4`VHd3yA$iJNLGc|k=NLOeGr6qLO`|5MELN3%AGX7j9i63pp;V2r zCsX<*cu$kbt_YgMX;-)rbfj2X^h#{uAV?ADY&J{+GXkyb zYvVH9cPhcGiQMfG6#c76_JtEn@6k4Q65g~$>TLX=`WIG%@XW)LJKcqA)`FRbcaKLb z9T_fwg94-*^LrD{3CVSm0uQ_bW;?2BP<(g7CPFc{<G_~8(lyWCWl=+o2GUe}% znMz`l58-+ixmtj*v#XMR5kHNP&>C|Gt>|<+<}^}AkXqIR`>b-tc=@IKL+w^X|T{Oz~051A|CDi#gSWE&Q80LRSu;^)2dGp7~H~( zdl+UEuHX}K>q=DJI)F`%WDtm2BKntHGSa{C(sG~cVKPy4n$fB$t5$((5vO!Vh}pWt z=!$ddF`9*Rr1DF3&hNkdyGsM6PAKp4lp0|2daBFEhqraImo(TPga&ly!7uq z_*G3`e)h3?KTs$`fFmDN_BElu?`yGv*$A49VIQtM=vTXqQZ9d29C7IA!FQqmw(bMc zw(hr7a&=p!QDKR4DAaFao%r>8H_iW6-I>27ne71_Q7pGa(;~NPCFY($D5#YwhzkNH zo2WgAfJ-QZicp%GH&fGY*ECCQbZpAfa?IT+a~YRbEtRX5mQz+v<+zpEK8Nni^UVDN z=7)Qp_lNVm=bU#t+xL9m@A-TINgKEjqhi=nsZ@c9i}aHSfC@vxLPtf%`Uj{gWE&qq zoFZaF0MGzjR)Qt_sMe++8b_n3dI(F(2ND%vdlSHNVq3f@iYLVA|ly%vQVPH0OYX(c0ru2I`MV~8N#2X(BJ&b5Nf?eImc36#!U8#k2K=$HR4pwjQo3IO ztU17@P@Fek8V~rNiCBTJYob&IdI|MXHLEZVO?tfW}M4 zCb6P;0548Yq@XbY0j^#`R3rsZ8St_IObj^&xN%nfINT*k_Hm)e_hW-FOfru}178QY z<%!=H#8c_;=rTM?M#S&;5k>$nY<^6Mi%9}J-XJWS<|6Y2u15Cy zb3o_!0_Kk3?f&nAN-`#kWjLBx!BMI^E3lQOg30QvF7xyyv-k_t%vji{c8dC?Rc|02+wVQzb+x|*(gK}3sooft9#nE} z20F#%YV{;!tWV#$NS?q?y1L=tJY3ISy^!A*AEqm6gOr-xx1xHO-?Q2YA#OpzIbq4! zPHXaxE%dM;CRJQ}W3C4DDSOgjIjO@zS*lC1N zn;HM)aQt%y1MF-|ZBg6dPj=$=k2!fTZrSOe*sroV9t#`SC47WHKe*M|pjPhg`Y8NC zaNhewPnmjIxqLsQZcAO`2+j%I$r#G!9cT-@rypH+Crv z`%POXG|sZAvw-829&Q$&(@)j-?ykwR-JX`1OUsa+DRHNLM8LnAzxZ2iXBiu;%+0oa zuy?8|{O7<>cFdfN0eRemlMt#co^=Xv#VpW#;Pwx7j8Kp0?OHyK8+n=;Ky8AW5 zTp$0EC4Umr7bcr68o8spp(ZYP=$a94Uhl+)3?}T~5e(^T0htPuX$&~$-0ZuMR}P_k zUwP~7_1P)voub}$$AQ9}w!93c_50%t6GY0SR^R#8Cz`{j=(g6TV~V~Xo-ARfZkp** z)LczcwiDKFWR^!vnfN>F?A5Cps><=<a6@wAGnYfl1=eo zRQ}lNcq4gggZ2b(erSw}Bo#qC;PTs6XB~qw88Oqek%A5+DpcLxOR9W+9ML{}nwX=+ zr;6qaHY5G$TAgRQmvgaKY`PfD&l8ln{ja=>+8nYEo(Z(W-yZeBO)xBer1>F*NC=EF ze!*(DuAN0w)3{}CC7;-|;yrxhfFZ-il&^6=3}X{`x)}5v*8&UkvXM0{j)Sr-1(sJF zU2A2T8#IwYwf|XLX$-fQA*mOcTAg?J)o$q3c87Vf&7XgK_p0$VL^lda*1fho31gXUG$sVgq3Et*r;Ak|ZD6J`485a<97!IgI;Kw7HJcN|Eqv6EbYQX|kHE luKF!C)0tn@nBAPk(lkiN(!oytxxw8K5d4S~Lai6~(C-dGW48bR literal 0 HcmV?d00001 diff --git a/data/pictures/gcs_icons/transition_right.png b/data/pictures/gcs_icons/transition_right.png new file mode 100644 index 0000000000000000000000000000000000000000..23c1a9eb0a1aba0cebc38c403156f58660c83263 GIT binary patch literal 85111 zcmbrmN3QHzmME6LBM2Y(6A;u*pwy@YJtLa*p1~l4wj{)}_q-~Nmt<$Id{jPrlSu(HcOMMFh<`=se?{;IivJmB5DeZ(>Mt0DM&JG8Xa4#b z(0CtJ+s1#!MDi~P4D~MoivAgA;hSE)mJL3)3-!NK#Qz50n7(gwKTos@<`qet{tHf> zw`lUi(Ep5a@V%{?96qExZQkcCTsgiwXc1A2`U`UYgGMJV;<5UxAHGD{(;syIj2MsOd;Md#;@$r2?x#-=8~?-TXM^*1AU#|C zDx1rnU10S0TF!Pkub$rO zSEp{{^7aJwFF5|p0~Gh4zxOw2jKA^Be`PqJ%P(q=+vNwk-wo5e|Dx6B_+EP8!FcMw z@m5sZ$=kq`=dC~ZjFVsef0hFbH0Qsl>lY4wU-Z{L{(Z>(4nHXeuARpnru#MG^=FzN zYDLe^=W^EHaZmg-m4AiyUv2RZwLO0u`Mmj`!3gyELFm7~`cq#14c_tZC4T-LU<2)a ziH@rKB6s$LVg*F5$4TB=@2Q%7#gY;p4mcVE69D~IE{u2}HK7W1*SMgc{EC`0VJ58k8Pljdg z0^=~O!Z4s4L~Lbwh6x4s9*QgzO7JPOYp8G~qjAqh<;wewU)Q_1-Qma770WP|9k-Cm zsg~K$SGe`5?BPpu-o0dBW`jfC2Isyt=bZOz^m_^;zF|Z|gV$Sv5n$-|f9-XSzCIb7 zV|0_5FFfPGhDw2DHUUUpyxbX?fxR*c5Br7BzrIq1Wn77Q8b3Wn=5Kra^tNE6m|<2J zgW&|mxI;VJp6`Ekq2wgXmp@~y^DK2oEH4}=I@a(xG*!63Or$qm^W@`d(q)+63!O|X zTI!$+cEFo@xg0@A0#UQcx;A69aq5Q6d4Wu1hX#9f>>r<22fIaBbQJdWuV((^SKw8V zZ<^4R()AJT?x{fg#k4L=@~%O!i7V64Z%+7lRknINE?g^Vo>hW)gjd%zUr`8sEQDd? z7h*gZ{7zayMkxzw92JvUQRoHGLBCV(_<_|PkAsxp^V~W1RCx;HwwPB36fG^yT5#k2 z`|`wM_6T%Cu!Be)Z+f4|q4wAd~yH!trQRv85JZm$t)R40hU!OQyF zjZWeF$WKTX6Kw=m2s^2?cc6Wg_~RlK3?pUH^i&xP-6Fpmo7%~ zEyL1ryj_Jbrb3qO=1J3#(CiD=Yoo$%bC;sl62CS;0yd2K*sMsN>dgJyHUIn#puZV) zo^M`cOvdZnai*fZxQEy7?oQU^%=tRKDD6sXX(>p1gQW5Vy?W`51hJjlrYa}MXOuRhaqQ#ZQy5;w#gZOYh7cK4g#Xie5uHs&T(ad)(6k;a7i zi*IsgH^abEl=OrC@~Nxc$A_!FDIv%WU?(dr#Aq=^=nrNMn~r91Cx0+uvGmg#h|zjQ z{>SWos5*X+``kOL1g1PefMWC;c%nM!p`~2ahp1Y zOcp?uEX(0LZjqf~ZZch|oT0kalJnD(N%3Gn+Ti%8{rEa@MoO&iUc!5zzF2CV)$2e! zZrPmynG3+G7ge_?WVDccC#K2!P}80}3;Y(KUrUf{Oc^T-!Ea(7v1IMY@de}$n;Eeb z9jKN)kh^?)SOI?3T_tPZ z0FU!3IE_q%&`Exv&OT2<&u~7UuX3Gcy+$6E51744x2dF?>0DdkU%G~-h zeBAKYO(y~1LgDalXGuRBkJ~0_aYnKlP9o^p-h0NV(z@S~+ruBCdW7AiCGWLHI?!{D z*6FLBv@xR|;W~j-|8+^D*{=nANu!0oM$S#{innzVuUYJ__43lkxn?1j+#JJ6knZ9a zWee6eBOkN5UXv;UI9@Z1~)Ky-RO6!Bgdx7JlRV;B;uoUb(q>y;^x*JLaw1gT(2JRVDw)3K z&+bVKk59-*rFnVoc0Jx1>vg^58r)A?j|_XHFdnCbNJj}=aa$_N1tqR1=xocXFwM4J zw}XX!E{H?`9#{(#9Qzd)FYaUI6n__A(>_hxD*=2VhKM=kr4F>MKsv_;LFf2PoTOh| zN+S3ryjaI}{eWUSZHf|Uh|5C-Fj+tWV&-j#BR|sf*UU+2Sfd!x&8az`OkvsgDH}m1 zi2HG{r=kusz?X7qZjy5&pB1;NUp!%2K81?mF+|8{N9XoX*LV=xSj3@Ht8UW`aPy;m zVhtX$7h-NhFS__z8Df{+tC}YWea?(DVtrPgGPVZ!YG`-6+-a=|!zeEBAdh*#wpAal z@k;C$f!X6i(#H%WN7D2Yd`ASi!Lor`9vqGn1Wahuee}ASO>qULe^Z=(Q<3OuNo!{i zemI>(t5KLRGI?_MF4LFlQ~1}e?)qqW-7BRH=)(Jnakn;f$QdPWeIr$7iyjY?{X8vs zvahI*LnoU)2r9IFZG?c&NI zzgj^A^9=%zSe`w~^ETe^i3VH)_VI={bc+KD-MfZ5g^hX(WyPX44+)U4c_|R1|z1Uw7O{#-$t>0 z#NVwbIG~INgWa}QC4W;n{K5`(Y=5?oB7V_ak!z!u8|w|p$D0kOm8**1J^40qPkvJuM+Ci4zT?7jTfsCCLB(Fd$skZVs6 zajXfefF2{f82;{jF&V>IgxB75E{s&-n1F0^9y@!zxI1E`m=ADcKAi(CM*tSP*w2~Q z5F|+*F7_~^8P;f{$`lIDL+4#W&b~L#F!e5}`xEJWJ6zgxt_zyYqKDvZsa<{bmXerl zbDx`UiYJhEM7}jlG0iEKfq?w$r3e{@*LaCpxX;>~`uliw! zcy7z@JvSf0$f_T5H$DheoXlF!2k2Jum0u;Br6(3)1N=?4LUENpHaUu?89T9)E~D#| z!ym7kv0-dLC?nE>b;}OR$*8Gr5L6|qQ7D3-aIM{qM zv4Ss4RXlfxpx4WKD#=+*8^LwF&qwu+luFXJJ||-sFUO=0P|OrPDs9Sjb$h3latT8+ z!p(tJSmWU)DdK4dQuuOUu><4HDv%!S*zBX#NPUS_yXW-)QQ z!Wi1Ecf=?w;^66=@WYT>tFxE>^eTxWAjxKvg+qRQ-N*vPvB&J!vG0y^2)6w9i*s(pgO z3dMZ=<_X?)%avcepN?|G{4%h$g|*CEpF6ayaraXE*5T@pKPv`v zIbNT>eGbifM@)@zTmLZm9f%{j+(aDfdrvBJKGQ36_$LwO~Frg7+n7G{vaN>c%(L6188M0 zy705C|I$nv7QV=?ZGOgr#d_6l_2Z8G5a7Q-X$bn-bYY;Yja!K6J}$bT{r-nFPqVpH z`xkirNPA~gO6*HuLijvzzkz-HKoLI1jj_r$-c=nwc}?Cq^~g3!oVG*Y!1Pg=xCGz3t@Yo;Fw?N3vUe0pbtz`1=as+aGB{CPn+B zfW-uUidCDharr^uc@JL`_tV_#xy7h2WnZ0AY+=aFI1&0!PGObh>Ly8v+uiaC!e4gG zIX+w~aeLvb7vff6@4kh5p}O8c@%;g6d{>_|s?An2t>aBMw4*w>iTL9m+m%|2aF$RX zJxBg4#Ic9}DxdqcfNkzWe2a_U2=eR+myqs;c5bz~uBp?inb&jbtFWj)?y}57?U2By zZ>H%Uzudd~_b-noy915(2kDJLWWbmwMt_$q+d*%7pKBIQlMOBPzl7)4I=HED3FC+vo$;fozAj=jp_9==Uk6*}?pXs`#-ls8K3Ew}a`W9mG^` zPrrmf=54Q6!1)-MC&r}G~_c)7e~);i4d>1$TR zWFuyvjyJ+^OZ@qnI|0cG^7`v0?$(PNHwo`;?8Fuai|v6lcjqGFOTaEWXfi>mz}2E zKfZXMRy=~-kw%q;VhIhiRY z6Mk~Cx^W*W=EOn!q|~;gg66n-`p(&&(}T!mht_&I!;;Pv*?Pw?doOl$PjO2>oan?$ z*F>z+_tN9jAhpiJi*;)iGb#CIZ<<6WAhFLdDSNvn%;~AxHbI#!xF9W$dQulLvye(^ zF%Vuq_PV_eqdj8R-2G4=!JM|0z*ZI|aN;bv7c*|N(H!osnsD9cJHg#-N13atnV*pL zfXD+4X`9x3YJCb3bmvmkp8F6nk5a2RR;zJAMQ|)xTef|EGE@C3j`$<{=vH?%r{UIh zivyOdK-4{~=(W7YtdGc6ab5cLqPtdG&gjllu{oS&c6MN^Qb%NnOKGBDu&!}G2PYN5 zj!kYOW3xJJPu}j=A-y+Gu6?^_?ZnflwjqvAzn6VjnzU)j!Yt>XSlegoC4D#Y6L7F= zn%x|vsq8BXuZ-WpOl}^RbSU>kXjRS3k>T241t}cK$u7G}k-gLEp8#fIq^`MNGZ)=# zk4o-3Bl6NS`w;dM!cRv%GT`BRM)z`K8mu~2sT!fLn{g5i6>Vq?eF;_N)S@Wt0At&D zM{1w@?>_eyEoi`HWUoR;ikV*ADvv#)uIwSsM$_S(Uxz!LD!p(z6t-gknIe|<@&>tW z`Mn?WCX!_heqIB&ZV3BadGmUJ^D9303Qu;(VPNyaF~Bu|h_@UrLrx3}YJt1|9@o&J zx7N*?I@%nvvtVgWjUW#VLkG2Og}ue|mK2qtPZt<5aBvl+UFEdCj3%AMe!tpzbE!GI zR$AeykeR+kZQcS&wZt1f4@Cv|EAMyBg2$k^uh8iz6yynOF1TofYI7>~xAusf0J}}N z34Ze>Mhhz8>@m0#R|rzBX|PlYj<}h$xj{J(&!?BuLUWH4M@Jq4 zzTPKF6St*GI~Gpth~q?KM6fkWYb2Pbw?>s`v9&N(s^hK_k8AXzy?i zb=Hh?l4emT&v##0R)PNqcl3qK#|H3;=t2??AC-Kf=cG)s#-Vs=3i;u%%8toy-oTsV zcDouZq8i-QSkaWSDEA%Q>tPFTBP_+25Tcq?e$$C)kt=C3%wxl@ow7LYz?h>gT^pnc zX&UQPNBvYYtsimv@C6QbMSh|sq0o}ZZra7@#9_ULE|g}_Si%xBwXPpirZH#jvtoM) z7J8-#wExUY7p;4J5x`4=*+4W%8$kRdij~X4N`6!stsU{SxvqY%3w8$=ZwyM7Jp-0lMBd{?Jmq6 zXO!z0u|AK^im8M1h1NDOA`I$YJjqD@9JiZd*=xf%?>slQzN5K|h&aX*54o0XH%05; zs6p%eo$-T7b%MmEBF9g6j*91Msa98jXlA}&u$SnSOfe$SpjXEUz{Bc`Wt+PBGGy`O z=`$%>fwCQ^j=cFLXncazAdv{s`?1$X?eTHk7{I$DccO0HBkMrAk%W$KU0O+azlJpg zlrKVbeW)GV=(*uB_UWWKL*Sr+KfG)C_-K)_8fBJ#ZqE?mwZN?ALrGaG#my@GUIk~BhxQvY5 z@nI0SgW&h!T_}|~bP%?qS4JrJJ9bl8n)5r1=8#%BX~gIvybo)6HLfK>a;RuTf_D(g zBXi!-GLKSrdz}0X;zbNE{&SNDH^S`D3@0$taNqvr0vKp`E%o|gljL2l8-on&)2vz zQjO>bOh83+zre29_JI-^6eeIs>aa>DC);&t&tQ!Rq}ZkQbI$M7RPyhg(l0|tny&1U zNtJOO4256x;1gf0rrKO)J!*<9sH#cI{NIa9?@YjKa)Xemg7`%KhHJnYL@FJ(%OU0o*MgjY| zk+`UA>OpN3437KPoH>{Gyj?DW;@YWQd;GnL&6oK|123F(=QK2jj2|~MOoOJ)7Qd8e zXhUjOfnzL3y*xQ_kDwHv#eP5B;#pTxqCtkQy4{rNBh&Kv~u%e8haK|)k3HI!&OdDhfq zXAB-WFwZ_y$&I}M&yKuFa>xrZ4%f`Uqb>?-<$lVkIQXoyPAZ0Zf2B9Y|}%xL-dVSZrpX{YE%zu#hab&uV{ zl(Pg^Og`Jyn7@|r#Wk%F^q3u1IdjE0-tmM_?FsjHOjRmDYn@K;1V}{fk!%DR#}FE& z7@>YPb{(@`j5ZiW*2*?h>B21zD&D^s(5`dKM)q{TYee4d%d}Ygr@QA}p}#jVOyyT= z)@GT?VmKsTxx5TXwW^p2>vRHs;=MBxDB*Y4;raFwFI}qwU*2I)_clh5jLj;FylEh# zBClGk3`z(lHq;+iB9C^}`I6dmp@5q#ceoxq;6%-M_o z?&8$G5|sS~;t>QBYy84;34zaAVLq_!CY6I)I3AVtk@iP^w`ZqWuo~H$iEPCDCcKF= zsWx2UP3&t`VEZ)%onRXp(dgS0<27Kg2i*+s+7hvuDfO=sy!29 z_M7fcSu~T9+P=+oy|AJ$GRf~5a{WL@C_8vM6%0|%3G%{?bO}4$Cs_3D7 zR)YmDAqxD>IFOzXv8U(HF}gf^fs=Jo&#$_d#Y1J}2HkfEcZle*&3Rxaor}vogXtK7 z)(5!suTf#L+R-H*=l;61h<2rLp0J!RKV+2V9iy@pH>!i{n%YQO<{3;Bz`-h3^{ zqd_T8Dq_DRcrvk9V>Cap3E7)z;xf$mRYe#LdH(8ZvsU`LtU0i7==pf_F~T ze%FiT5-{D`gV2$|IuxOl2?SNHY->p+=4O_;khp-R1#T6Dm`y zH+Ktn&!N1W8d090yyM|b;-E*M_G{iNpLuR$6Mo|0<*O2^)JbP#0@)*&f$FZOFHmq8 zX}|#zM%V-MxiD9-kzU3X#0w#}mLFj@=&4i37ZzwkRJM;Vc&i}4IowtRG&q=ixZ-LY zgIew-2UN5cPLEHu2^?E5(U3gRky|gL9`D4h7rc^Pxa&q8%vCh*EEYV?xE#w%+jB?* zOr~5R#mBBeZ{;nNMpB|q5UyBUKpzjD5F&lXg2x73%JuVLiwi^7;nj(-x|0|n?rKN# zD~%8Gw$@d5q>s=iJ9Ty*Be>Is1>+Z$B+{u~DU+={6j#Q9#GVV4%?}f7p?$2rN$mcZ zNDY|E#fc!%QOF*No^l_}%W|vynjc>x1x|!ilf4zSQn?|4P~mk+p7tkBpBkpSX2bfK88EN=yb%bPyG96mi+VzFd+rTHhP`b; zv{CSK+lG2N;J1Ne_Gn6KO$(I5S)h}LWH%PXgc5AtK0JZlQK&9i^z=&?Jp?Sif51qM zs8}kW8#-f;`spwpMtbgA$YEQOlK`$)OECQY8Vy>R?WP0&Vslr#(KA1(FNPf);74_* zwC3ZBn0#K_i!78&S6}%Sgk)@0gB;>9QNH22mWH`8_3@{LsH=SC9Z%4`$|w zE!}B(tPnGi@0FriX*NHX?c|eNo8(-AKXMr;Tf6rfl1Y(5zV}HDwgmh#omi-?l~q3D zAvi}wwNfnw3+bT`P{+Gv1p|n9Cj_X6Avq5vlzSf!Pd$xlOxTjPl}m)h6*SwXlQiKI$<|fij zWts>7B~vhPFYhOPBB%XygBMsNU@rmDZXe0ew#+ikeBDJ$H{PGsJ$s0Jzo>{7ZNwO? zf*rO17(ehv?31MFyxmHE>z~hg^dHH)apRGj++cs)uBMKvE|;f!lIj_2zl|Ga8s`0u zu<|1KO{{`_JoFwtd}3U>m~~vaO3d<|m`;JOfR7+g#xSoP6=`Hwl*TC&Rs7LU-T-rOfpQMh~S7Y%4LI#;vJez$OqO-+t{ z9M+UT*-)3(j#f2(*xoU#%@8!<;tE1x-de_MX@T_!5icq332WHSP!g!mQyX+bTJJZK5&vL5qNE+`K*7xTx2 zQeiV(A8wf)#-2`ze;O0n)R)|p=k@n_Si&bktQ$Y;Hx8ztGLhrX@D}7lYqX4pFU2mWl zB);>ctcY)VUQe^W21X>n@XF6>VZ-teUSzPhAj{=DEUOtg{R`|JEX(#4C%e}EO(jEE zn%k3ae^NX!H^pB|D;-7D{7>Y~4J{7&OEM=8DFoClWC*ZJNyj1~^=Uddv<9^a=P?9U zk?L}L6tYalY?Uz^W{G#dP9?;xSU%OJ_eIBpaK8ppFvyR63nt?^Z=h^W1BJU|syf;4 z%bx;vcJ%YVXAO!HUnhkMCqELM5V z3;LZjNcFyr+G1_(8h5UbguPBRB3`5^Sa7h3Oy!^+Fm9l7$fVqkN)RNMW4*rg{Xj94 zjeZHvT8;gILOq_ic<94h)Ikq-2pEb|C||vL-s|i0G9RY4MS*T6?sgiu1arCJ z{8_CnKkZpBHipLNQt1qtd5>s{c352GQPI@Vrg&DM3ZLP3c=1={A|ol9V~r7Lx0+SN z3<8H5$YxRF`My@>0Adwx>tE8!Z3CfNy5xSWlf|%6l1^jSVDl0kv~*p}wy<4hY`bi^ zhbB(+xd1Y~U()e(;6hwCFNp~ic zB1^c8i}u;~oXl|v+3J`>LT42&zHqzbs7zpJf|Q!8QeII7sGb^bMxUlQFMoejggENH;at=L}iN}F8yI4Khzc6=wf!;^Y)(zr%DPB~{tMw1|;?OlA zb_n)wPm+kV|B3G#`X>7fWJt~(Ewx9_h{J=g(OO?u$$rFqTP34bM*I_x7d=_O}Yp{alito*FOn3`gU$R8F@%Cy)Zupu?=DSvD*Ks`Lh5Yw#d@?d_Oo` zQbtZJl7H$>FF0K(-|~q&0IdhXsI6rHO^jy#vtVDn4U7)qG6l4*2p7ck#ei^pI{(JZ z75hiD_)Stb8j>$KabyN^86TKCyk2#@i2)b>Gdp;pDV+Gkjaf!)T1Arz{lMyR5lts_ z!M_Rflg({Fv{#3tKg-R+lw#>ltUea(s%s=^zLG8jEGAV0a_dVzZ~w%-7aGF*?+Q1y z9Lal9PW`cM{*G8}R$x#0aC=jq|HP}t=M?#!#xk{dmM8OQQ}H++)~r^zuFYsV)_(zq1DUhKmKVDp2Eb@zabYYblM8CP?i*2`!>$~Xb^LVF3$T8Tf>BqRzEo@ z+e=C?GGA1Q&nJ#DsZo{=vbT3cH>$>^vmY@6>ruY(N8XM>@+58A^unLq+A+bxV$oNc zPfqJE9R<;eh22^8y8PX2@uw)`e3wo}^`A9F<3#3thBSzdxVumA)ium?`9@s7f=!tI zEeY_;sdT@@T|<^n5mtcX&oz=jQWK34Te#6n&*r|Td2X4mYP5B02FmN6|NRoeMqYhJ z%{^0T2c@bV)#nvmkQY1&ACrOeVht4uj!ImR035RW)%$@!tWb;@Y={4;ySb3fGuM5K z(MyVs1UdePKX56l7eDkdc4}&*CUr5crYkaI?g|c5U#VI1`qxyr{0c6O`M5WTc7Gth zv(yVxCYX_A9va}K=qrtX(uo-+Ck6TCjm})!QexMw_U4QNu^SC~PUK!`;pA=amW~%V z1Gh_C-u)8rWJRL^Y7%gSNZwJ!yUdPQ=x!PObALQAF*jDPe2hPPdw)eMu=@ojeljZG zJ_#j}@XIYOOGgWmaJ*mdP@e_GuXnUp>h*VK;Ge^tN!umcT`A+-M5jKG(dS-NBA5U27SqK>t9xqgFNa(B5N z|4RJ8kIsO$>TAjuYfU=HnsSdPPJwQPyRHy|kI;)OgO|$Amm)62{d59mv+Wxng~&21 zvIgV@R-8IR!{NI}RYx2bn^D8m%a>~|=40IbEwO@Le# z$%3gmSa}!9mFgOmz8!D?+Y!U*4AXqkOcXT;Dz%|Y6Z$1uB2iYe#aRxIy>l;9fn=0L z&2*YP7RcZ+s`pgcV~{@(5o{Oy=8&~L#`H#=sRcM8%(XiPVH5|d*l7FGU0Zd?cW#Y+ zaDLTC7QexwIemfoF>xhn+au?k)}TnpN_>d;ZVc<(Zvp+#v94B7DA!QKD%;sp$g=~k zQ6_hPzpvJH39I|A;D>PtaQu4Ch7!M|d;Oo$eE&?}smTo)$-GK;s`q}gzzI|c;p_Mk z2q7_I3kJ$wkTk$um1`T(^w;iP&}69?x2zJ3M=bv5{(TWpl?C^1KjS2;1#bew-tDsv z?(Z)BL^bwg6y|QQ%;Ul`r^1E*8^b`B^DLhK+lyP_=kJ<04xTCie=v9P10V3TfqMzP z@9?yNX9MNvP+hlv*WZ1$F_6a=pwiA!YJ(ST_a!_OUM;WcMv^n7g6K*& z4zZobyRx4|@%tKKBlYD(e>kPl9>$N&lV6Gzu|}k1-UZIcN5KZ(cZ-#qYJ|(AMx#y{^Ld4#AXRC~Spd+3T#MW=GbLY9k<;dH9$z5~NI)gL@3t;QKyh5^+5E9(A+WG~g+@eHg0l-Bq zhI{9O&=eU_h};3a8u@+AxmvoN0Dd+e^*BMj_{WEE%|= z_tWim)`ag;bIZcbhDyfBqgSY$Yd3A9&yZHC9sDk&VXw{NN_$}`xB24%feqWURdbdQ z&CTC7$-E^r>ZPI+dRiH!)TM|V3izVvLlDFfpRGOhcu9y(-$%*QRXN)1?j@gOFY&uh z#|v83T8j#aOAzl_b#ibH6W1NE=W3{7_Lu@jSW77Uq&5j^f!dI&soMu03Q{(*3a509 zD$3<6oL$iBjWMD?J$=DZ3iSE)bd;U+LO84nF$C0F>m6XuZ%(*J{fiiS=}wdZLuo^O zzu&Pu2qqhDhN16c}MH)n=Qp0Os%}BN3`#7K)CCQS~1bf9tBu1vJp1RmizP&lYGH$V@XZIHnFce9` zl-#h92}h_#qy=rk8I|i@ysa@zzV;0}Ia%zbZGf!yfSNQYIp+=CB~~Z`ajiKSc9XAa zB~Rt+dwV=SP=>HAy|>+!9qYWpuwQCGYPBBakM$j1-m@hNVc!F~-kNG!+L<6fWg0Bx zPB|t<;ReX~j3D4#!$y4;@Pb8Jib99~$WnoQZidQ17FOB(N6vk+07_F=q=$!i#0)}u z5-~~&oU}e4x+O9e@K-Od+gAB=zzWe^qJi|R&HZzDU~=p)0+?xitam6@dVG!=3briK zuM#*_C_OH3H-X{IjyZQmHqygUm}>j4`;wMMG4=tk*^-I!VjSbuSHTVVB(e_~i&%=#j}&Px zE>h}A?+ot&o+MO~eXpdh(Vqg59(P<-$#h`gd1h0pJMb_a9R;NK3Zae?upLW{pntYL z^+0lKo_$HvNSJnu;^0X$h%p^#YfnCz6N}6*f;M$hM&88BZ0R2t6?(J6pjZ}WDz0eT zc@ACFk_EdMcz&GE(6FnzC$i<`gMkL8IR=xYx6gF)`{{q zPg9*<-s%)#%CO-4_5GA~-=k-%?a5Yzc=vsZk#165(-sT-7N(d2C4h_V`7if{RnKk@ zbBs~v7B5q$!Jt_5G|)f&`oStDQNQ9-{PRV`+4j^l-#@@%fT)`dS=s8QK{YO~Lhbb4 zUAOQ78J2_*C~#;AIQAvy#MGfQu0f^a4+Q*Iiw~J-KSBaO!&+zN@RfFCbdyTobnHWZh@0{LDT>6>vM!dz5U&c%$d{K$Bfq+^98%IBmzSZeB~ft%{)&nm zq|$mbMop|Pcd5tfWZvmhBX&;}9>t&^EC>woafv<|eC}KySG^52LoAjI$N^{CTy`#* z`!nDlt+NZgGgUzH;ZJm8knp?@WS|iZ>GZ+AW@s%H$(I3q|~-ZbO{>mBj*jZQA`j?P_jU#A*9y0GF;3joF=8V z=_7MD&|ale%txwT{Tr#EohA^xxu0Z{Fu4BK(7P;yR=hCJ7sB6UE?bhvSl_hOryCXZ zVNb08rWKwhR~!ziH&nq~-*9d;$_a%W`Gs0u$4Q;yY1|jLQ%umqBZDW3#W87RRrPpU z!Ta`H10?eHaumd%mCQ@gtvdMex=6rd5e5m8-A}ZT7`G9mOI)z<#H2u%CfmEOSkhMO z&7t--z@O&FjSDQ$2p>vm-vkZo9sqxrY4npLlw~N`piz>r_jIXQ8ORkfXq?OYk00c< z)_ew0SwVCv=aDDEVxe((dC2NcF%38)Z?62@J~TVXl5xSX{u920@J=D|+t=!sL*F(P zESg{z;jCrH=4@GLTXLGB^-F`(48uLuo}Y0L z1`a+auK}z%QPC6X3v=B)Rfj7#yHW4S;1oi#rVUvq3k?y!o_wi8AeFtl0}Dc$op_go z8YMCqISxa;0iJ{Og5$)P+r~$#@DpusIMfNf*~b?Xr6MjF>NSUjg6r6j8S56ziGIt< z8H7zcvE=eaG$G2>WuH-6Ax+z?D?%cxQQUGUUzx%fcbOE8`N4&%0~LKDl^W`n_`{EpdeR|XYwT#LLKtxWQa=GU_T9fG(w$Ahiins9xVj7f%$;vR4++@ z@xbQyjPT7jBpkWPu3w`vNq83&>9~t?SD&UZc>GT2d-5u_l1qxzF$kbI@c9hx)Gy%$ zkIPA|QB$iR#@qUJaM>{F=>*Ql66VVy`tMV`ZTpp1Akv700lcBIr0(M ztquJrUf;#tfA3cqdF6oM4vsrxxcNSUro6JK#58h)&!^8;KiZjO{3o@BYcJk4bF;VV zc1jSJ6B7x6FCEh=jAP!VmnIFC^-Zuh*h<2tx*?X5cC}HCnfumkOZt-E0c9=os zomN^MGL6vS97Dq&kXfabqGQTgah~z>Rv6=T2i87)xClz~@|}XSln>H35;u}c4T(-< z=cflM8C(bo3927TL*Ih_#I|gH=<-9v%zVHq3~_fMQ8-;;fs(RHBsErN&SR~SQV(jsyEoZIGtnmkfuw>CmK^Siv(pV9~*^5L&YP21rE_Uqk(jF zcA_t0)>neQ?(gK22G>z-3HsioSCD|gl~&8{W4mIk%pZk=2kERqq>Xh>QfpP#A``#j z#XQFyIR>f=0Il1bnKeq(C+a;kVe*;9m~YiGgqqsb$Fyn6)roVN!Oh@lfB5wdb5du!@KaJ$bkq**}%lZLLLel&%;2S}(*b$#jRE9liH&JfmKJ<)YX z7lk27C$mK*O>VPRDeX&tPO{PF{uKMh_22Wdxl> zmhdnz5gvLolYJeU5pfo7IiS`-EEXs)6%l_~;fW$n6F*JmYzJyRs0~U-@Yy#{-k99f zA~9p3_j{hURlty8)k8`cej-J=396QtM136R$N$gSdo?PHZds#$2^A18$yqWAcZf=o zBr4(8pMh(=wQKixdUthOZ>``$c;HDhj5)^5N<%RP7^P$;m$LkHKw&^W1FgO*$Wh)U zM|&^yHmID0;=8tv=PwK%XJU1nk5dsnM?CN(M50AeWps^57oVs1?#*@*SWeGMI7MT^ z)XQ675FCAQrvcNOvV+rSdmW2R;##ba#Pz!SgrYPZV*I_Hzia7R*3GVeuWM-&Z^E-t zPtOwW^y6jG9G{V_QYG}6q2nXvuZEsAzHf`;xAAp%`@W%1!@1Rfd_1;X0Dcl++A+Bw z`~<@*IJmFn{x(Ay(Vh0{Nv$=={JQKkly{GtGoz{j&l}jhs2&7`yG@Qb`397^ToVu} zx;m8}N%oWi@nW##IJ75hua_pl4tiL9u30a`&g)`6DF52)<3oR+xD(l8mc_@SB5!Oi zAKj9D5W%)bu?e~#klD+^In6rYKWp!MILS}%^B|~RBhgW4&4TOm2jx{ zSzae6?gcY#L%Aik$C5{-&j%YmwFyi1iajsh90S-w?KbTURNNd$A%z3~ZGq4Nq!|QM zhW)+4hl?lA^w>&DmpDEDSaSVp$qC`2|YdnFLKvu(LG%%4G|$ z@1=9WzYrzbP1kAQw-1S*dNn)x2eChNR|a-XlI<9x1rA^PYdql0tR0}^yg|~xHcmQ) zR0EpSO@0%b#-k5KVJLEkzT_EiH7KLyDWfY47``W>g_-P%-1Fc}%683I0I7|gc2_o! zOP}RnBpOehpeg@u2ZF{bjAOPqOtzmxDJ)fe>Md>@1II39j zLeVwemwsxCy#_XoogJFe>%cj~8P)5G5PzJght2ST*F|kpKYO1o;Vh@JxJzGFKOImz zl!dnZv4ZR5gfReZ(n*^Ame{WW#{SM^uTQH>!X>UZkss0mmg|N~KhE>Kc;&nf} zX) z)Zh+&ZN?kMQ~rdjW~`w;;DvsT#&YjqYw9!@%dT?N!%Tv8I_2iuNVvoKEQ1xd-1HdP zG^mFFvDbE$yhE~;4Q$al%lgMWoQ4y|it{f+bo!q2)gg0B7phT$8emD0w+J<6U@G>G8&$CsedCc$@G_%IkI(?;_FyS?3#v=60jyE@ zi2xVrpx?nMKwH(90kS_oPGvO8Tan{$PuG_>&d4zrMX}>G5ru!{qXFH{MeGiB+Y$No zp&zaNr?2_1QQC%^ZOo1JbGu2}#p^A2VIR*@ztNE0DOKtRTytX=_GSu4SocYJ#`^I! zMa|Wcl*u!{aFJVlV5MZb1xs?dj?^%Il3^T6_^%`#fDq+7V(dj;?X-{fc8<<4YF93N zAwFiHXcuo2bUqTLaAKa=GP$r^)}i=rX1Ij6Ef|C96Mwx*g136)e13glZ>pb9nMkH} z;n-jySQn0audGi=XD z4fC*vX>E|ny*kxd?c9X?Ts=QFuI~7fVJYN1cZ-am4Ee8tIt3*B%6NYIFzu)8IH}A%Vrt8; zMTv*e`s6Q|$U*!%@%!8M3X9Jg$RNzS8&fF_c_}AOb?~`^>cOo-d z1TefC7JBq{;f%oACfL*E=NAH8g zd9WKzI;!WBeq0npX?_OJx+lpUIzcJwb;C8BYyF_C$dNSMI3dmFo?gge)E$6K;p439 zZTY+2UXl#1p_p(8?+z!gM}wJ{)n4eao|BZ+;EbTmPF?LTFFb9(9uXJ)w5%+$%vv^h zcV;m51CHm?qcK-Mrm4wUmuPqVz<$L7vp)31G^siIoCh+wa5o?IV!ezq%;8_gFt=RL zVsiZ=*G4JH-)T0@>6mpk0ABM`MHjfgfr@aU!lkH3;d*jA>MC#CV_6)irFrn?x}(3# zz%nCP%J1>tW(TuwHJ^G9Hk*wBx(`Mi&kAVl%4l9rwTv1R|3O6RXWI}QxyB13c6F|u z)D=Vd&2H>XP*XFL{`kBCeBSO-i(C>J)ic~QG2BXQml#j0?VI2GeX~*-!K;kun5gOVc?uFraL&ow?njr-w_GxfCzaz-qq}O&Q!<9JC8)W z2K1c+CWgw6p+}4A#Zi|?J$#ffnOtDDzIu_q1Av=jL)9pZBRNntyFoFt+yMj-N}XEnvX%3J8qe*mU2@xgRjFz*=rQ zF(yW)#Js<8PCJ521k_T@FE~LD`og|muGc07ELF6l0u#u(bgbs;>YcQ!t2pJNrOR{} zfkz9J&2cVohV6HW;E(oEKQLo%5kw}`{RQ@|8WySuqJu1<<-ee!3#qT`e(;L>#6I71; z39bH4+T$y8Qt~H02ru>dlE{P5MyWppuGp^&9|$*_nBQFd6ZY>U$C0kOVg88H;T9DhcX^67F{9MwHlFO=h(oUm^pImd!H+NK-t6Q!)(3JnjN-}Z5p93C9(fp-Zsx8qm0fR;X<05Ys3Vj zWLj-_ey*-#)>}-q#`j+r`Eftc#a*5bw+Z|MUPo<^acnnZpe+}fUI#WJsgV;tt(n~) zgX?X3)~FHHMDbClM~tZW9JtYDBbE(C1gYFbo5G~^*KW1ek-bZ2%7=#)PsSyqCvQzO zj4vfdTFu9#DwrU=oF2#`dgn8S-&F3Bj=AdF@|F&@v=70hZAuHW; zOD=chE#f^|(9)UOqEfO!_F$GIn@6qRsl7-fsTk(#N#b4r^;3)@>_L_RLRPx@L3uGPA2iYSy#K)WlB{n}lqxC9=G7TwiP|i?EI_ZbE z4lM)WiNy9$DGAc{@hx?yX{zDB3Z(}$K-#_7C;ZRlXn}vnyq(5rj~pWi(VGGEK6m}7 z?5e5m@)yEJVRH=x*vb`{9Mu#UNXw6%5`?gz}KW&d?tPJ%W?vHncSObFKSCHq}-y`h5_Ksns zf!Oj)Y=&Gfw8O!8=`6o^tHjk+;u?Rh#Pu3o)*{Yfz&wwftlqGoP!^%l}8{!M6U!xHyt=7!YCO|vAA=1!gECGV|# zwE$SgkU7-LzG%T*ZD@1LUIN8gZQ$o{B(tZB%our2FDyy4?boaAZ*BgKqbT;+NuEe(w{5USI^Vi;>)HB--CkmibvFW|-8uD!>)A%=<8Sen_3^E-(`rqv zz+W6K|B5_9^VEN=W+85|1&^wC%nq>BUXDwi%qwE2iTR#Gd3RklK6kk}$O*yd7f`1& z?$CV1G_U<-S}=_OEcQ!NGJDJMUz@(<(;$S$IE=EqF+xPU`&&A?@6{|ExsrQqtCrsH z1+WaRgTQ-VTPzf$fn|du_$!%^dUw3*-OeveaOq&r9@I5V(L2vXV0}xaLJKvWrD)WRgPE>IR{O+$<;m-2(=JI5JDtA!pGrO zN7^G=%0JL!2Aauz#b1kMs00Pug)YZ^AG;*UbgG~Y|Sg!1KXa4B1*Tp$=8k5 zo8052i+k3J)&(Nik4cDWpq)S5g4}unX%yK`jMDVBcLBslsmx^X`FBxYr6yrvCn+*Bfaf@bnd-HErx2 z@e=y)R%{%aH6Ph~@Pabh`8dp7|qowq_20oCU#X2{p%o-u= zjuX&j?X!AZx(T9aFNNFXFgb+uojpVT9M|Q#!Lc^wBUKiuruVr#Y30{O1x;?=G19>b z5oMkDYA*=>F;yNCz{DKT80qdCLE{tgzf1?gA=F;gl0|6V^yk6JvkXr-JN1uf4S!|{t!2H}fwu7pW6Q11?>kL>?nll>N0aYOW*KyQt)`%FI zPs*F|UeA|s(ie)E zj@@Zmn3G*bA0`1^@j9(L!p+a?gX6lhg{OdfEzc7M!%gt#`F_=ggr5D?vB?<)hMPu@ z|Ky_Ed{|e!WA)tLmvxj&o4_vd@dKg!qi#;iC_8!Uo>KY;7{SjW9NBjx9b1pgF#dUE! z+GdlGQLMl61#NIPh9jEGzw7%bD%7~Du7KW=G4JK1pcIQ(4`u702UV)pEVb=Q9s zr)`B4Ns@uxYH`ykHQ@Adl{CEztwb(13gGc_0I?W|@cNxzznt?80b6bj*viH48gvH^ z_Gci9-9-IH)D0&UIHHl* zoQ)=c+SxE}eZ&~F4UiNq5m*&GyB1dY&$J?qa!JAq49Ob-F)A zxOQm8NoAjC+BCZ`{M(+w=kspq-Sz&YZ4~CK(Y(oSb<(c8oD^u6-v9(b zb|*GyP6FRhs^p)B_Txv$v$@{jh*sd5m-k_${#x1&k!$lQFUfimdQlrgzU_M}D`}GEZIBqlDB|A#+uwPH$s%^EKcj=* z+n+U*W);7aq~2&P$9Fd;bgj;R?~vtqbs8oM2K4hFAvD??fKR1F3_?^EOGmra!(uf3 zT(2*WbB7%JnzR*D5JmoMO!_W7NU(R(>zR1e!_n-(F^UwTt4i3cPGR!sIvylQx|hiE z`B^o}ShSl&`k4S$^fLjFk0+K;d<~y)8mMD_ymoIfa359!)+F4($S;vFuAz;-O|T5m zdTYCI7KZUwk5?EQnS~UyrZL8!b;0Q(Zf<-q^i10%u*#}iV^KOV&5UV~yX|pfoi>%? z{F)t?xuoA9>_kQ=Z!=7k4PdQMUG&Z@q_s%>zFLfgp4=ToBMHp+p1NP&47kH#PWWlm zZxc)CRBJf>S!naLP4kZ`m4-`)rgm2RKB%B$cd{3x9Te?`nxWrSX1xoX{_5F>?EQIy zdSV;|V$0_RdG0gy@)K7(^lG4II|m)gxl)j9j(bZg5Ok3AHT&#uMBkdJGyi9vI<|ez zfRigd&&;jUlLSmr&jUq_uCd+sQ>uc_uv3q&aOo{*KV8|MwOG`n3X~-0{_P{P4)^M* zA*b&1XNrNEgtr@DXGpK<{$Rxq3dm7cLsng2x?Ist^1NDxS^Nl^-3DT(a|~qINGR+r z1B0te40%{kX`?5Wk$tb%SA-qHH-BIdB8g8QK!Eys`y(!q>IaTV*15f33%j#kDP!aP zjw^8KS8_-m0dtWHzcOa&4iYmkQ2;+R zwgt0I>k*)GWbt0bGpD`fY40ENg}(963Ba}d*tAbXxB~e!-l~G#8+~uT76exfB#PsC z&a0sSF`IDpxJsYb!6i?R-KG!1iF(USdIx@ffMrt`E)lF)Pn8nb&1%ihEHRdxXDbJ! z=$T4)o+p93#{K)W-E~OMgC}JLxS;lg#>&f#W=0t*a}mMtwfnBDsBnMBp4knA&2DY4 zFTvN3^EP$FCq^w`zavN3Gl&v>gV!nNEkI--%K$((0<;J4&PKVAFNl*)lvRx$@;0aw4T}?!PTs<% zY5R_MuN`8zYXCn!d5l%Ozk6Mszqj&a$!$NnZ~`$yZCs4yYes*BY4nfmwg1tiQxmjf z!;RHecO+2_!E3RwvFy`x*#qIH&Lx2e@O`(hy8P&-VEBYc9EP0I-Q@n6zK_Z4!(*Ui zn)7X)&UUYjiUNs2Hj!2UTvbe3I=JfAL-ESa1v}0^`-KWIFdlXvY(}wTd7!Xx^AUXK zwZ`g+3M&p8tTG23)>V`C_rDk zKB9)pa)AAUoCe=8;u~C_d)x^Z+yYf)$jG%erRMLK-TNa}j4{kfhrb>l;qxvQNqkF0 zRG(?%1EnMVo6=^c_s1@zE?ZsP-eTl>)$R$_nvu{OpI{H9^I|eDJB#ZzAbgl)3auxv z@)jW5GM};&n6sLcwP)5ra0w>?L78{J-9>^nP&hxnh0{08hiVcnpLG`j$xPBd*ey1Q1}U$?xK zxtf;3B<8qR$-SXz_~}2&71eEdD~Y#DIp!|0C~0ddhHmqaKmZST#V_L1@tq7(z@}Xv zBPYre=nifUP3#fHb3gNwp^+--AbUFbv-^$_!nb+Pu9S~96#N%?!HxmXXhRNuur}Ca z8|F;==Nj}!CsWH1#X%K^*>}1|q0-_7W*`d~a1aW3Aad2xEosa*kl^w-?sc3-b(hXb zS%6jQ(7o>BxuihdfWNIXY$g^-k>=wb%-ob3Rl=uk9-hP5bHlv?V08qF1%n`KRkI$A zQu@9Pn_zD5uXYHV&;4xWH1|FR8-!#CUSox~s^87SCuTtx!Y4MdFzhQBe02Z&7TWw? zZQc$w%KS|PUri0=KgR?&_yNFgd0jiAvH~5ibzym*wSrBBdjAtBcnR|0)zNs|gH;Qt zH#`7NfsnP%`^9|()qtJiUZie~_?(P`^I}I%tAg2FBMI-&Tt`n~Ry>)e5+U4mkS)^I zo&oNi0NK zdD}c{^%MlDo+bqs~qLCTn9o)QJg1{GQ+TNDPQOLKbThRBG8;ALLGHQ2! zPy>9yI#bmU6^c~TNinZs)>6FD=meb5*X8ZSwYNjr`u?4yDiKwibU&j){4=izE z((bBM-3{muKex}^CrGqo>|4oj8QIvR?Y0k>MY9k8%-ji@R^L0qb3_y`$T zm3I$yuNzUo-XTYZJRri%v5(5OElLozr$-%`W&p7nOX(R)4Truztwx@xtpXT4yLJj> z3HPFqb#%CErENF~OO3D;FRP?-v!xc4NPQLR6-Xds1w6_+7w-a2dH4jXK0%#Vqv%je zNBerrdZ6tJir^3^ba*SiPZoc+4ihy1b>j)DY@_s7bvLHcpW(`LZ!E>Oo;Jqic)4VN zJ7QIms?g@3!4xL4Q}EL_ZmAEJc=V&@8Hv$}OJQLYFJw1Xkb5L2mupF&x-=t8O1~eYOi&M-RJh4O)pHde|VFceCWkn3ogF$FprG`&j zW<4kDrfp$8?ajK?hT|=_x6RJO()ToP`|B;5MC`` zI?zh9dDqp8$!oJNXo=7qxGA*s?tk+I&2W$uxtL;%N-2(3^Bu`t)|gafOC@9lclDV; zNj&HbEo?c{a;LaGM;`23#4Fj!x*<5L+R)HF@5Fel{m@Ff<$v`hXh{#ksJp`U%OSa9 z13cM}v#=Dmkz};9FmTjRfNW2+x!02!ZZ3*c_{ijn^MTxpKAWyfXaQn9w1b*<$1)~Q z44I2&g#ALtR;jKT5F{-Bk6$D+gg)Ztg*DD$B^v62h;~tj^h$Wl4Hbm2*vz>&qf7WR z;Zgh64^!n77q>%8>2xaxN3)ayGKF4DQ;wz*t%K`OR;p=2Uj>bF*w0QHt-%S|e95549mnriB}*|3}WGPk<4YAt8vvtD;Op<>$7Y`EDgb9>LTk*TFgXI#utxylLX87?`O_jh>CnGL1o zp%K%Af~9+3iL`??sImrQyT?k2-AKMS!&$_1*WNuR(n9{le(Jg*b5WL$KMbD{O?)OHt;yz$*)n2x=|jB4Pc58MbW) z^4QDPvc#ffExxlS^|l`if{3_6_)UQHMf(=>~qCa@b!r=85{;oR=EeSaHg_ZIB$wX6!!9tma+H3raPvF+o1 zl&`)z^F!@gaFXQ+2Fk==z?;Fkl&!P-1D*#-c{9gO)P`@QenENYg*6r75%h^CF+12; zI3t&ZJgp^1(aC{JoHNL~Qz$fdAQuDPPoMxfUROnZoRA(&AR&~<&L90XK?s!U;%T!cF9UO*CN39@A@1mTu@Y>)h zf-rUs5hIMV1t$cCht`PuXPXz3Qw5Nu78lxO!BnVj9Dm;#>BunadXL;aM^o=PATUh9 zfB}#Bmhjg&?c%~6EN%t7k&gX!d_2t$H3TYnlz$xE)sP+>^~^Y!^%g&N*{wQXpE4`X zr*2G-7K^o0n94R8f;YF03M6|%=uUnQ+Ja;QfJh?BH+ zs4~tfr44ZSwpJpF7S~$W5b-6X`t&Sn;;S@zL!NPfL)Y(8*=E)gW#0kGfinz4=HS*m zq_h~H1P?KE+qBb%_1}zYWm`TH@?#8}qc}6e^-4YH*LvQIzs}j`9mN&X!n%x7;ISfj zez0t4HX6rrHmD&bbXM=3<)g68jED*DGNuf~S+Z8-g)}=EIW%Wjporz9uBObr9uA%!j>=NC(uPHkQKwzK*B`4P8JFA`hrv?lFIo}D z?Y)X+k()D-b!qW#MS4NCtp{9H(Xn3|+V&!_ zYH%+ykY<+|IB~~I){>1euiZRExNb2UNKI_c`O(=7CIJRWzgEhPmDu1w|oLGf3+NqYy-#GGn!-CC((j%+c4 z$HbjXP*yMtO&S2;$A!8-S;^J$l}l#UBG{*f7BwA^2Z_#i<1s+zSjJ=!@eD}t7c(wx zqpMLVw5yscshb-vlhvUk)bwEPC{=(#3|^re98q=(e6meNcI(G3!;ZR{kwSYu$u&(O zovW;|4fJsa0dX)jQPYum3~alPHA$-8K_X88N>*-OiVnkF>tS=FR7Lv?R=kUtyDn_~ zQ>;2rG)I8=GgOu)Q{nB&CY#$VW*Iz!-Nb%2GiSK-6UMeK&^T zgz`JGcx79K-CT4?4_>kwBpnVZlvbBEDX+aj%g){pD%-OVps{Kj8rL;Ut~FEpR3-Ct z-7lEnCUf}^x3SmcLIEj01iar1-p4^=`?jqjKV;|pqG})GxQ%u?F!E{HmCNW1@w?8& zCY!VodV{y%T;`vE%QC945JxH^O1CcVhVVgl} zxOI4|r`}{q-SAHklVW<6zVo-E!PKdEzKTKoWSgse?EKWbjUnU|fc94E@rewDtm<(T zM+gZS3}IfLk2n8Zu#4S`>!xzGIc+oS7d!n)=3#n7$<_l8m#xj(o3m_N2$VxP1XU$D zmtK9AdH(2dYb>;s_mEGNP=u&0H(vL7Y4*;VBg~<{74DO>J-k0r4O0lQ2Lzd1@OX== z6(6_ve1ID{mvDDoDeLYOiCNP>!R!>X?v~W3aNt+Re36UX$E_#1|GBRYq)!Pd$gBL~ z#?4e#k{ewbX_9HFw{Eq3A@R0kcM>v~Xh`8b>wxq2V6yHmK?W%~`;?noAT zkq8!eGTqxFG$e9%N_f*0*^_#l)pt18>d7PW1{8iJETT_}G0KPtb~=~6_Q3=p^44?? z5;jLrNU>VZM__$J5gM9wy;{<>e(Po5e@Nz2BNzOpTXi{U$iCo80vD3;W4>8XV1xeD zUAvfh{eac)wy`3~CpN7fnFOZQYmhb~%v2(d3{jATY7}Pj;gj}bngW76=Dexmt|T{u zh=H_#MjobVz%b5nUQLx0j8iZ(p!9lDEpY6#dkx73_~7j2>uP`rl50${+{7}4!$(j~ zOl09Q4{}H?F*6W!d4 zKf@6@xg8fg1cT#5#f3{Pz+!q+Fj6Ei3v$f3--UdXe`e>q#~*>edvlwKa)tDC8Jy#x zI#&gArz>MKf(RWSv(t?L$5L7wdSRHG#T=-0-`qyqQ-(YX2l4vQ>KScgP9U2srlnnn zvJ25d#d$;Xqs{KefYc990@^g*nj{03&`l}lcD!})hLI5k%KjmHk_iRCysC1z0VS_G z$jmgt9IalhlNpB;6!UrjIJ+5TJ6$@sNROLh&;}$(Qg!tdaH8xTgj2DD4eajl6y=HW zbr;*vw1t3NUf-!m{-6%U@NN9z|2|8V>XSNgrYPpM5`H`F<_e(F`kFDYs7BYls zboOLa92?cC;jYdMr;a-?H5jz0=g6Czhu|5sAwHk16xGT}&A@en>ln4#0(5($d-K9u zO_~?tf$^NtfHZO)ASC^@zIN9;Wz!*@ItgZFHA&9{7GJ=6B`j*R3Jxlp^2;9py0I2_qCS>g6~+QPnA^pfU2*%TcYY<7k#?-9*AonN!q5^Z%@d%O7*2sU^Zjzi!>R-jobP7(Fu zm*l~HY$6j^2gGZDa#%p9x9(#I1yL+-RYqPs%eS-_;hu9sh#SRVh#&s5xSxsZ1g<|y z2tek`MykeUJ7B=KCnby3{E3-X%yqyX`gU|4x{(RPy5SVNC`wMdoKIv8XG3-yy@E3R z=w!V$e^@bV;8kcIWV&Q)GN3uP)B_@4P*j)23-W~~zYbM#d&$Jm|Scr#J{(3G#>l~7$Z;u7V(wtZ=N0g!6GQ!x@o(yUBW;iBb zA}_mxq>DKX&EfWQ$rR=Wp}!X4x#~RaVgHx#>5q3dqms_7!|Y;o4SShg^RQu4IEfRa z6E=Pnu8iJN!#`)}BYg^LR&V8WR*39UkBmHX1oVI=hDkMXzK;4?A$v~h2PDxH$nGUN zvL@ut70-IOKI=@Vpv;!ncqS9)FwsR7!Lz9%Uo5kvOzL6Dt$mRz#CcE;9(Fxoe)I7N za+lL~)FFhzGz3}SHnwXLqCooN<$(vg_lr|ephy%vL<(uORGA-1Xd?%NIZnH!+GfKD zXw_`FmCPQD(@lAAnQc7M8lq}%E}`Fn)@|7--nhdz+#HO3cB47lnTIu>!wIqg8gR2! zE|Rqd5X^g~J+KP-slToSyY6|kEaViAh|JXkV#|0ljAWgA3#-e442O=FA}H5sWSnznQVGC5m#;=SzcJhjmQuU2Xd`$caIht zm8g4oi5vOJB{O|VLoPK}#!JqR>Br@K&OSsJnRCU}THxlLMbi{tf z=A(s#%t32!eZ#2E*5DC~3ZXuUHJYR{h19`DIt0@)pNkS|_6^ z*eAiyF&85e%jv@pZewxGvuFKL9%sm#xDFBFx;PA~V~z-w0gCTKkC&z>*}93hB&rP_1PLS9JB>T%4stw{5hurH0WVrUa6d{JPV->2N(}deWYpw!|1CZDI)d+xa~7uO zHLtM_%mZQvhjfXZ4-E4I!cf#80Ouj~B44lHxHW z)jGLa)$x#IPq;t8U|_m((P9;%)3|rRF+$+wuE>%C*aYI}C{gF*EJoTVN4O60Iib2B zU1uT*%;zNcP?A2^WCc0C2GS9?Zz!gqg%o=1>hzG;Hn&l@BykAOxn630y6*-0wwJZ& zRk{=M6g&uFP%z@5G;Xge7eXuSEY=>|HDe2bP4&{U)VcS#HA-Xrd-JTz)Ddw0nyomN zd4lXGKErmW9lwVxk**df@ej!-xyyr7S_=bw7-&Upe2#c8;7;YB#-`|BCy1*pL+H^+@@Kk17s zLQ7$Yl8A!A4|%zwNsa#LU= zz877jxr1#)R}RoJ+;6VPmA;$%-TdJR6e*Vx7Nf2F5w`S)u+pVTmZQJF`&-ruYf-;_ zVD10+w*=$4&)bkVH;^y)n*Vqo(=>yixVM8(_uhWI?J-pR)y%;^Fl~Jv_;-_9so%*H z#9N2sO?hqZZ91-@suTxZOv>o@^hMaR8A^W}8PvA_?L%Ab)HN+n+8tRy4Kf+ql`o33 z{?nyy2cA|9#bmQl#Y5u9G3IFZv(+DU*W))4^WHoI*XggR`kqW2-dua~@)ZN3rut ze?}=7Trqu0=yGqnfmxi>1xmbhcU_pxPS7E{yWR+ysrrsDF`tX#H5~;=gb338T6e1p0Q^Cm~PH4DsuNmSK?L5c=Q~{ zgGL(!&!s~B)y>sty`v}i7%wO0pMUF0(T%ti3oE1KfJX?{i*D1updDj!IG*gNb^v%LluPTUK_OHxJdL}|FdLkP%5Xz-9bzSp#o;AL5T!?wf_3n zE>{##B6kPG$6D@-YeS6_cjd16RiZ7Tvz}H6Mgp(F@*mCz>D-t5Kf)3raqq`W#u8tC zLS)PNGUl-b6e3{*iI=yQJ^dL>e0g&GYr3hPV2Bz%t3W8C5=`))e`?SKnbpK;Z-(lr z$;a3G$-EU6#!NMUnV5Gp(v)=#;_->vgm6Z0mvt8TCDtmbP@e2o$PSOxeXPL&wKt3^ zU)@Aj&e=mvAsn#%^PI%C6q!yM(>2hXyp2pGB#Kr=Qe@9;db(|g?yd7F$$}-Frt3gx zb3vuJ@g`uEE$4W56C`D&}6LL%*C1Y8PlLd#~&wsb*K!O)|@1ctSZ4qg?tebIhVdjImZL(p8i=nD$%1q7k)vL|SGut?pqj!o@%PV1^*?g(BEO<2`V-pA|}iln}w5*qPns=Yrv%sqHGa zNZhJ`DzUdayQN=^@!;03OM>(EQdwSLK12gKWK&;!L!wm}0&wGTkDUi$I3yZbII2H+ zb<_nR0uBr)Vxq!OI4@fVrSY3SeA@Y$QEhd@O&-95{oIlAE_7}HooIytGHt@z!fW2& zX|Caqg2?DuslN-pJ(7bKk%uN?k@Ug*P$=Fy0U%+WZ9JJ4L{uW`_JRknGMr7UR~8Ni zKr?lwYCu-7sCPs`^U~;?F=TV<6r-9Mv|qG-kbOpZ-Eu0_t_RyTG$w-)BFz$Aztw>XO%*q{ z`L^Yr&!YElSeFAijH~0mx8b&r&|OYMZ4tKxCIm=li;pQZ#T}|UYa(5JU)KOD0qOev zb2n5vzbVw4;**kL_-p)9a?iQzUo+D%n0y_co~WLHFJVzpeEJjVd85J~gz11-O65^` z(NusUkm_v=4Rk)>N!>N6PKT`qNRgvI$b)!fIg@=W1l7qi*~S7``uNm|-0A!h!{cNL z!HJ`ENl{V1QIr@RrBl`{@Llyao>3KlKXe^2L6CZKN80p7HRD(9kaG}SKUYDfvKEl> z82t#A>+AcoIcYgjt`T1#nU*0iN6+?$XNRalkqs5}vi2Rr!9M2pNZbn0ppo+)VS1WG z6|3eD7aMuw$YHsU$@uC2V0`?K*2!N7sO))k_BDw*_Dx=-Y-!3KvVgP9hYGgKH&$(F zS0>D;y_`@OYlzlniL#F@I;bsW8*XUuAFif}1g1=YrLsV-wwh!mhYOhIA4B}V{$}5S z!>iY9ScX*f`n+&*sJRHh+#rXzDz+eHFD(E563Tg=Ij>weU}@mK`~Uvuzx`HBIZ_$R zg#VBY$EU`b&xNJ@+aXKlmHs05i-S6u^{{_?&C}>DC@DhqQh^IXW)i0W@gsitx8wf$ zWnN~t909>v#wq`8*bI2kfA^ZWpTEpgQT*QLxcoU>`0Mri@$$Fjqr1$1j{d#4Ch{NZv7P!89(oicnm(!j(?67zRmWR z87-Re@Ssb9^UTX%4J-iY_&pdP3B2YRU!3(IG-U=0^(1*HF9EE)jNg9C7jHc!p5M=q`ZKan zhK45KLH+!7Ss(N~=D!af1m!>P&mi&szNWR8COm@gD-&?_-{F_J&TmgZxYz@;$a=)u zWq*#I@eZ2(eF?eyXK3TGU-Jhe! zvRDV$Uq^pO(41jP@foVpf5-4BWC(q^0*H^+kNrG2{4lTGFvB}qFE#>?w0_6S|7pwG z^M86}{b{}C3^*LdulL(=cq7L(@vzXTIOMl4@Dnb+zNRq)K==FO`UTHqUO4{G8^2Fy zJ?{4@l-$r*T35RJ2L({dR$+B zJ>c(0P(t$OH5cCz@$Yx}=a1j-{~ECP6_W;^{cn$+g?|l`pOawn{5kq}4E}43vVZHE zpQEvE{Tz+0=BM3%*2LWZv)uk1`SUd|e#Xh46a7x|Quz1#{rw0wtM#G%nP~I;oil%S zn!lg-=jVAvjr}ZO>l^vHy8n*P|M%c^fhhm}oc$T$-~aHMeg4lTC13|}|9_kOx6c

(j}_Uz3?q1dZ_8CxAx*RZr8BG11QW5>4@s)yF{e-|8FR{&HAoqwZ# znr0#WI04=$EezqZxp^h#2V%dTG>a}h>QK{AwgR)+7yk2|JQmr}_U=>k#K-;G`D@q6 zidVJnl`klBhwzITFr!{{EHyPBpTG4Bc#sKd^*wYiV&kQG2T-qvoO%#zCo&jvHTg=> zNqFGkn-eVlJ`e|X>uS)tq)o8ppFj~6t1aM|OkJVwGNGB-%i4VQt;l?=UW{>ETAvj! zQM2uGR;|N*GxTmCX3Or=CYq7!62#iOpy(cF=>S=SRwjTCq$S+p=-gAzf8U~Bfp{Lr z?H@p?pp3Fl?&ia#KvsOq554?lD3}Q072Iu^2dt2Y#CY|R-SE}H(}INhbXp+$P-ldc z#?2;J+jTY@5G9%M0pS8EcDJDGe)-B?E4%I5YG%dye)tYs9ce+WYU4wsM7d8IiS)a$t8!GtPh13l#%T%-~^9!Mh{C5XX& zZJ$h6+BvBeE$4W}5I?uaZ0qw^wYZfoFY6w4V_fpARUA;{3t(Wk$$D{!3cJ-hkk}DN zYEHHY)NF-Av-FpsZH^3$d=+s8Y}N%0zgb>gR4uoi6-TGn46I+wTkxe0VY=vCT77)W zI6z&td?z(f%fc;ktiIeczK}0aLBz;-AJin3UYuB)Z*hb~(Nin8wVnZ+w96mJ`4>V1 zeXtW%EFbT9jBnc`_X4Wk193$=Z9^orQw~*OEs@3b0a2AE6S}RNB|RBNqNzmGA zmsz|#NjGFV`lsk`}1Y=(1wZPu&U)3J@Ndyb{!GlFGJX&ewUq?n>JWL@#*Q4 zjYj&N0K2OmUZwDQ3iwi*bi_4etS$)LJr`B5XZSb0g)Vq|&ixTtI%jD|;;X2qAzypY zQ-MJB30OWD7q%i92O8?|WtC3-{5?Q#|AnfjbOhUi)rCPE?%1bzA}y#IS`GpCAnB?V z6~NS@!1saTco(Ju^MTxxVQX+Yx1)A!#nU#Oka6AGBfJk7Ez}jX+gI1=4VvB485b8%G z;5OK0Y!w8LfI$;rQzQn&*LeiuzSaBsic-}3Gz8Eomp+! zKNy>{ay7$cT#VN?KS2lK2s+MgKhEiEP*OR)Cx(}CATi2`r{Llaq-mn|C%#?za)$F% zHpwOh-(2FJBvgLen3Wmc-xY#P!;l62uZAazmsG33bw7?+JvCkq*GCofhlVGumGaM@ zxj!W2=o63akou!^-&6HQH)!5bPd*(4St7R+^ZxE})H>HSsc7HmwytGdd^tkGydeo! z;E8IdQXkf<<+_(i=XbvAa(w&?3p4p)Gm%Y=R?AG;87WZCvxb5wu)ecj9 zbfle5zfVJ1x0M8Q<}hR>*GsTVo`0*pRG$Jm^FQ0=***Q|FT+ztLl;51TYZ)D4dZG1 z4XpVzaQUB`#T6U@Q1Q>L!EkULR#?vFr|9*lQ{bxoch z<{A0tlaxGl_fmazx4Av(XEub&8oi{wl5iYiABA}dJz3#PH*IFrvn9EAdXsqr;wOF7 zGqS%TbA$?f-9U;-bj~N&B#9_c;9c>iAIY_99PFz0GbzaOG!pkN+nKfG_sX;Vos+;$=F8U1WxoU&@5AtC$6W{#gVR_Kn(N;a!ok#3*%M35|yqpw#>e1u>n-_WS!j2TWPaypo`t~-ux9(98gj_m2C%6b(KmEEp;6q z*$a8k44RD|SSu8`_~|kC>JM$WF{hK=L_A&dLI2^yM0{b-y?nY85WN%*rhobE_w`ib`$L;nTG475x+Hzan4{d!p;s^UWtK-1)3Ozi3$am{>tK zI;SoN@s<Lz)Er=PSkr8?#9aAa<*2c3OvdCO>#Nmq?(fl@)p@q`VzHMYlL_bq<% zq9H!0g}5S%jtYfy`>5CDKl!hhhwe;=q;5=d>pjH>c|Lc==?o*8ZHWQQmWAe3l^5a} z?a_;~hqxEW($?EkKR()ZAS9#S_SOMvM|u11WpAqc_0fC+{?6Y^UXofr%Q=SFh@oYYr(%c7Xj z7>`h&X#dDuYu{fw9zRZJx^*yO6c5j%yY=kPtUnKyDLX*gA-wFE3EyZQQx)b9qE(CQ z_vS|Ygh88?^E17Zz?mTli@Y;`?>fDiK0_TAo8i1Mb4e`pb`bHM;Sj$r8ht;vMP^zC z?S`QaCi7`(0y=)s`tfa3OzpHHenBbIPwgDh|W^qbU=y`F4QVy=dgN} z;1QpvV)(>~3ucTl%`x(-XJfVbpjuvNsmC7X*!M~D+syWeH*ugMcHF5h;i00GXd0iCV`bZsvT!!n|_&tsQFY2n$%ZzNMW(y_Wn#-_U%V@yN zygY8-@JG2_9#&$imu#MV$*%xV*B_WAgbd^hJ6`7Csd#Tg!&APncR#${G?3dn^|ngF zi>bJgy|BVVK*NP3U=oJcKOIXkS|euixrrtpN>|-0IkEb2grnxLUm$|+TGPbkDel!U zarjlNEiaKp;g9W>zKs)_(#0_-tV6m2oI!ZECv)U9c`<~zCS3cy|NP<~6_OA1(9MY- zEz@F}=xuL_uFQpG<-*;AsHWNgC9mZpy$2=cZR$LApSJ|!X#_^RMYAMx<24aFye%%Ae?uBTsO083pQ8c|!x|UTdwb&^ zOs?tl>0z~IR?yGL*m8>vo}(@cE3W1SMt!0hl(9L{9hIP(x^RsD2#jTx#!!R@{ZufwD_o|kZ90z2o2d|; zZJb){i~WgM$=7qzSc_{u_-@VH)DC0P zyV>)MVJB;1aXXg$6N3a)DK115AF1kEb6jgZl0b*_?(NYDdN73C3G#7XmG99DmB%X7 z)XBR+7jQ#_8WeYj@=kwae+`uK5Zu;17Wp-u(NWulo@3WTBr+jcezmIl2?BIyj5j#$ z@K-1jC;UE8~+VgY*~6WE|J`vjF~-goSJ_L<+_ z`E>hQr^S3rFkjV&uS&r`_RH}Q`(kfCMIlS`Lr;|+URXK60ootvqF#AIzxWrqWo7zd zjxSHA&&jos(>|QE&%FH#5Mgje-Vz!qb)*C0zB22P1=BCs7dXGGo|Z=QG0v!CN-I36 z^EI?4hP%LX!MVJojWE`n4|w@ONUQoGP75i4^i?dyEu+GAm12TU-@zps0SpneXa&V? zHpZgvWMxP}35kOKDSN zy@Mq1fu|Oa7m#qp#y`GX8L_$zsY181tLK%=kLB+!-5WFAo?*19&CuTI>&>yN$!drT zbrxZW@S^MqkKHjpzAaK0c3e~CR3*=m$RavfwQu(M)onD$rZ%G>yuw;R4KuKyaejv` z`?>UmG>M7s%KBQqz35d-r`$+Q;^8!Y;Qi6y7q!oC?RK~q_)I6(ZYKJ0I>PBAijMny zmjgC(2oi?AAPE;8EYh;UuPh$~UK8by+@e>6kV(tlAX)SP)=CRtFX-jt?fWGPL?7}~Y`inSFU3T= zt-m{20m^Skf_GxQ_Q$vL5p23|i3dqo2Yba~Ea0G>@G0`fO?}6i{Y%!6lE+P=w?(>K zIOScuzS3<~mwsBgqB3$>z90iw0Bh`Qj%v?84KAPuK`b7TmJ#_E6uz4TV>Kq9irg$_ z*>xZ1gLRh9?Pjb{LVM zbh#p(5A2~7^7!z0K?L&aNX>8|`^(Ldt;`VbGO$FS4{lrBd>ytzs!dEN{&enKssr=t z2cKA8fjtWNoFz|h1F3`ng)vA(J=^=AD+LkYwCH!igL2ys_p>aGNry+5T_KZLR?4-R ziFp!up1oZ%!LC5x3;7F>7~h*iD)mH&sSBAH)9ZMpbH&RZOoDR|AbL!gK!EKj;MRB# z7n6Hr^tu&pr`8d#{eI!W?XvJY4}?8IgTsZ_JGK&hm`&05m-&f)w~03|K4s3hvmlG# zW`rdjpfbtEMHlTk4~rtqPZiB2vd`OkO`l?T820qZ_q%6B9*O5a`F4$1;jFH+3~&=a^8M4QZUl4p>jR-_R|6yTG`u z(Ck~UKm92Mcfpvr)3Us%Pn+iJc*>EmC1k;R(2!PEPs@|R_k!8)5(&ORko>?t@Q!zo zZ+>o-Ph=lQC04scXgU8F0@z(t)A-QA(>Q_^z#K`LyLgQc1{zgg{#f$>66dGOJTG~}sbIE3Pmh4*!Fl18 z$5Z^>HFX1q=^X9C(24_RXjmJ%?*NPGi0qqfuEW)>f4trnLdPg^rWp1un%kLbKwkig zNLtHAqBhe;D+P5PH#!Z!teT&xiUK159L$5S5i}O%KUBo;+`b(3OJcIp=EM5?1Lug+ z)TqOGl=W&=%A9}q#(D9!^NbK0{u;9{IrbUeLRPIE;3%GhPH{7R_nUGrCMAHPZ8Q2* zP+*js`b+yk@nv5`pFcwTfXUNmKgMHU#!j%DH1-pW_uNf=;UZa29xFqHsUr``cIBm` zEMs49{^Z}+?gSMxxMKXc7wM4BOa9prb<0f0y9q(}5n$Ua_R2%xEi1osSPeDAyx60# zj}6hBfGeveWZbr9%P(cQ^ke5;uV+GbW>81ppG@{lerClws=QZLPl(GDoSjlT`n@^! ztb7Bz+d?R=4Mv6`=Ug{@ByG` zzZC}{vHi7FeHmF0fbDucZ$ei6^*(sZp^l}O2&s;h*Ov|tnBerFZQ6-q($>jLsj?;g zQ3BC%J|S6ZYUfYs{wB115#mPq>+9)gyT-YF4kXk98hR237JCRT%0xbt2`BiBzRW2} z$}6GoQ-dTzm!^s5}IoO+QL1=5IdwTAuBFpA6;eOdLq*7Lc>QL>(ih9`eqzQ@( zb0>{+7#ffT`DIV0eA4UFP@hmec>0gjR>Rx1zTQelmU}|DzGa$)()3hO zZsB6$m$_4Yd?AE}SCZq{%So%{{mZG4*s93bl0FKa7q_(e z2e(w23Skrb1em1~z${ak=sUV!cB4rPHD6bcqd+`AOiMf|ONv;xz6gT zvq_n9oVKh|M4LOc!eKAk{EU^kBsErUAnh+GvRt>uWKGT0)UIkEs7#&^lQzD-3Tt~l z!(r?Cy?f_`%F6z(;{l4>pEW%BiO6;w;cFV7+lITJ6T2(wCa?wbtC$|mSJM>>Gq%2yGN`+MX*z&uKwvM^o>l-^h z7Oo_+t7A?nYLeFII@jl$#L0M!brMhMYW0_m(~CbdBmnmPJ+zvnHc){V%;xBAu(i%` zBZu(`(SmGl>GU{+E|dLf`cFp;zD9Tg-AjuQHPVC$)xfl+hiJ?Xq5pIot@`p8@~0;! zEnbh21ScfnjUkO}%LX}6O{n0OW1aQbOcX}FW;0fVZ(xtfyFg!xX9KmKF^@R-#MLAi zc=XRj_<49ew1vakW>n4?bkoXx=hsc#BkTZY3mXFt`3ieE@UVHPu`Cdu$Gu_c%zIy> zhcCDSLJIl_6?DdpWyX4h&^NByIp~jQK~edDWCI74(8s9#sH1Igt}=$9pi-znDmPC@ zd7Qg^*xPB#C$w|u)i@E47Itl6b)+lf_vFa?B=a&{A+<&RQ`1JDDU5dv*kaG;FMZ7x z+m=Ge*(e5WVt%(e1v^J|!SGjfBm+-o=t1q)v5T4b({eQbH(agikh^A~@(-CJ4jg#q zcsTSPL|?QMv~l4-$N=^Oh{!30_gfwtADLaQLOL@%9UiwRQF|E1QXBez6(s; zkkh4Ezo7u%-k$u)gSv+)^a&M*@K%C_;|>${pbq-R%(>6)uOAB_xJ_s!`W>*i)$ zvmu%w?LFI=FQD^$cz8{)kcO^8H<%%fHyG-%=G;|=)sEmSkNrT~6Pd+lLL3-*$(_Tc z{?p-&o zQ)IOto{+z}AbWQZW^Tj;uqoYHE zdY~K~KF!%T3*w&i$2;;lm~FX;j4h9DzQor&M;T9puZsKxAYSLm-Z2)H$gAw`a zJ@aD*D_{<3Ru9@6V#ai_r>L&KH)82Gb^Q>|A*vA1;wxM;sppf|*GJs+n=kR)d}OZ7 zM@>A~4Ae-n1cu(!IKFmOej)MRb#Y34x^d7N26-(;3YPn9qh`aufYv5^@aMyX<*X@A zMRoGpTP1phfdc19>le!G4xIM}wYY=97voQY58AqKT+VR=5AsCrN1qL?jr1}Y?UoF~@ zsCyswJcZb?rUX3aw?Z8eS1c$F90{b0xG(A*`Iy_Y!3$<^Usb5QJ^QEp+tWIw@x1k* z*J@;J4w)d0sIb|O?oy}kSn>sXzbabCZx2>{Aa@ZkhgUA9J?ECew-4f!)IWuD zlkVH(KhZSr<`bmka%kjcmX$Ny-nNT!WYjM6U%$`!B7Pp}0r>g%wEsff1m5Cq`w?y0ry~yxjB@AC2UbsO{k$IaOoeempy%4?lRf~W%Kr(`x3++@wlwZO8eqWn<^(qx)`zoX~%*e zdYrCz-YkSWZ+X={e16D%m}!*LtaA;IOd0q=;f%n!YOZ~{B?oX8vhP!kGswyROf7{l z^J2Xoo@<_9*Cw4qNe!k&?uR*J7jDj)KZiGS>wE`;U;KH^cp+ciUL+zW=}~!2h4V`Eqo;U zTeC_)xJ)EjcXvNzwxWhFAHfrrAb%Fo@xh@_6U9hKoChSLf!XsBeLKfTm z6w2~oZv(9{a7sBmu(0x@2_LV?#R?DKF}-WCeNodQO7~nCtGr>!sYV2dQ6gw)*uX>*N(k?- zTHkkxBnWU>5T=gX&wPXMB-D9$_WTFqA;nl#x|1)mk89FBV^`r4R}EWtE7lWISMSrQ z6^XrXJ&W47h1%`J9eqJPFuSk)^Dd>u%I|$8CD1^h>};#3GwFx4u zZ_D9=*J!2K_UXRy%f?E3h-o%r&(=i58dkyC3B3~gA?6(;yI-6vR^zu(wYsf0Rv7s9)E!_0C+q8uS|y9uxd zw7j0ZKuU6}LclF?a0;%|cp+k%3MzjeLYs`i|5L(6ZR5WB_bqWRdY{FE{X>2JNL}M@ z0)EJ0p)}xTU-v!O3~1fc@5qwZPC6+1D-qcOgb(9((muu;g8zDY2Vhe_$+9FT!rh*& zsH!vCn*7`}VdA9q$b%LFP&Wg%Q2$5skog5w?J(&xMD8J4l zda!SJN<$((aoCZQj(>^Tri*EODs0%VR94?ZnV3VoIq>-1s(2+dTMkYb)A?ky^h;xFLd! zKi}RK?|)$l6tF%F=JZckNnMVIubCV%Ce(W3k;-SZ%U3FH8`Z;=s@kE@T=>YZqF+zV zfYT%u1vZ}R1DB7SW)$y;==gJ6MmUPw|7;{ZY7ma88;rsl!J3IEdwW%4!H{hw(*B^kDsAU984|Bz-GTI!_kJapR>|i5X7?cX)TQTZem(;b zaN~sH04n~0xH_WzW!DA7NFZl0fz=oC;NNQjA+hV<@U@@k^27cBEuv>j*n~X#@f=@w zmug@#U{7$nc5g5oZ?El~D+dvHwYtxb4T}1~Ywx3B$GcM{TZkgEZ>RfijYcWT+4E3% z_gbW^FdEbo=sG!2u*Bn*e_PIKC>~gbRk+P(xQ4gInS@@jN-~$wY3m3nD`?mj^#kjL zqpg|s0Nt-5*l+DB)r%jix|$A|%{nWyiy?@RAy?ls`O~@#t03aFNl6<^IV?$z2f22} zeOf*PbyErFYxE&*G_iNyiDpL?5FHo*$$X}j5Pt}9*+_^d$hWtB0+ppZ1M|V42Dl7g z_$9q)RIEZV7z!ZUyqRa{?d~HHV2`JfK-v?lgP?0Vr5QfPv}t~oKnZqZZPm~UtjP9N z2Qt&lCHHJB22k!%&A1f)3?Q32tKv$bld#9K?v zErG)3I0HhDY3iRCqOhVsSRXNO3()uOUm`1%L8^b2WD2y{DZ4=gWLr_yB#`>YbwS^W zGUa)$CH>qwjC&ayraVQ&G4GluZS63P+u;cn&=l!wrb|ey$9ukx_Zd)lH}^VyU7yC3 zfJ>rTX{TGT6ZRKm#W*$|6cq9@0ks(Mg0xsZ39Jm1lX}zT*Gzxd9Frh~*4lwQH1|e0 zq~R0#Q6)hX52yi(MExORqhgQ&x}(2fCQ}R+Jb$P;(fW?fhMYdkL0>~5!*&rm7^J)$ zzuplL9iZ{|k^?Hf{q6e-`p@Tk1>6EDysGI>b`DDu(e{chCwbxE1(d$`N93&AO-x{5 zQFd)Ewcp!Yaa#32W-pyb&E;Wy#n%Rn8DMu&@Pt8FM6dLBl%D&_r$TLDNUhgje{lMr z)-KFHsZx&SD)Kd*4~{VDrwgq;9iMV~w+#dS{7zPeOaUo6Bszgy4y2tq5hgMZ?sNFJ zH>j+>-_TyfJbi#Z{k`9QlSZ((VK@EZggwH;QtV%322_KSG0vDwr?t z-mfJl@!rvKZ&8%0vU!jp3?%hCDXn+DN|%bn^0j&orCuv8Kb7+Vv{~I=?K7w!@6B0JL_vFKTc6yMrIA)aa`rWrE|%Zr(msjz zHr>%MYE5uesDP#@|0+G;aK1y{uIK0WR@-~dM1mFyInZjLxUFg^aO7VB#NYcrPOa|_ z!bh*7ig5eW0KHr+;XtH(VB}B@Rgt?c?d|-&c+;)_!SBv|=|b@cihc;TYa-zU;Vw^v zVqYyUw!c#xr;bmL&VUQl$L$4og9Nvz;{RPA%9M5dQrL_=?r>EkQ9m*_o_hDFES-kd zDZWg2Cg3%}z`8g*b%2^^*jVu8fEk7+{`>1f0u0&ALLm=_IeYSo_aHXYZ-y(I&u5mC zolWtaZ(T%)av&k}AMf?(sTy5>DhT=%S%#5De9iIRLwUkj%4=iI+XCtS7rWt($suPuMsqrz1r{x>du|7M1U z0T{&X81G$UzOvC$@|(xh>^|6+|M>&UBI#$@`;_$|tZg^M;>%r?`%xk}pg%wJ z9zKTu`F`_R+Dqv=Up%>oeb}ef4?!Rq{@uq{1j_na|1miF-`IDtH^jYT$_c#Np@)dl zYstp&ZV2~juIvjB)!oBydLc0eRoxyDgZyv1`x;803pTrq8&~;}nFvst?s+VD1Nh#m zB#;afHRpbMcsKczJp2O4CR><)3%&&0olK7I=vJA9!DRVd*~FF(up`7qd2wwh=h9#3 z*JHf9(c<9Fk}q8g?|Q()Yiz6kg*%MWglx_BqrGO8wdEkgE!sv^S{2$>m&4@^!;(I`TzO32GVqqF(Uk*dZGHYImfc; z2ff4twKp_z52#voA1AtT?i{Ptk7U7K$S-7()4$&;>Ezr7nS#NU)I7haP$n;k6#nzS zH<3VnH*4I#wVUzf<`9k{xXk@gWqXAmyCWau`x~ldr1a7tl%xE8D6Nf^?LSbFe$I#b z1F-u(ftAQ>Ayx(NCcAf$>1E@zSOWo4yFvLmU4j2cXn%qc#ZP& zyY;Th2Guq5^i#)Ic>$R3XpKHRphtwSyE4INp<#fmyC(02lr0K^%11DL!h+=pzTnm4|6ADM^}`^W!Ve59|Bd zBR3)QbU<75izUlRmr4>NK4YFf(S}s5U0Xe(xA<=_R+@xd!rD`$qlEyY3+@bA2GqKj zJO$}6BfR>xZd--r3(jZ?ra&k}wb|W_b)Idhlr%yYad0~h){ppX-JQw&O!L=#O7y6m z+!#9EE)=%^*AGz-QVcRUcz6|l{qp`z6$y|<-Ffp}q|6C!Vo;%{mlNMy_>~6gPJ6!2 zNh@tKSJ6l}(QA=^ZnuSjAdvITPYbf8uIi;ME5=k>zW?6CrEMwH(7YG+H#UJ&eg5mk z{#obAiqztNn&^Hi%^Pzq{obml)kq5m6X^Ufr&U_o(XwlM?^7 zowC~qJHsgpemOy3+6$t;0|c+PpV}s$L-g2SDc0>)n5#EhOYlsx=pzp8M z6mG*o@U87G9%8Jwi0-wXQE1*Xd6jorJ5e>5{4G*h5Sj1;+nclqt0TCGReL-LO*rHG zy+(FspC3qC9qqq|81|VxSbLdIB(#s4cgoIPftq-C@V5KkejJ6IPEVW$Aqg%Wh=!U<5w6S zC4;-bzY>(YFu74?=q~G*e@&l`|Ct#-H7U%!zUJZ6c6r6_Jxz9%&36)rb_e$)zTFp9bZE!^6l5^+n0tbv~ehrdpY2J32Jy!yhnt}Ra+Bhdf2+NXLeqW@?hSJYbwB&qt~2EUWju>?WMwmw&A&jx_2Fc#~_j-g`d{pgiV%eS7N9be|n;v_$xnDoFd( z8VIp@;Q*=fHGDID-B5r~Us3pl`haP_ndKkN*CmZFO3>9?h3d5s z&(Zk+i@iDE>$knDG{CG62eHndJK(nFOpCupecNS-PHP!4TqE`p=j*o5C4a2|aC%qy z3>~vrM9%)*$Ly1JUgy{MO_d#U&;w#V8h#ye%|TJQojDo-QJ6DUA22D#t38h zgBIzqt*@Z6+)+BLEnoe6d=S-!FE;DVP0y;hS4i%UHNAM{Inu4th3Dr@GS-MV+Zk%+ z*z{SrwF;^a2#leaT>z+WtydNGA}p#!;dy@Un4dTAm`V@P@~Px5%#hl!hd|K3Rw-dFP5 zU!+iyU9RHc9d^{UXx>vct0cdJhw(%3vbS;VKm*>Cg7rRmBkQ#l#!_H5xIG|yKOgs} zXm$*oTV6=9KNCqTKD3!J{WU!LzS0f3sk+06shou-zQ87In{yy3kj3F@zG+jw-s;Pa zB13X!&#``?^(NkIrhDB&16mIkDK)!jWyia{2^iT+qE%`X-s?`a>hPRG_#AF1qECGrHQwWH0cwfJ=pgKjv zFTYqB7;ZsVwsvPpSYyD*Y3w?`%3SjcLqXkKKEd^v?CIpSX~FhA+=|!_dSUwVIU*-6?`Wrwr=lWDWpdeVq zP&K~!@o}Q0Q^OVQ?-sc#^bs;SU!$mpx-npXjY;L_NKh}%{5e{1c1TrN3W%z9{8~cE z>}B^=(tl)c5V0@&{Ul3|lV{hS+ZNUP9p39vTKr8X0`3pBfH$6mkUg}0pdFDwtALT` zX!xF2p0Me0COQ7w%id2resSXj%Ggc3R)zR_3$m34&16XNezsF?Mo0LfxGDzn|6zL)^U%9=x|?xFQ0vk%nQsAB73~@ zv-kh)*%j z#kRjgQRwIUTYWMX@A;YPzyC)|uT3qwS22%RP(YsM&mQ^zW=V!V z9{%}%?yndS z>z%*em}Y>}{(57olYam8@k%PZiu(h2fc||+(xSa~(*wlaH$G3hc}DJ(7Tf&;F@ZJi z*e&l|>VPxC=g4;_ON)E(uRQ=+UhUtUyB_Z*Q(t8JEh-;dfztyc9S0Qt`7ZI#clKQ+ zbMjxb@Yfk30r^H!M|uRJ?XHBO0!fS?>*m)dAE`@_A@OsDUPXjgnJ6p|hGX~%t(!}n zg>eEn;E!gK`*wU1YDn->Uq8pl@%OFt>{Q1Ygudj-#GjljWcg=M@db&GZoVr*6Q|&E zLVv>~90>Joc$^(cJq51Po}Q@L1LmlR+7m`U(1h|&t{+F|s*|7705o)FmHd361>cxSeZZq`VS->?dcC7yu5dn!tlfe{Dcq=D+s+6dhg2k zcRRh{-0*_tHL_pVT*;xr^#8vfj0-!~1u5jIKSL>pZdl_v9sBy*y;;OJVm?IMeQd3T zEXq$%gLASmfes$NVc9;L?Za>@GhpND`+$~|5Hr++BIp^II-Jn(MLyhyvAASouv>nK ze?Xu&2A}+KrH8WZpK#FojInRE!QFQS_`-m4)Ft0JH9oIp$>aU#TRH!HHfOHs z_bF?ieezC)nk@X5S8e{2GbNy{gYXK@o$oY6KpiOV-t3|$JZ_h(Hir@ISad^r9>au8 zJ6k9bJ@)5qk#USMO5~{}*a24_Uy>@Ve&-A7giw&hTRe!hKP!E>OGBnhaJM+r7L53! zu9FO`fpO%9@&e!C$MMwy<#Pa5Togu!gu4(Q&0EnIfB458mP;a`)oTUbGIWF4~f~oRs zDz3G0l*5I;{5l+_N$)(?=ZGU+2fz@HK+_cg_a{SkfF_xOrJ(w)>(4na!$+7ddE7+h zpJ&uF;1`$ldS_|hrtjklzB|;>*%2$NI~}kPn!E^!v_AJ%(!MFS$k<>kL6-qn=onws zN>~pD_1Qk+9LK-ldTKtdhm#)n-V^+;WEpoK7(fYviD+by!YwRvcQ~nD<-!USzL|Sp zRBqAT+>-$oHvHV926D<*ZOjtkrnt)9c1MtkLtd$4%Hy9HFzRANE!fP)153zP*qZQK z(-YKH`=H0CJYlEJ_lE6A=uFtOeK51R9P()&;InPW+c6ul$iJ!P484 zncWoR^r_5OwWhx_G!U{eVw}DC;7B+Z15ZT^WjzM*-W+r@Wy_fl%R;e9`nS=*;}}() z|BPtwEWSHuEyIS{lFn{ zEvT+k{PGRm?{QzR;9s|Y8%3Rcy(&o|zJ^tdAK@kBKc`IGt!aw`oVYh?zYEkUf7?@* zmcSEs!ZrDPNY8rm&wT*F$KCnQIZNMBmTx2ODczG6)s*go_Sgvq0?W+xD)mF^4`0CJ z5KG%w;;Ir7@k$%}dzai2tOKf!3P*uG4v&$t&-SN~D*_#&D9Riv4glHn`?wr_x^gdLGi{Cna1;X}u4r_Ql7iSyJN& z_+{i9F8^^sA*cXYeh0`TO*We9aChkr@Tg+0*l!*^s9#ES%{eWLf|ER({bDIKkRM|H z^y$aD%FwB)t$JJ*OrX2YK0sxe&Q7Gw?Y)riUWkCn-BqQ1(R8jynfvb}GojRnu%g6%(76@(Oq}!Pl86&WS#Jq4+IW+;7)K{xb`9u1 z^)a;B4(2VT;N`3zXji~!A?^+Rosavf0+<#)+}nk2E5{#0R|QPwP;VCN!?wG&OJl#c z%hRKde4$bfK}kNfWh=lt65ieT43qkkCHL-1X34}{GCfiDJCBhl4~KU;_SE^N?YF86 zH;)2bKy);s*^6NS(!fij1is28ef>L@mX=#?epGO@|9Nja8aaQx zIyZmrrf?Co&djf+T`?oM7fpS!kyfetu4#X^n}WoG!{Fk6oHK;=Z-@~|G$}ra#X&JNbff!F!GMWn16(uX@ip#bWX-+`5|A z!ZU6(U7=_QE3Tkq7ukCz%bGwO3QWkNYhGb8tv0EQ4FMAPy~D+~`Q%ys{r8NkM6lla z`}b$MOMw6yI_?D}29(fqi(N7muC5S5xuQC80AMxDBucvm*T)ZXI>wlQ?y!-^q5Fd) zX$iJN*{Wafl0|+QXFpOuW9|{rk}T7+Gr0mZI_|&6L?l=E7{1iOn#j1iu`-bwhD~0M z)KB~Hfm?=9Z7Es9Q7cU=zepxb74Z#-OdIY|NZYDi6*~WgK;tJ6-&U5;l68ZdiVQ$- z`kGtfNB&4BNpbKzU78bY(4@l`-Cv|j7ynO(?nNC-Q*?~lgVa^EunwPj)tnP#4A=o& z)&9y37lzCCi{XMw0-cFd_ES21H_gUWq$Dq6xH1grr+1-B);9Jx64H8)rGn>Z$c*~Y z00wZeyglJLeUjN2bUtH#L$(H1B*=*;6avGpvct}9Se7F;o6Vq{i7QMk;~Ucs+#S@* zcUV4*%c0Yphj5{uaAdliz(PW8@h@Ep=4> zTarK^$(=BZUH(#-JO4>NL4ClUaTI9kjY)&tOXPq4Dc*py5w|xa_bz06*}HHplO0zk zt;bo5&o{wS$WNd)01R>e%#A^67D$D24$Zs+luE!-p=B_%-AJ%q6;t|gCDurkmy)kRY5Bz0l ze-+N?mSkVTx5pUQCL1`lHHG1<9QtcYTAxin%VUU`4fPiR4rm!q@k4z-JZ)2>jVy$h zjqLxt>*syq_gDmZ37@A%z|I&DK|YDp=w8tf-p5(xZ$J=S=VR%JkU$Z6o|V-7|V(GM@s3hg4S zJK0-gu2rH=))x_zgY|;1uku9Fx$MZ54mIM{9s^DNUvpxlCu(oCIqlULo zW7TBi2L{?IwBKArVj?Dj&%u`>OlG1omkJ>T^ltuvhZe@Tqv$?j;J~b15a3sH%69D_ zg=w{030mVm*)q!nPiW)u#^VFgbv*Gt_hm95tYi9His5Km$}4FKBRx!&63_7hC-ub4 zX@!0q56BKz!jv>bt}*P^)MN|0V-5DwB6NYVcQPNv$J5~szh7|3oLtcR*vkqd%cBie z91y;&a zt}{~mJz{yh+`esi&kDbrPg`QZrPKRxQzaS3$4>+sklvq5Ipk3igh6iuKH8Y%L8MC8 zzWSL?Dkz?SSkwW(<&v1lRp;mqG$ht+LG@dm{Ae#j2TL^%VDV@QJ6JDVzvzb>T|Z}K z;qK)A?7MnZgn@yAZyaBpA%afXClW`rM!~pUW3R{96KFK;*4cP#Zb$H$I^qX)>g{%^ z3O7i%vw0jx#uc z_z#WeRr!unQCQmq-9eu`UXi{youS`LjSq>XA?2H4;8Qrq0Era20>~BDTWgik!;epC zDG^9kNH^`zF&_lvHMa3jj!pkAc^IK$bg8bbt7yl^U+){+sE;slp&Hbx@u;(Jr(k-2@FlIed`M3>xG|>JzUiB@M zdv-jg&Azsa0Hq`Mz26L$*sl;%SlRNZsNu~lIckO358V6pm{Rz%;|mx<#XZRa>ta!7`H&oHn6?NzUB&9Q}0NkHSOFWxo>7E&q?|zC8YQs!sb}wmWb|lpRz=1w>KE z(j*O|Lee(f)3i;Swg}v`X}ULQwl0bwj-rgfh-?A^qN0NWIw~llUiXDfQDomlKoP|e z1@X!_&*=~6oq6BC=a2XIxmD^W zQG!R9`m$qc;%r@p)1H>ikmFKRV=NK+H1s@;oI_Q10%07~S9IVEim)1!34`G~R| zusZ>REe~^GQC!aGZgN#Am7+L9T3haoeCSW>bW~4Es|K=S@E()g3>8MjU_J_D!hwlQ zuXM&fH0Y*-PAlR&KCVkjIkJ>!?f%L-1+u`a`{k8Q8@v@MJDCqiPfA{COk0o$kwQrZ0XaVDl?#@c z)H%%=3|j@0!U#zwQkV=l;N$h39vYt+K@uJUTT!pOes@UmO{v+sP5eYvFp89@uk=`V z90xs+l*(&-o9-r7I2a{uiSDupY)5_Ms%?@%(KSm8mKv&68#6bo$Iwm9mA%T5Ct*VOi2+#D)JRQeG7$u+%}RdXjn{{j(Csy~xV|2vPJNtO zO__C&uepp-^BI!9)+K;+oLsGgTPRMNI+a(a?Tz<@UqgmxduEchRpARUNKGd%*0lTg znkLp&FxVunSBMqwmj=VSqrod(9@n7IhL3tW90;@ccv>kFz!)fG5>62*hb@9k;{^=0 z#5V(6h|ynWCf)=#V8SlvrWNyTY&cOUHmEHZ?%W{~G&{*s)yJpFe%0X@6LNo=Xa9bG zB2T)hFdl^B4(lU}Y*! z$KvaTG5la-K29ptQa^^rmf?1C*9|kzG{ki`=X16}=v3uAovH~OekqZjjgd@xrI~DC z5D$CZMl6mRW+||J^Z-Z5d8H??E`EUqM=H@oc4d$Y;(j_Th7fAW4vAe#WcZ1-G0sR? zULPOz*cjx^r^|R7vqZ+`7iOI-XCZ@Br!ck>T3|E|X@5P&ExK5CX$x>3 z!Jx}4hRi&Ik(8PVNs&0UWsh3vM%Z+{3B|YxhOdJO(Jq)mG$F^eg6v{R0NXUfSF9E} zvACeev%pg%f-Q~o_^imO^+0Vwl21x-^~eS9IXNRLwr9|RGNrU3-%aw^&$W>8Mbp|Vz&Z~U8x42&Cs!e7mc-`(5??WfvGkr1QvHt z^JFPCaU74Hs4m!O{dpgqSYirG%8&|?!_tfur) z-?LOC%6hGF#Ucj_97gc>gmP$m(N!Z0sSIfKF$A*dFwl~)hh?FrCU_cLN2W?Pkj)m{ zJS@4|u1*38r_8jSZg}OEo>OTXLMqr6M0K=mxtjxky@Frh;agbZyDUYh1m!d&WeVQ#5PE6;5i*+L#bx6A^cq# zAeCU#uzUz7t}Icpnjq0Z&|ghy$Gc{f8>hx>dJGI$K#(CQFA%yA8_otx+@<5ces>gc zWVQyS=4>WTYSl`|kjSbw4?z2AL_>cX4u(n5pms9 zFrkfcP!UuIZeCI)1&a(pP`eXPbZkx4#F5~^w^W{(4&+5`-goITH!1T}8>a=itwv>B zjV;=|+^18G#84%TurgI-k2V_ZR>d&2IZR#3_naz~Sb6rO-EReTI?e}^897Pmh;FHm zD2Pm8?`74aaxb=Utu;hdj+r9wBLW_oxr?!>#iU+IQUa+*P^@}e%9CLbp+S^lM$)Rq zS--R9vJRJHE2>QfagTA~$&J#16OEEm#w{h#1!G`${wx=ibfmk-Tz1yshV=+Ck~Xd> zd5B!eA#Thy(7n@CD_IOmNC)dtJl%{3!*0t4LThyj;j&#S*-{qnBo^W;1ZBjZwqjH( zhy|F8z*-mOhE+bp)nQ8%3psUho3}}Tnv5~5_=M9pGjrn0LT@NGJ1aB0AlUG#JRh$R zHNWclr2UTHVxD;Q-M+(UH zYYDYp07W#ZT$bF?9D_7wLE#oD!vLsFpQAaheN6z2_l8_RiQ`sQW#xgJxQZ^orYjyL6Q@Ju`(AYvu(Tx5Bmtbh=ytA45oY!n}-~@M0M^R&9}A{9jk4Dt@6}&%<^E z{K#$tf~)gl91SdkeFYYMCL3&`FjPuo9zI4dMYC*+O`3ClQc)sEUs&2)2)i&|${=Y> zOa&84sWBMva@bc*@3MjNmhC2S(-3)OO(F;<6G$CX*ij~E%2$wRPyh*X z(N9mS;|0L9nb_bs08xcrr3f@4Hll#F2?o*&f7 zY=lor36}%jo5+oY62re{RyoodQ!7c6Qa46u0IDc?%Cmh7bbGXbF|p9Bc0;sF6sQ5H z3271AGA%mKpcxX9F&q0SlQgGKBN$Omm63-o!gDqV4YJTSBF6l$_b)GS`8;J8nTFTp zkQP3zOp`E`NwNO&(65f^rZ<;A-RfHWm{!=Oc>?Ajjo(AQ|1}n>xlJq zhnC62?(k#^LFi<@7(2Fs2*HiEtOcWiqtcZOJQv`H6FAr^&?SN1E8~yI{NigNX0eXW zR{~Zip-&1oV>FxZRs%~`mu_#}=Tg`7{H&vq-Gyu8Xrmy_WQG5YeLies$Cs75^>(00 zMY2^WSTxoi{4%kVE32pIam6O}Bkhdv?@l(9CpV zx<9>;M|gCH8pPX0aZ+jz$eL531$gwc$u4=n7O8IrD=CrbfwnzBTp!#>2=yK`lfD2T zMv}T?y{*9HrpfV7@i-QjODF-yFvPV(Wr>7BYo}X3-dxPxwdT$zYEh=>Zmxa&4%$A%1pdiCP&JYHdk?eIRF)@U4 z5sns_0Zlrhmc}{?KGuALi-pTd#?VSo+M1!IaX?ZE!Gwk6kTF%6Do$$%+6AQ@*)B+P zV?+kKFgA+I86-u>M9(e8)0O4>wTf==UNm{zKm zB$lW#EP)~{^lCV6RG#68^3@#2OpC4M3}A!9 zm08DXTXge7uVm_~LSYXkESX_ge8n-yzNY~k42&Oop@?U26tU?emQ62$I`+L2ky`Ny zZU&L>as_7siXn&SY9&gS=}B_wmaz=jlU88RO-dkv=yJEvr2^=wNVA0MuF7fjOG*Vo zk{kH6X(O@)B&p^MdJB@lgF$87uCI}(^ctPW*G8-$nR-F1x$Am>Q8Z}MZAz;wts&VY zKsVf%hOJ51t3|OmM$I^>Hj~|YZYCnF;Fu&S`T=l$}=;o0mZ@Gf|yBi>Xs16rBQ{ ztgrNqQ+tYEF#x)1uxAR}18Pvr)&wR_x;MXYVhN08Swo=-fFOUzT?#Lv5R)7*W`a!g z%utB0QJ3=6AjC@Rdy(t$A`Ss?s19smTA6Ej3=;!JR?ZF#dXVb{Kzt@t9Rf0JMuK%K zXPLiaF0srU^^tO!+bDO>K^eJ?Z7Lz=hB@k%uCIEE9yg>_~v> z2_$BR!N%L=YeG8pVv9j_h3aMp8%Q63S~y=2m6dA^%tCC*j7TtE*-^t_T|zrjk_2}_ zerAQBl7pZ!POQqYWIdk5JEeeu)cr(jT?fAirr#6tsjIKhS$0ImrD!=Nv9yg&)eFfx5Xk`pML&7Pi`Clj> znFJ+Vn~+ls4YmeS&*BPn9d$85>Y0~E&WosDjxVo058YgDxJI{{v`}r98shLpoN-F* zIOTN#e8C1mvS|ILdC6N}U8SUHU^jCODu+6hCz*a`0}*d_r=Sd8BYG`3@7IW6%vHr9 z&=O3S8g`U&6FmG{b~5M^JYoaeEv{zp7Zo=!%t}}u6RcKB@fpw&KuJOADT2F!fHp@X zv9^)JZL%EXMGF2@>|*8$bTaHt z9HexkiQm*jW8KTy%4|?65cE99k@K}mdWST8qR7@s8L1`J6diT467^Pz;dP&Z2 zczwzvs)568oFc9*R4`sBlev~%=3CgZzv!t7gKkntT7p3W_9; zEai(44gRvk8h=ity5exdBbq7Ln2WG1Rpj%)3p*I(lh9OHdqlH~eAXLu;nfA{gDDjk z#U zQLQnespw#@Gjf&D+K-q5NA;vJglC)%)?SQ$T6UBonY6&{c*+DF6HDh1xg^Xx-eeig zRC}IsxH?@KCl`IyHsHIahZG{pD5<^3vDHquK4=B;{-VW`KJGM;h#0B`N!ABsaZ};~ z68@spq6S5TB`(M&5T7B}7Hz`hDd+4koi18kdPRlZXhBLQkRMsNKk^CiB)Ry z60sJ#q|9i?<(h1ovf5&-m1&^s)NaI)gxYj&DAW>wr~-pBd&YQ?N>d4sW#ZEXJya&0 zMjF&LWxtorGqfbIeXl-Ln)D3ygNSu{T$7z#Pqn}e-0TCOpXCYigz6~;0OJljO5v7; zw^%wR5nNt-^^r)Ph(l1RLV=Q!*4IfW;BV-aNJK@0qkXQfmL?IWwC5Hg{w7AH(Q38m zLUGha;vmY#R-~8Hy4aE{zJb3GuAi-!tcB88M&8_)#y)~Uc!q5MbMU5NCgt695kklF z;y4{}bhTWakCH|7b6&2AG4MHK`Bp}5DVUmzV!ag9l}2S6m-0~jvRaFnzl7Kap3@cc zrj&4(Ll438&5;3qQYR-;oL3HdF`0Y4W@Y(UO9LA!ycAnc%BB+Kxu6D47RppvJC z$Z8$6r%oxE(?lE2+PUhCmImUVmzGbY6k>~Xt1KnwmOEo|CBI3fTQjxU=^H=@WOe}E zrg@bKrl9(*hg7At=!31WP)cJOD9HuEiT!3RVb0WAL|!{%71xnerAi8~;ittK(JR3M zh{P0xltyHW%p$-7m-bs~-lIduMf{AP;$AX=h$E<`VO}^lcUBT>+9X>x)?AS=kR)RhRlEyY8vQ&cxb9NXlM=8_S z13uRf+fdI$?3yF=EU{ab`&FbYpeY?AgX#*|Jl0`Z^Pt$FBXc7&HHI5eZ6OCBr8^PJ znc7r{=5?kXk@LS9G@5xJK)>Oo%Xw!5t4)hcv>Hq&al37<*F#&Rbb2BI4Y zUrnr$Xo1;FDxeZo*Q+qGF`{ncSfG>Za&pKP%^OS5DzTLqu^fB}S*R()pQfXLf{Re* zdThPrfprO-g^d^j))2lfG^;{43WW@3i**RRD9NEwOCd)O?cGrmp0D>;sp-550TdV0 z0dSJiDbPc~td1)y*CjiUoLJ=GpY+&qM}RUDY6b?mA=pQC8=NxW)l!B6OHK23I%u5?8 zE}BzjxTts2bGe4`H(rUPOq%hkWMUTr0ZawOb@bviW5T8M3iuv>qFu(m5h(aS{e>*ovstVDgKlb9HE;BZ(<^gJTnKO zgGtD=Rs^$KYgmPx1YdxN?<4$22~|5NH3$|`<%v~AJk&yuIKB-2IU{mn$)r)5${1?& zB53C(%~eyH8Q`wh$KI45%_crPl?G2Qre&#_8d7Y0oad^YCe+eq5uffjP&LG3suNl2 zgklAG_$IIo8R4p4qcsRJ_`QqCrTR>1(k}+(m{YGo7niD3P!~<1w29O+>R86?KyoeH zvl)xLJECgI(QU*@a+MiP^MPd~+@kA24Z1G2NBN$(s(0J8290?5a$zLsR2kRTt!2TM z(mk(K$+{M+)k`hc3DS8WTyiQ$WkaSGcns1`@NW~*m)WK_K)`nmm?a<6XLG#(o*xAe z_;OWlz%1ahrreF?`57(78rak;t;~o9Unw!59k`&tL>kssaB1JtxH)v(iQA;2> z>)YTUl`})T6NB`u6{+wK+tBoOdlN2}OPWcGby}QF)`*M|L>g^~O1vf@)vzN*5DBe~ z7w#g)>WMb(WPG+oycY_cTt}%mZIF5=W|ArN8zPjm0|hxLg|H{`v@+*`O=p~b2pev371slU=%W&jo zI)ibkl0#ZtW3cvsK{PZnM=J8k{A59G!~PA$@vs0TK9Z9%^VykACTn(RV^5rAxhxNt zlFoMr1-EaioXZb|cE1`3kvr~pO3r#>+fJ&9DbRs z^LPo7e32C;yAA3r5xK)`FjJ<1iLnpbF+o}9uOM$Ek>k~%lO^XxL!5P6l@Nj>5_AN- zd|r3TR=7ZDLs$s>G+f;^j774}1wBO8G|Q`&Vb2D3J8qYZVC1Wrn1nA?kt(K_ATnwU zJ)z%ifLNg>fX4@sdH6P$!_<*+1SqL(kcBNrB<6(VI!T6(xB_OCOLh!<9urog1gSi} zSg#F6dRt85Dl;OB7sCDVw-p9r$AJ@qv6F(s(90ptgG!qj#^ArlF(wn|=o-mWg0~wF zq|I4&rCTl!Vau+Z?Fr$uT?#Y(z^1xvWUawr4`S67(PygqIKiwYBOk1|y?mk5E$cX{ zx8z_TRC*1*8$_vy0oygNnwHuW^wbzEHBGW@quXTFvq%TP4pTCdS~VHi&>lidBn=SP|c>a~AD}1u(%B zIBCYLwZzJ*Dv5 zcp2b+IgQy!rZo6sX{Zv8#+8yVP{}$_6$O(83D}YgIJMKn%$9B6K$=dz5V;*;3}d)X zw#4U!kp@p%t5dA0B$$Yg2GCH}5NaWJ+R+RGyaJi?8fz_}Hmi1r4Xh@zoV0|sstfck zeK)@x7TQX<@M9i?s|NwX)HQt?*5p`$Cn>$;hg?Hg*V# zI~bWZN>y}90H-t`!D^MoQmg^#Cu9h8l7!uox%V)t7T@R8g zQJJKXUrQqjJrCI19uYx4xi;WP(fLv=RmIw=RFH0+9~UYmconsB)(d;=gl~&0w->N< z7SZHL29c1k+{?5z!f=M6?{Je!HpmPYRVFnItl>hS9QY~AKE#2`WYcAKsg|Fyr5pv` z#bURHtnPMaO5=EpOqUFH3UbfG_CaaMqw&;12mrLQEZ2yWnI)1_`n+Z=htpYa5;v2O zOPNL$Ecpm!U$p7@sA8`W*@!Tiw3@B(2)~N0sD!q@B8NpsG3F7yHr-jIZPltBsSHM9 zS|2i031rXEoi+!{(R7*y7fnMbu?^4jmh7aHb-REtz`06zi}IvHVitIT&0!OS1}N6$ z3JN)!;@63$k{M!n7Isu|iAv%)5u1WO4>IE|k03BmoLcDQ8kiKaoptO`B(LC#*uEz- z?eZcXa+x`f@+vcT`d-!Z98qs5jUE@cY84#(|= z<`b(i={PUeGz=hKQ6p{i+O1`%2Py*+6G67=jtW)=G0L-p!eBu4iQ`am=L)YbR$Ljn zc5OpL>&QcHIxDvchNnj&9i_yTVw;@7X6w*~@mQWMX%MAzbm$1>r${N)#2Pmku00%q zxb)(+Af9Nt!{ty{%3a#-WuX07WbHN4+t5=QwWOnG*sxNnm$`<h zAZz*WEH8im?0=KiWljA8S|_lk{6CC0FVe!Kp>p1b9AEc;lEbXOwFYL-GeOz1zWK!) z>;WNuA)ky^=ia+%)0P(yBd<5ozrASpe(j!vzJKZyZ~Op#?Ln{l_#2L<>7VpdXKgKy zQrEPuP91r{$BzBWp&z;+DqMNx`FDKmr}Z;Gk-L&PZ+J-e(tF>iAGGJukM1A7|EsCr z{N?TM`QDd*e&9YA;ym%pDNjG}NYPzpucFR7WO3Nj54_{H7asWA4VQKP{Jy=O+4G4< zPd$WsX@6_5^Ivv;`t+~=@sx+(;vTR#_&;JdH*T#_d;WR*wtpM{!;{Bev`O3bXFKn{ zt$NzNJ3sg4XSZ(K)BV?fJ+HVqkl!e7efQIw&)ELLzIX4r%gN6?e9`-M9B~@E_0Q_B zPMUWcCkhAOebuJN55Dvv?#O59FW>y`?;o%4wfUEi9lL4kp6a#Vec@$C{`JO9kACy{ z1E1RTxv%W`Yc9@=u!7hiVUB@Z5a z%g_Ji>gY$89QyIig+0@qKi##<6L0?M%WrvT`&%|W_rhJ*ylT%w-+lb{0}eRg!n;1- zN&n&(hkarDJzwaF@BA}$&a036`9sIffA`r}A9T(kZ@uI8e?H)tU;X%FJAYg`tNrly zcdvf?{4-A+`vk82Oto75+fI*f{oJX}`~OZ~N%8zrC+>%V#e9{LlaJ%OCD@{q%t!eJpXtuYU6JE!UiI!&Nulc<~u~ zz54Ne4>;huot1+xz5UJ8{oQLne{Om6{`>9sJj1dtG@H#IB<{NIe)%z{G4FclW$%Cb z^nbjrdD^bK?)vpdzJ11yGxmJN`+jrdrN^Cm(%-K7)TfU6r*|H6^wIVATJQgs|HN(G zA73F2Z`*pn(ML8EWw)~*b)Wp%8+LvBO^eTOxqJ6ZPkr}}6CWNPX6bued*WAre&Gcd zJpS}OUwHleclVdsn;)7!aO1DvlRo>E*>3lffBED~HoyGt&iBtQpM3J|Uwg-MnL^we=oQl5`|R`W54|p}@4o!>pYJ{YDC3VGzTkqZ z&OGTRcnE&!Kl;5rPx;ta=TBdK$CaN3fV~1sOZyj3Jfi$!^ULWv z*V(t+Qn)er^|${0(eMB1aOIpsKDrNnwmkm&ZKuBEy!VKk_$^=F{jZw~N@x3g)9h!r zpLT4?9$j-%ll>yXdBagWb_S{VKOP3N9_uJ=K@-QK)>^?3jIt4}{+rw{J@%+8YuORj!C^~l#x*>T6!wWE{|9{8H<13RC0{}*3&%Q>fwwx6aw_HR#bKL3zy&uRDk zW>iS)7|MKk7_n&jlbDJRGUcTeuUpAjwe(tzkra#zq^S)nBY)jo+aj$yM zj+bA=eD3ntFCX~iP6xhZw-?^=>{I1C_uhN&_~Q3h{N)mT@3Q`f6V$W6yw}D1oc#57 zW<8H7UH-N^ckGh8<*3cE!5xRSJ9zK6W;P$S=avs#+ui=uZbuz%58kumom; zyt?+L6Rt0uDBW@EC-WCxbZmI_MK{vh9)0aEHa~fi+?oyE_R!{MZg;l>pN{xNz=J8tje4?3#;*=^rEeb<(#UK;=R{f^)L(ocNiusybJ{o(Dm-?alHCAaBS z-~YhD)VVjE_03m)@$QE<-S@hWANc#H@BLDKpX>f?c8<@!`m)mxKm726@40TLediZ3 zsn+oq?|S4uTfhC@6P|tfy?6ZcUVA)@4*HkjYwfdlIj{2gb33?J_XpwSw}0w~Z>dP{ z{_v03*)P7c|K?|R{nWnqJNeN8yC1n}%S&Im4Myhb+kSNZQFnfaIqZcD_36$lJ{}qm zUH>`l1%3D@m?Exm*pFLRUZH>FxHGmscin3@KlZmX&)nlX|Fq8$g>5%q@a8wQzL+`c z17F-$`JRX${Fkmd=bUryI$<~(eaXvQdBIzbbRVq0rhV4_o8NcW z>}{Lxd+ZO79iPqR&K!U18_X`=pPsz);`=`Fw)EL&y+Sy6_Y3a7|Lwo{_Ku5w@SD4C zzWF!je)z-l+_Qt5roVjdhLHX3_8+|V&TWTpJ@uaBZyNmM$ZNlQ`aLgwHcD)H^Us&R zUA*Fo!g$MVm&-pqLHJ$z`N@Mfyz-FZ9 #if INS_EXT_POSE #include "modules/ins/ins_ext_pose.h" #endif + +#include "modules/gps/gps.h" // DELETE FIX //#include "nps/nps_fdm.h" // Number of real actuators (e.g. motors, servos) @@ -193,6 +197,12 @@ float act_min_norm[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_MIN_NORM; float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0}; #endif +#ifdef ONELOOP_ANDI_NU_NORM_MAX +float nu_norm_max = ONELOOP_ANDI_NU_NORM_MAX; +#else +float nu_norm_max = 1.0; +#endif + #ifdef ONELOOP_ANDI_WV // {ax_dot,ay_dot,az_dot,p_ddot,q_ddot,r_ddot} static float Wv[ANDI_OUTPUTS] = ONELOOP_ANDI_WV; static float Wv_wls[ANDI_OUTPUTS] = ONELOOP_ANDI_WV; @@ -214,37 +224,14 @@ static float u_pref[ANDI_NUM_ACT_TOT] = {0.0}; #endif #ifndef ONELOOP_ANDI_DEBUG_MODE -#error "Debug Mode not defined" -#define ONELOOP_ANDI_DEBUG_MODE FALSE; -#endif - -#ifndef ONELOOP_ANDI_AC_HAS_PUSHER -#error "Did not specify if ac has a pusher" -#define ONELOOP_ANDI_AC_HAS_PUSHER FALSE; -#endif - -#if !ONELOOP_ANDI_AC_HAS_PUSHER -#define ONELOOP_ANDI_PUSHER_IDX 0 -#endif - -#ifndef ONELOOP_ANDI_PUSHER_IDX -#error "Did not specify pusher index" -#define ONELOOP_ANDI_PUSHER_IDX 4 +#define ONELOOP_ANDI_DEBUG_MODE FALSE #endif // Assume phi and theta are the first actuators after the real ones unless otherwise specified -#ifndef ONELOOP_ANDI_PHI_IDX -#define ONELOOP_ANDI_PHI_IDX ANDI_NUM_ACT -#endif +#define ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL] // assuming abs of max and min is the same +#define ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL] // assuming abs of max and min is the same -#define ONELOOP_ANDI_MAX_BANK act_max[ONELOOP_ANDI_PHI_IDX] // assuming abs of max and min is the same -#define ONELOOP_ANDI_MAX_PHI act_max[ONELOOP_ANDI_PHI_IDX] // assuming abs of max and min is the same - -#ifndef ONELOOP_ANDI_THETA_IDX -#define ONELOOP_ANDI_THETA_IDX ANDI_NUM_ACT+1 -#endif - -#define ONELOOP_ANDI_MAX_THETA act_max[ONELOOP_ANDI_THETA_IDX] // assuming abs of max and min is the same +#define ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH] // assuming abs of max and min is the same #ifndef ONELOOP_THETA_PREF_MAX float theta_pref_max = RadOfDeg(20.0); @@ -261,6 +248,10 @@ float theta_pref_max = RadOfDeg(ONELOOP_THETA_PREF_MAX); #define WLS_N_V == ANDI_OUTPUTS #endif +#ifndef ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD +#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0 +#endif + /* Declaration of Navigation Variables*/ #ifdef NAV_HYBRID_MAX_DECELERATION float max_a_nav = NAV_HYBRID_MAX_DECELERATION; @@ -274,14 +265,26 @@ float max_j_nav = ONELOOP_MAX_JERK; float max_j_nav = 500.0; // Pusher Test shows erros above 2[Hz] ramp commands [0.6 SF] #endif +#ifdef ONELOOP_MAX_ANGULAR_JERK +float max_j_ang = ONELOOP_MAX_ANGULAR_JERK; +#else +float max_j_ang = 1000.0; +#endif + #ifdef NAV_HYBRID_MAX_AIRSPEED float max_v_nav = NAV_HYBRID_MAX_AIRSPEED; // Consider implications of difference Ground speed and airspeed #else float max_v_nav = 5.0; #endif +float max_as = 19.0f; +#ifdef NAV_HYBRID_MAX_SPEED_V +float max_v_nav_v = NAV_HYBRID_MAX_SPEED_V; +#else +float max_v_nav_v = 1.5; +#endif #ifndef FWD_SIDESLIP_GAIN -float fwd_sideslip_gain = 1.0; +float fwd_sideslip_gain = 0.2; #else float fwd_sideslip_gain = FWD_SIDESLIP_GAIN; #endif @@ -289,7 +292,8 @@ float fwd_sideslip_gain = FWD_SIDESLIP_GAIN; /* Define Section of the functions used in this module*/ void init_poles(void); void calc_normalization(void); -void sum_g1g2_1l(int ctrl_type); +void normalize_nu(void); +void G1G2_oneloop(int ctrl_type); void get_act_state_oneloop(void); void oneloop_andi_propagate_filters(void); void init_filter(void); @@ -297,18 +301,23 @@ void init_controller(void); void float_rates_of_euler_dot_vec(float r[3], float e[3], float edot[3]); void float_euler_dot_of_rates_vec(float r[3], float e[3], float edot[3]); void err_nd(float err[], float a[], float b[], float k[], int n); +void err_sum_nd(float err[], float a[], float b[], float k[], float c[], int n); void integrate_nd(float dt, float a[], float a_dot[], int n); void vect_bound_nd(float vect[], float bound, int n); +void acc_body_bound(struct FloatVect2* vect, float bound); +float bound_v_from_a(float e_x[], float v_bound, float a_bound, int n); void rm_2nd(float dt, float* x_ref, float* x_d_ref, float* x_2d_ref, float x_des, float k1_rm, float k2_rm); void rm_3rd(float dt, float* x_ref, float* x_d_ref, float* x_2d_ref, float* x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm); void rm_3rd_head(float dt, float* x_ref, float* x_d_ref, float* x_2d_ref, float* x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm); -void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3]); +void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3], float max_ang_jerk); void rm_3rd_pos(float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_des[], float k1_rm[], float k2_rm[], float k3_rm[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n); void rm_2nd_pos(float dt, float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_d_des[], float k2_rm[], float k3_rm[], float x_2d_bound, float x_3d_bound, int n); void rm_1st_pos(float dt, float x_2d_ref[], float x_3d_ref[], float x_2d_des[], float k3_rm[], float x_3d_bound, int n); -void ec_3rd_att(float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3]); -void calc_model(int ctrl_type); +void ec_3rd_att(float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3], float max_ang_jerk); +void ec_3rd_pos(float y_4d[], float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x[], float x_d[], float x_2d[], float k1_e[], float k2_e[], float k3_e[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n); +void calc_model(void); float oneloop_andi_sideslip(void); +void reshape_wind(void); void chirp_pos(float time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[]); void chirp_call(bool* chirp_on, bool* chirp_first_call, float* t_0_chirp, float* time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[]); @@ -318,14 +327,18 @@ struct OneloopGeneral oneloop_andi; /* Oneloop Misc variables*/ static float use_increment = 0.0; static float nav_target[3]; // Can be a position, speed or acceleration depending on the guidance H mode +static float nav_target_new[3]; static float dt_1l = 1./PERIODIC_FREQUENCY; static float g = 9.81; // [m/s^2] Gravitational Acceleration +float k_as = 2.0; +int16_t temp_pitch = 0; /* Oneloop Control Variables*/ float andi_u[ANDI_NUM_ACT_TOT]; float andi_du[ANDI_NUM_ACT_TOT]; static float andi_du_n[ANDI_NUM_ACT_TOT]; float nu[ANDI_OUTPUTS]; +float nu_n[ANDI_OUTPUTS]; static float act_dynamics_d[ANDI_NUM_ACT_TOT]; float actuator_state_1l[ANDI_NUM_ACT]; static float a_thrust = 0.0; @@ -340,11 +353,11 @@ struct FloatEulers eulers_zxy; float psi_des_rad = 0.0; float psi_des_deg = 0.0; static float psi_vec[4] = {0.0, 0.0, 0.0, 0.0}; -bool heading_manual = true; +bool heading_manual = false; bool yaw_stick_in_auto = false; - +bool ctrl_off = false; /*WLS Settings*/ -static float gamma_wls = 1000.0; +static float gamma_wls = 100; //30; //This is actually sqrt(gamma) in the WLS algorithm static float du_min_1l[ANDI_NUM_ACT_TOT]; static float du_max_1l[ANDI_NUM_ACT_TOT]; static float du_pref_1l[ANDI_NUM_ACT_TOT]; @@ -353,7 +366,9 @@ static int number_iter = 0; /*Complementary Filter Variables*/ static float model_pred[ANDI_OUTPUTS]; +static float model_pred_ar[3]; static float ang_acc[3]; +static float ang_rate[3]; static float lin_acc[3]; /*Chirp test Variables*/ @@ -390,20 +405,19 @@ struct Gains3rdOrder k_att_e_indi; struct Gains3rdOrder k_pos_e_indi; /* Effectiveness Matrix definition */ -float g2_1l[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2; //scaled by ANDI_G_SCALING -float g1_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW}; -float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]; float *bwls_1l[ANDI_OUTPUTS]; +float EFF_MAT_G[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]; float ratio_u_un[ANDI_NUM_ACT_TOT]; -float ratio_vn_v[ANDI_NUM_ACT_TOT]; +float ratio_vn_v[ANDI_OUTPUTS]; /*Filters Initialization*/ static Butterworth2LowPass filt_accel_ned[3]; // Low pass filter for acceleration NED (1) - oneloop_andi_filt_cutoff_a (tau_a) static Butterworth2LowPass filt_veloc_ned[3]; // Low pass filter for velocity NED - oneloop_andi_filt_cutoff_a (tau_a) static Butterworth2LowPass rates_filt_bt[3]; // Low pass filter for angular rates - ONELOOP_ANDI_FILT_CUTOFF_P/Q/R static Butterworth2LowPass model_pred_la_filt[3]; // Low pass filter for model prediction linear acceleration (1) - oneloop_andi_filt_cutoff_a (tau_a) -static Butterworth2LowPass att_dot_meas_lowpass_filters[3]; // Low pass filter for attitude derivative measurements - oneloop_andi_filt_cutoff (tau) +static Butterworth2LowPass ang_rate_lowpass_filters[3]; // Low pass filter for attitude derivative measurements - oneloop_andi_filt_cutoff (tau) static Butterworth2LowPass model_pred_aa_filt[3]; // Low pass filter for model prediction angular acceleration - oneloop_andi_filt_cutoff (tau) +static Butterworth2LowPass model_pred_ar_filt[3]; // Low pass filter for model prediction angular rates - ONELOOP_ANDI_FILT_CUTOFF_P/Q/R static Butterworth2LowPass accely_filt; // Low pass filter for acceleration in y direction - oneloop_andi_filt_cutoff (tau) static Butterworth2LowPass airspeed_filt; // Low pass filter for airspeed - oneloop_andi_filt_cutoff (tau) @@ -411,22 +425,30 @@ static Butterworth2LowPass airspeed_filt; // Low pass filter #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" -static void send_eff_mat_g_oneloop_andi(struct transport_tx *trans, struct link_device *dev) +static void send_eff_mat_stab_oneloop_andi(struct transport_tx *trans, struct link_device *dev) { float zero = 0.0; - pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID, - ANDI_NUM_ACT_TOT, g1g2_1l[0], - ANDI_NUM_ACT_TOT, g1g2_1l[1], - ANDI_NUM_ACT_TOT, g1g2_1l[2], - ANDI_NUM_ACT_TOT, g1g2_1l[3], - ANDI_NUM_ACT_TOT, g1g2_1l[4], - ANDI_NUM_ACT_TOT, g1g2_1l[5], + pprz_msg_send_EFF_MAT_STAB(trans, dev, AC_ID, + ANDI_NUM_ACT, EFF_MAT_G[3], + ANDI_NUM_ACT, EFF_MAT_G[4], + ANDI_NUM_ACT, EFF_MAT_G[5], 1, &zero, 1, &zero); } + +static void send_eff_mat_guid_oneloop_andi(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_EFF_MAT_GUID(trans, dev, AC_ID, + ANDI_NUM_ACT_TOT, EFF_MAT_G[0], + ANDI_NUM_ACT_TOT, EFF_MAT_G[1], + ANDI_NUM_ACT_TOT, EFF_MAT_G[2]); +} static void send_oneloop_andi(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID, + &eulers_zxy_des.phi, + &eulers_zxy_des.theta, + &eulers_zxy_des.psi, &oneloop_andi.sta_state.att[0], &oneloop_andi.sta_state.att[1], &oneloop_andi.sta_state.att[2], @@ -444,11 +466,18 @@ static void send_oneloop_andi(struct transport_tx *trans, struct link_device *de &oneloop_andi.sta_state.att_2d[2], &oneloop_andi.sta_ref.att_2d[0], &oneloop_andi.sta_ref.att_2d[1], - &oneloop_andi.sta_ref.att_2d[2], - ANDI_OUTPUTS, nu, - ANDI_NUM_ACT, actuator_state_1l); + &oneloop_andi.sta_ref.att_2d[2], + &oneloop_andi.sta_ref.att_3d[0], + &oneloop_andi.sta_ref.att_3d[1], + &oneloop_andi.sta_ref.att_3d[2], + ANDI_OUTPUTS, nu); +} +static void send_oneloop_actuator_state(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_ACTUATOR_STATE(trans, dev, AC_ID, + ANDI_NUM_ACT, actuator_state_1l, + ANDI_NUM_ACT_TOT, andi_u); } - static void send_guidance_oneloop_andi(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_GUIDANCE(trans, dev, AC_ID, @@ -484,10 +513,15 @@ static void debug_vect(struct transport_tx *trans, struct link_device *dev, char static void send_oneloop_debug(struct transport_tx *trans, struct link_device *dev) { - float temp_debug_vect[2]; - temp_debug_vect[0] = andi_u[ONELOOP_ANDI_THETA_IDX]; - temp_debug_vect[1] = pitch_pref; - debug_vect(trans, dev, "andi_u_pitch_pref", temp_debug_vect, 2); + float temp_debug_vect[7]; + temp_debug_vect[0] = model_pred[0]; + temp_debug_vect[1] = model_pred[1]; + temp_debug_vect[2] = model_pred[2]; + temp_debug_vect[3] = model_pred[3]; + temp_debug_vect[4] = model_pred[4]; + temp_debug_vect[5] = model_pred[5]; + temp_debug_vect[6] = RW.as; + debug_vect(trans, dev, "model_pred_as", temp_debug_vect, 7); } #endif @@ -593,6 +627,16 @@ void err_nd(float err[], float a[], float b[], float k[], int n) } } +/** @brief Calculate Scaled Error between two 3D arrays*/ +void err_sum_nd(float err[], float a[], float b[], float k[], float c[], int n) +{ + int8_t i; + for (i = 0; i < n; i++) { + err[i] = k[i] * (a[i] - b[i]); + err[i] += c[i]; + } +} + /** @brief Integrate in time 3D array*/ void integrate_nd(float dt, float a[], float a_dot[], int n) { @@ -615,6 +659,30 @@ void vect_bound_nd(float vect[], float bound, int n) { } } +/** @brief Scale a 3D array to within a 3D bound */ +void acc_body_bound(struct FloatVect2* vect, float bound) { + int n = 2; + float v[2] = {vect->x, vect->y}; + float norm = float_vect_norm(v,n); + norm = positive_non_zero(norm); + if((norm-bound) > FLT_EPSILON) { + v[0] = Min(v[0], bound); + float acc_b_y_2 = bound*bound - v[0]*v[0]; + acc_b_y_2 = positive_non_zero(acc_b_y_2); + v[1] = sqrtf(acc_b_y_2); + } + vect->x = v[0]; + vect->y = v[1]; +} + +/** @brief Calculate velocity limit based on acceleration limit */ +float bound_v_from_a(float e_x[], float v_bound, float a_bound, int n) { + float norm = float_vect_norm(e_x,n); + norm = fmaxf(norm, 1.0); + float v_bound_a = sqrtf(fabs(2.0 * a_bound * norm)); + return fminf(v_bound, v_bound_a); +} + /** * @brief Reference Model Definition for 3rd order system with attitude conversion functions * @param dt Delta time [s] @@ -629,7 +697,7 @@ void vect_bound_nd(float vect[], float bound, int n) { * @param k2_rm Reference Model Gain 2nd order signal * @param k3_rm Reference Model Gain 3rd order signal */ -void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3]){ +void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3], float max_ang_jerk){ float e_x[3]; float e_x_rates[3]; float e_x_d[3]; @@ -643,6 +711,7 @@ void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[ err_nd(e_x_d, e_x_rates, x_d_ref, k2_rm, 3); err_nd(e_x_2d, e_x_d, x_2d_ref, k3_rm, 3); float_vect_copy(x_3d_ref,e_x_2d,3); + vect_bound_nd(x_3d_ref, max_ang_jerk, 3); if(ow_psi){x_3d_ref[2] = psi_overwrite[3];} integrate_nd(dt, x_2d_ref, x_3d_ref, 3); if(ow_psi){x_2d_ref[2] = psi_overwrite[2];} @@ -652,6 +721,9 @@ void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[ integrate_nd(dt, x_ref, x_d_eul_ref, 3); if(ow_psi){x_ref[2] = psi_overwrite[0];} NormRadAngle(x_ref[2]); + //printf("k1_rm: %f %f %f\n", k1_rm[0], k1_rm[1], k1_rm[2]); + //printf("k2_rm: %f %f %f\n", k2_rm[0], k2_rm[1], k2_rm[2]); + //printf("k3_rm: %f %f %f\n", k3_rm[0], k3_rm[1], k3_rm[2]); } /** @@ -721,7 +793,8 @@ void rm_3rd_pos(float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], floa float e_x_d[n]; float e_x_2d[n]; err_nd(e_x, x_des, x_ref, k1_rm, n); - vect_bound_nd(e_x,x_d_bound, n); + float max_x_d = bound_v_from_a(e_x, x_d_bound, x_2d_bound, n); + vect_bound_nd(e_x, max_x_d, n); err_nd(e_x_d, e_x, x_d_ref, k2_rm, n); vect_bound_nd(e_x_d,x_2d_bound, n); err_nd(e_x_2d, e_x_d, x_2d_ref, k3_rm, n); @@ -815,7 +888,19 @@ static float ec_3rd(float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, float y_4d = k1_e*(x_ref-x)+k2_e*(x_d_ref-x_d)+k3_e*(x_2d_ref-x_2d)+x_3d_ref; return y_4d; } +void ec_3rd_pos( float y_4d[], float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x[], float x_d[], float x_2d[], float k1_e[], float k2_e[], float k3_e[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n){ + float e_x_d[n]; + float e_x_2d[n]; + err_sum_nd(e_x_d, x_ref, x, k1_e, x_d_ref, n); + vect_bound_nd(e_x_d, x_d_bound, n); + + err_sum_nd(e_x_2d, e_x_d, x_d, k2_e, x_2d_ref, n); + vect_bound_nd(e_x_2d,x_2d_bound, n); + + err_sum_nd(y_4d, e_x_2d, x_2d, k3_e, x_3d_ref, n); + vect_bound_nd(y_4d, x_3d_bound, n); +} /** * @brief Error Controller Definition for 3rd order system specific to attitude * @param dt Delta time [s] @@ -831,7 +916,7 @@ static float ec_3rd(float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, * @param k2_e Error Controller Gain 2nd order signal * @param k3_e Error Controller Gain 3rd order signal */ -void ec_3rd_att(float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3]){ +void ec_3rd_att(float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3], float max_ang_jerk){ float y_4d_1[3]; float y_4d_2[3]; float y_4d_3[3]; @@ -846,6 +931,7 @@ void ec_3rd_att(float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[ float_vect_sum(y_4d, y_4d, y_4d_1, 3); float_vect_sum(y_4d, y_4d, y_4d_2, 3); float_vect_sum(y_4d, y_4d, y_4d_3, 3); + vect_bound_nd(y_4d, max_ang_jerk, 3); } /** @@ -872,7 +958,7 @@ static float w_approx(float p1, float p2, float p3, float rm_k){ void init_poles(void){ // Attitude Controller Poles---------------------------------------------------------- - float slow_pole = 10.1; // Pole of the slowest dynamics used in the attitude controller + float slow_pole = 15.1; // Pole of the slowest dynamics used in the attitude controller p_att_e.omega_n = 4.50; p_att_e.zeta = 1.0; @@ -890,11 +976,11 @@ void init_poles(void){ p_head_rm.zeta = 1.0; p_head_rm.p3 = p_head_rm.omega_n * p_head_rm.zeta; - act_dynamics[ONELOOP_ANDI_PHI_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); - act_dynamics[ONELOOP_ANDI_THETA_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[COMMAND_ROLL] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); // Position Controller Poles---------------------------------------------------------- - slow_pole = act_dynamics[ONELOOP_ANDI_PHI_IDX]; // Pole of the slowest dynamics used in the position controller + slow_pole = act_dynamics[COMMAND_ROLL]; // Pole of the slowest dynamics used in the position controller p_pos_e.omega_n = 1.0; p_pos_e.zeta = 1.0; @@ -904,11 +990,11 @@ void init_poles(void){ p_pos_rm.zeta = 1.0; p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; - p_alt_e.omega_n = 3.0; + p_alt_e.omega_n = 1.0;// 3.0; p_alt_e.zeta = 1.0; p_alt_e.p3 = slow_pole; - p_alt_rm.omega_n = 1.93; + p_alt_rm.omega_n = 0.93; //1.93; p_alt_rm.zeta = 1.0; p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; } @@ -918,7 +1004,9 @@ void init_poles(void){ */ void init_controller(void){ /*Register a variable from nav_hybrid. Should be improved when nav hybrid is final.*/ - max_v_nav = nav_max_speed; + float max_wind = 20.0; + max_v_nav = nav_max_speed + max_wind; + max_a_nav = nav_max_acceleration_sp; /*Some calculations in case new poles have been specified*/ p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; @@ -940,7 +1028,9 @@ void init_controller(void){ k_att_rm.k1[1] = k_att_rm.k1[0]; k_att_rm.k2[1] = k_att_rm.k2[0]; k_att_rm.k3[1] = k_att_rm.k3[0]; - + + //printf("Attitude RM Gains: %f %f %f\n", k_att_rm.k1[0], k_att_rm.k2[0], k_att_rm.k3[0]); + //printf("Attitude E Gains: %f %f %f\n", k_att_e.k1[0], k_att_e.k2[0], k_att_e.k3[0]); /*Heading Loop NAV*/ k_att_e.k1[2] = k_e_1_3_f_v2(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3); k_att_e.k2[2] = k_e_2_3_f_v2(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3); @@ -951,9 +1041,12 @@ void init_controller(void){ k_att_rm.k3[2] = k_rm_3_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3); /*Position Loop*/ - k_pos_e.k1[0] = k_e_1_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); - k_pos_e.k2[0] = k_e_2_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); - k_pos_e.k3[0] = k_e_3_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); + // k_pos_e.k1[0] = k_e_1_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); + // k_pos_e.k2[0] = k_e_2_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); + // k_pos_e.k3[0] = k_e_3_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); + k_pos_e.k1[0] = k_rm_1_3_f(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); + k_pos_e.k2[0] = k_rm_2_3_f(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); + k_pos_e.k3[0] = k_rm_3_3_f(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); k_pos_e.k1[1] = k_pos_e.k1[0]; k_pos_e.k2[1] = k_pos_e.k2[0]; k_pos_e.k3[1] = k_pos_e.k3[0]; @@ -968,9 +1061,13 @@ void init_controller(void){ nav_hybrid_max_bank = ONELOOP_ANDI_MAX_BANK; /*Altitude Loop*/ - k_pos_e.k1[2] = k_e_1_3_f_v2(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); - k_pos_e.k2[2] = k_e_2_3_f_v2(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); - k_pos_e.k3[2] = k_e_3_3_f_v2(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); + // k_pos_e.k1[2] = k_e_1_3_f_v2(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); + // k_pos_e.k2[2] = k_e_2_3_f_v2(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); + // k_pos_e.k3[2] = k_e_3_3_f_v2(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); + k_pos_e.k1[2] = k_rm_1_3_f(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); + k_pos_e.k2[2] = k_rm_2_3_f(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); + k_pos_e.k3[2] = k_rm_3_3_f(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); + k_pos_rm.k1[2] = k_rm_1_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); k_pos_rm.k2[2] = k_rm_2_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); k_pos_rm.k3[2] = k_rm_3_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); @@ -1001,17 +1098,18 @@ void init_controller(void){ k_pos_e_indi.k1[2] = k_e_1_2_f_v2(p_alt_e.omega_n, p_alt_e.zeta); k_pos_e_indi.k2[2] = k_e_2_2_f_v2(p_alt_e.omega_n, p_alt_e.zeta); k_pos_e_indi.k3[2] = 1.0; - + //------------------------------------------------------------------------------------------ /*Approximated Dynamics*/ - act_dynamics[ONELOOP_ANDI_PHI_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); - act_dynamics[ONELOOP_ANDI_THETA_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[COMMAND_ROLL] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); } /** @brief Initialize the filters */ void init_filter(void) { + //printf("oneloop andi filt cutoff PQR: %f %f %f\n", oneloop_andi_filt_cutoff_p,oneloop_andi_filt_cutoff_q,oneloop_andi_filt_cutoff_r); float tau = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff); float tau_a = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff_a); float tau_v = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff_v); @@ -1020,7 +1118,7 @@ void init_filter(void) // Filtering of the Inputs with 3 dimensions (e.g. rates and accelerations) int8_t i; for (i = 0; i < 3; i++) { - init_butterworth_2_low_pass(&att_dot_meas_lowpass_filters[i], tau, sample_time, 0.0); + init_butterworth_2_low_pass(&ang_rate_lowpass_filters[i], tau, sample_time, 0.0); init_butterworth_2_low_pass(&model_pred_aa_filt[i], tau, sample_time, 0.0); init_butterworth_2_low_pass(&filt_accel_ned[i], tau_a, sample_time, 0.0 ); init_butterworth_2_low_pass(&filt_veloc_ned[i], tau_v, sample_time, 0.0 ); @@ -1032,6 +1130,9 @@ void init_filter(void) init_butterworth_2_low_pass(&rates_filt_bt[0], time_constants[0], sample_time, stateGetBodyRates_f()->p); init_butterworth_2_low_pass(&rates_filt_bt[1], time_constants[1], sample_time, stateGetBodyRates_f()->q); init_butterworth_2_low_pass(&rates_filt_bt[2], time_constants[2], sample_time, stateGetBodyRates_f()->r); + init_butterworth_2_low_pass(&model_pred_ar_filt[0], time_constants[0], sample_time, 0.0); + init_butterworth_2_low_pass(&model_pred_ar_filt[1], time_constants[1], sample_time, 0.0); + init_butterworth_2_low_pass(&model_pred_ar_filt[2], time_constants[2], sample_time, 0.0); // Some other filters init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0); @@ -1051,22 +1152,25 @@ void oneloop_andi_propagate_filters(void) { update_butterworth_2_low_pass(&filt_veloc_ned[0], veloc->x); update_butterworth_2_low_pass(&filt_veloc_ned[1], veloc->y); update_butterworth_2_low_pass(&filt_veloc_ned[2], veloc->z); - calc_model(oneloop_andi.ctrl_type); + calc_model(); int8_t i; for (i = 0; i < 3; i++) { update_butterworth_2_low_pass(&model_pred_aa_filt[i], model_pred[3+i]); update_butterworth_2_low_pass(&model_pred_la_filt[i], model_pred[i]); - update_butterworth_2_low_pass(&att_dot_meas_lowpass_filters[i], rate_vect[i]); + update_butterworth_2_low_pass(&ang_rate_lowpass_filters[i], rate_vect[i]); update_butterworth_2_low_pass(&rates_filt_bt[i], rate_vect[i]); - - ang_acc[i] = (att_dot_meas_lowpass_filters[i].o[0]- att_dot_meas_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY + model_pred[3+i] - model_pred_aa_filt[i].o[0]; - lin_acc[i] = filt_accel_ned[i].o[0] + model_pred[i] - model_pred_la_filt[i].o[0]; + lin_acc[i] = filt_accel_ned[i].o[0] + model_pred[i] - model_pred_la_filt[i].o[0]; + ang_acc[i] = (ang_rate_lowpass_filters[i].o[0]- ang_rate_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY + model_pred[3+i] - model_pred_aa_filt[i].o[0]; + model_pred_ar[i] = model_pred_ar[i] + ang_acc[i] / PERIODIC_FREQUENCY; + update_butterworth_2_low_pass(&model_pred_ar_filt[i], model_pred_ar[i]); + ang_rate[i] = rates_filt_bt[i].o[0] + model_pred_ar[i] - model_pred_ar_filt[i].o[0]; } // Propagate filter for sideslip correction float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y); update_butterworth_2_low_pass(&accely_filt, accely); float airspeed_meas = stateGetAirspeed_f(); + Bound(airspeed_meas, 0.0, 30.0); update_butterworth_2_low_pass(&airspeed_filt, airspeed_meas); } @@ -1083,11 +1187,10 @@ void oneloop_andi_init(void) } // Initialize Effectiveness matrix calc_normalization(); - sum_g1g2_1l(oneloop_andi.ctrl_type); + G1G2_oneloop(oneloop_andi.ctrl_type); for (i = 0; i < ANDI_OUTPUTS; i++) { - bwls_1l[i] = g1g2_1l[i]; + bwls_1l[i] = EFF_MAT_G[i]; } - // Initialize filters and other variables init_filter(); init_controller(); @@ -1100,19 +1203,24 @@ void oneloop_andi_init(void) float_vect_zero(oneloop_andi.sta_ref.att_2d,3); float_vect_zero(oneloop_andi.sta_ref.att_3d,3); float_vect_zero(nu, ANDI_OUTPUTS); + float_vect_zero(nu_n, ANDI_OUTPUTS); float_vect_zero(ang_acc,3); float_vect_zero(lin_acc,3); float_vect_zero(nav_target,3); + float_vect_zero(nav_target_new,3); eulers_zxy_des.phi = 0.0; eulers_zxy_des.theta = 0.0; eulers_zxy_des.psi = 0.0; float_vect_zero(model_pred,ANDI_OUTPUTS); + float_vect_zero(model_pred_ar,3); // Start telemetry #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_oneloop_andi); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_G, send_eff_mat_g_oneloop_andi); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_STAB, send_eff_mat_stab_oneloop_andi); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_GUID, send_eff_mat_guid_oneloop_andi); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE, send_guidance_oneloop_andi); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATOR_STATE, send_oneloop_actuator_state); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_VECT, send_oneloop_debug); #endif } @@ -1125,12 +1233,17 @@ void oneloop_andi_init(void) */ void oneloop_andi_enter(bool half_loop_sp, int ctrl_type) { + ele_min = 0.0; oneloop_andi.half_loop = half_loop_sp; oneloop_andi.ctrl_type = ctrl_type; psi_des_rad = eulers_zxy.psi; psi_des_deg = DegOfRad(eulers_zxy.psi); calc_normalization(); - sum_g1g2_1l(oneloop_andi.ctrl_type); + G1G2_oneloop(oneloop_andi.ctrl_type); + int8_t i; + for (i = 0; i < ANDI_OUTPUTS; i++) { + bwls_1l[i] = EFF_MAT_G[i]; + } init_filter(); init_controller(); /* Stabilization Reset */ @@ -1141,10 +1254,12 @@ void oneloop_andi_enter(bool half_loop_sp, int ctrl_type) float_vect_zero(ang_acc,3); float_vect_zero(lin_acc,3); float_vect_zero(nav_target,3); + float_vect_zero(nav_target_new,3); eulers_zxy_des.phi = 0.0; eulers_zxy_des.theta = 0.0; eulers_zxy_des.psi = psi_des_rad; float_vect_zero(model_pred,ANDI_OUTPUTS); + float_vect_zero(model_pred_ar,3); /*Guidance Reset*/ } @@ -1155,7 +1270,7 @@ void oneloop_andi_enter(bool half_loop_sp, int ctrl_type) * @param rm_order_h Order of the reference model for horizontal guidance * @param rm_order_v Order of the reference model for vertical guidance */ -void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v) +void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop) { // Initialize some variables a_thrust = 0.0; @@ -1183,22 +1298,25 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, float delta_psi_des_rad = des_r * dt_1l; // Integrate desired Yaw rate to get desired change in yaw float delta_psi_rad = eulers_zxy_des.psi-eulers_zxy.psi; // Calculate current yaw difference between des and actual NormRadAngle(delta_psi_rad); // Normalize the difference - if (fabs(delta_psi_rad) > RadOfDeg(10.0)){ // If difference is bigger than 10 deg do not further increment desired + if (fabs(delta_psi_rad) > RadOfDeg(30.0)){ // If difference is bigger than 10 deg do not further increment desired delta_psi_des_rad = 0.0; } psi_des_rad += delta_psi_des_rad; // Incrementdesired yaw NormRadAngle(psi_des_rad); - eulers_zxy_des.psi = psi_des_rad; // Register Attitude Setpoints from previous loop + if (!in_flight_oneloop){ + psi_des_rad = eulers_zxy.psi; + } + eulers_zxy_des.psi = psi_des_rad; float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, eulers_zxy_des.psi}; // Create commands adhoc to get actuators to the wanted level thrust_cmd_1l = (float) radio_control_get(RADIO_THROTTLE); Bound(thrust_cmd_1l,0.0,MAX_PPRZ); int8_t i; for (i = 0; i < ANDI_NUM_ACT; i++) { - a_thrust +=(thrust_cmd_1l - use_increment*actuator_state_1l[i]) * bwls_1l[2][i] / (ratio_u_un[i] * ratio_vn_v[i]); + a_thrust +=(thrust_cmd_1l - use_increment*actuator_state_1l[i]) * bwls_1l[2][i] / (ratio_u_un[i] * ratio_vn_v[aD]); } - rm_3rd_attitude(dt_1l, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, att_des, false, psi_vec, k_att_rm.k1, k_att_rm.k2, k_att_rm.k3); + rm_3rd_attitude(dt_1l, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, att_des, false, psi_vec, k_att_rm.k1, k_att_rm.k2, k_att_rm.k3, max_j_ang); }else{ // Make sure X and Y jerk objectives are active Wv_wls[0] = Wv[0]; @@ -1208,7 +1326,10 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, rm_3rd_pos(dt_1l, oneloop_andi.gui_ref.pos, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target, k_pos_rm.k1, k_pos_rm.k2, k_pos_rm.k3, max_v_nav, max_a_nav, max_j_nav, 2); } else if (rm_order_h == 2){ float_vect_copy(oneloop_andi.gui_ref.pos, oneloop_andi.gui_state.pos,2); - rm_2nd_pos(dt_1l, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target, k_pos_rm.k2, k_pos_rm.k3, max_a_nav, max_j_nav, 2); + float_vect_copy(oneloop_andi.gui_ref.vel, oneloop_andi.gui_state.vel,2); + reshape_wind();//returns accel sp as nav target new + rm_1st_pos(dt_1l, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target_new, k_pos_rm.k3, max_j_nav, 2); + // rm_2nd_pos(dt_1l, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target_new, k_pos_rm.k2, k_pos_rm.k3, max_a_nav, max_j_nav, 2); } else if (rm_order_h == 1){ float_vect_copy(oneloop_andi.gui_ref.pos, oneloop_andi.gui_state.pos,2); float_vect_copy(oneloop_andi.gui_ref.vel, oneloop_andi.gui_state.vel,2); @@ -1222,14 +1343,25 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, } } else { float ref_mag_vel = float_vect_norm(oneloop_andi.gui_ref.vel,2); - if (ref_mag_vel > 3.0){ - psi_des_rad = atan2f(oneloop_andi.gui_ref.vel[1],oneloop_andi.gui_ref.vel[0]); - } psi_des_rad += oneloop_andi_sideslip() * dt_1l; NormRadAngle(psi_des_rad); + if (ref_mag_vel > 3.0){ + float psi_gs = atan2f(oneloop_andi.gui_ref.vel[1],oneloop_andi.gui_ref.vel[0]); + float delta_des_gs = psi_gs-psi_des_rad; // Calculate current yaw difference between des and gs + NormRadAngle(delta_des_gs); + if (fabs(delta_des_gs) > RadOfDeg(60.0)){ // If difference is bigger than 60 deg bound the des psi angle so that it does not deviate too much + delta_des_gs *= RadOfDeg(60.0)/fabs(delta_des_gs); + psi_des_rad = psi_gs - delta_des_gs; + NormRadAngle(psi_des_rad); + } + } } // Register Attitude Setpoints from previous loop - float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, psi_des_rad}; + if (!in_flight_oneloop){ + psi_des_rad = eulers_zxy.psi; + } + eulers_zxy_des.psi = psi_des_rad; + float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, eulers_zxy_des.psi}; // The RM functions want an array as input. Create a single entry array and write the vertical guidance entries. float single_value_ref[1] = {oneloop_andi.gui_ref.pos[2]}; float single_value_d_ref[1] = {oneloop_andi.gui_ref.vel[2]}; @@ -1239,9 +1371,8 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, float single_value_k1_rm[1] = {k_pos_rm.k1[2]}; float single_value_k2_rm[1] = {k_pos_rm.k2[2]}; float single_value_k3_rm[1] = {k_pos_rm.k3[2]}; - if (rm_order_v == 3){ - rm_3rd_pos(dt_1l, single_value_ref, single_value_d_ref, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k1_rm, single_value_k2_rm, single_value_k3_rm, max_v_nav, max_a_nav, max_j_nav, 1); + rm_3rd_pos(dt_1l, single_value_ref, single_value_d_ref, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k1_rm, single_value_k2_rm, single_value_k3_rm, max_v_nav_v, max_a_nav, max_j_nav, 1); oneloop_andi.gui_ref.pos[2] = single_value_ref[0]; oneloop_andi.gui_ref.vel[2] = single_value_d_ref[0]; oneloop_andi.gui_ref.acc[2] = single_value_2d_ref[0]; @@ -1264,7 +1395,7 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, // Generate Reference signals for attitude using RM // FIX ME ow not yet defined, will be useful in the future to have accurate psi tracking in NAV functions bool ow_psi = false; - rm_3rd_attitude(dt_1l, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, att_des, ow_psi, psi_vec, k_att_rm.k1, k_att_rm.k2, k_att_rm.k3); + rm_3rd_attitude(dt_1l, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, att_des, ow_psi, psi_vec, k_att_rm.k1, k_att_rm.k2, k_att_rm.k3, max_j_ang); } } @@ -1283,7 +1414,11 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, init_controller(); calc_normalization(); get_act_state_oneloop(); - sum_g1g2_1l(oneloop_andi.ctrl_type); + G1G2_oneloop(oneloop_andi.ctrl_type); + int8_t i; + for (i = 0; i < ANDI_OUTPUTS; i++) { + bwls_1l[i] = EFF_MAT_G[i]; + } // If drone is not on the ground use incremental law use_increment = 0.0; @@ -1291,23 +1426,23 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, if(in_flight) { use_increment = 1.0; in_flight_oneloop = true; - } + } if (ONELOOP_ANDI_DEBUG_MODE) { in_flight_oneloop = false; } // Register the state of the drone in the variables used in RM and EC // (1) Attitude related - oneloop_andi.sta_state.att[0] = eulers_zxy.phi * use_increment; - oneloop_andi.sta_state.att[1] = eulers_zxy.theta * use_increment; - oneloop_andi.sta_state.att[2] = eulers_zxy.psi * use_increment; + oneloop_andi.sta_state.att[0] = eulers_zxy.phi ; + oneloop_andi.sta_state.att[1] = eulers_zxy.theta; + oneloop_andi.sta_state.att[2] = eulers_zxy.psi ; oneloop_andi_propagate_filters(); //needs to be after update of attitude vector - oneloop_andi.sta_state.att_d[0] = rates_filt_bt[0].o[0] * use_increment; - oneloop_andi.sta_state.att_d[1] = rates_filt_bt[1].o[0] * use_increment; - oneloop_andi.sta_state.att_d[2] = rates_filt_bt[2].o[0] * use_increment; - oneloop_andi.sta_state.att_2d[0] = ang_acc[0] * use_increment; - oneloop_andi.sta_state.att_2d[1] = ang_acc[1] * use_increment; - oneloop_andi.sta_state.att_2d[2] = ang_acc[2] * use_increment; + oneloop_andi.sta_state.att_d[0] = ang_rate[0];//rates_filt_bt[0].o[0]; + oneloop_andi.sta_state.att_d[1] = ang_rate[1];//rates_filt_bt[1].o[0]; + oneloop_andi.sta_state.att_d[2] = ang_rate[2];//rates_filt_bt[2].o[0]; + oneloop_andi.sta_state.att_2d[0] = ang_acc[0] ; + oneloop_andi.sta_state.att_2d[1] = ang_acc[1] ; + oneloop_andi.sta_state.att_2d[2] = ang_acc[2] ; // (2) Position related oneloop_andi.gui_state.pos[0] = stateGetPositionNed_f()->x; oneloop_andi.gui_state.pos[1] = stateGetPositionNed_f()->y; @@ -1319,49 +1454,45 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, oneloop_andi.gui_state.acc[1] = lin_acc[1]; oneloop_andi.gui_state.acc[2] = lin_acc[2]; - // Update the effectiveness matrix used in WLS - int8_t i; - for (i = 0; i < ANDI_OUTPUTS; i++) { - bwls_1l[i] = g1g2_1l[i]; - } // Calculated feedforward signal for yaW CONTROL g2_ff = 0.0; + for (i = 0; i < ANDI_NUM_ACT; i++) { if (oneloop_andi.ctrl_type == CTRL_ANDI){ - g2_ff += g2_1l[i] * act_dynamics[i] * andi_du[i]; + g2_ff += G2_RW[i] * act_dynamics[i] * andi_du[i]; } else if (oneloop_andi.ctrl_type == CTRL_INDI){ - g2_ff += g2_1l[i]* andi_du_n[i]; + g2_ff += G2_RW[i]* andi_du_n[i]; } } //G2 is scaled by ANDI_G_SCALING to make it readable - g2_ff = g2_ff / ANDI_G_SCALING; + g2_ff = g2_ff; // Run the Reference Model (RM) - oneloop_andi_RM(half_loop, PSA_des, rm_order_h, rm_order_v); - + oneloop_andi_RM(half_loop, PSA_des, rm_order_h, rm_order_v, in_flight_oneloop); // Guidance Pseudo Control Vector (nu) based on error controller if(half_loop){ nu[0] = 0.0; nu[1] = 0.0; nu[2] = a_thrust; + ctrl_off = false; }else{ if(oneloop_andi.ctrl_type == CTRL_ANDI){ - nu[0] = ec_3rd(oneloop_andi.gui_ref.pos[0], oneloop_andi.gui_ref.vel[0], oneloop_andi.gui_ref.acc[0], oneloop_andi.gui_ref.jer[0], oneloop_andi.gui_state.pos[0], oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.acc[0], k_pos_e.k1[0], k_pos_e.k2[0], k_pos_e.k3[0]); - nu[1] = ec_3rd(oneloop_andi.gui_ref.pos[1], oneloop_andi.gui_ref.vel[1], oneloop_andi.gui_ref.acc[1], oneloop_andi.gui_ref.jer[1], oneloop_andi.gui_state.pos[1], oneloop_andi.gui_state.vel[1], oneloop_andi.gui_state.acc[1], k_pos_e.k1[1], k_pos_e.k2[1], k_pos_e.k3[1]); - nu[2] = ec_3rd(oneloop_andi.gui_ref.pos[2], oneloop_andi.gui_ref.vel[2], oneloop_andi.gui_ref.acc[2], oneloop_andi.gui_ref.jer[2], oneloop_andi.gui_state.pos[2], oneloop_andi.gui_state.vel[2], oneloop_andi.gui_state.acc[2], k_pos_e.k1[2], k_pos_e.k2[2], k_pos_e.k3[2]); + ec_3rd_pos(nu, oneloop_andi.gui_ref.pos, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, oneloop_andi.gui_state.pos, oneloop_andi.gui_state.vel, oneloop_andi.gui_state.acc, k_pos_e.k1, k_pos_e.k2, k_pos_e.k3, max_v_nav, max_a_nav, max_j_nav, 3); + // nu[0] = ec_3rd(oneloop_andi.gui_ref.pos[0], oneloop_andi.gui_ref.vel[0], oneloop_andi.gui_ref.acc[0], oneloop_andi.gui_ref.jer[0], oneloop_andi.gui_state.pos[0], oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.acc[0], k_pos_e.k1[0], k_pos_e.k2[0], k_pos_e.k3[0]); + // nu[1] = ec_3rd(oneloop_andi.gui_ref.pos[1], oneloop_andi.gui_ref.vel[1], oneloop_andi.gui_ref.acc[1], oneloop_andi.gui_ref.jer[1], oneloop_andi.gui_state.pos[1], oneloop_andi.gui_state.vel[1], oneloop_andi.gui_state.acc[1], k_pos_e.k1[1], k_pos_e.k2[1], k_pos_e.k3[1]); + // nu[2] = ec_3rd(oneloop_andi.gui_ref.pos[2], oneloop_andi.gui_ref.vel[2], oneloop_andi.gui_ref.acc[2], oneloop_andi.gui_ref.jer[2], oneloop_andi.gui_state.pos[2], oneloop_andi.gui_state.vel[2], oneloop_andi.gui_state.acc[2], k_pos_e.k1[2], k_pos_e.k2[2], k_pos_e.k3[2]); } else if (oneloop_andi.ctrl_type == CTRL_INDI){ nu[0] = ec_3rd(oneloop_andi.gui_ref.pos[0], oneloop_andi.gui_ref.vel[0], oneloop_andi.gui_ref.acc[0], 0.0, oneloop_andi.gui_state.pos[0], oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.acc[0], k_pos_e_indi.k1[0], k_pos_e_indi.k2[0], k_pos_e_indi.k3[0]); nu[1] = ec_3rd(oneloop_andi.gui_ref.pos[1], oneloop_andi.gui_ref.vel[1], oneloop_andi.gui_ref.acc[1], 0.0, oneloop_andi.gui_state.pos[1], oneloop_andi.gui_state.vel[1], oneloop_andi.gui_state.acc[1], k_pos_e_indi.k1[1], k_pos_e_indi.k2[1], k_pos_e_indi.k3[1]); nu[2] = ec_3rd(oneloop_andi.gui_ref.pos[2], oneloop_andi.gui_ref.vel[2], oneloop_andi.gui_ref.acc[2], 0.0, oneloop_andi.gui_state.pos[2], oneloop_andi.gui_state.vel[2], oneloop_andi.gui_state.acc[2], k_pos_e_indi.k1[2], k_pos_e_indi.k2[2], k_pos_e_indi.k3[2]); } } - // Attitude Pseudo Control Vector (nu) based on error controller float y_4d_att[3]; if(oneloop_andi.ctrl_type == CTRL_ANDI){ - ec_3rd_att(y_4d_att, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, oneloop_andi.sta_state.att, oneloop_andi.sta_state.att_d, oneloop_andi.sta_state.att_2d, k_att_e.k1, k_att_e.k2, k_att_e.k3); + ec_3rd_att(y_4d_att, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, oneloop_andi.sta_state.att, oneloop_andi.sta_state.att_d, oneloop_andi.sta_state.att_2d, k_att_e.k1, k_att_e.k2, k_att_e.k3, max_j_ang); } else if (oneloop_andi.ctrl_type == CTRL_INDI){ float dummy0[3] = {0.0, 0.0, 0.0}; - ec_3rd_att(y_4d_att, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, dummy0, oneloop_andi.sta_state.att, oneloop_andi.sta_state.att_d, oneloop_andi.sta_state.att_2d, k_att_e_indi.k1, k_att_e_indi.k2, k_att_e_indi.k3); + ec_3rd_att(y_4d_att, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, dummy0, oneloop_andi.sta_state.att, oneloop_andi.sta_state.att_d, oneloop_andi.sta_state.att_2d, k_att_e_indi.k1, k_att_e_indi.k2, k_att_e_indi.k3, max_j_ang); } nu[3] = y_4d_att[0]; nu[4] = y_4d_att[1]; @@ -1371,23 +1502,77 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, pitch_pref = pitch_pref / MAX_PPRZ * theta_pref_max; Bound(pitch_pref,0.0,theta_pref_max); } - u_pref[ONELOOP_ANDI_THETA_IDX] = pitch_pref; + u_pref[COMMAND_PITCH] = pitch_pref; // Calculate the min and max increments for (i = 0; i < ANDI_NUM_ACT_TOT; i++) { - if(i 25.0){ + du_min_1l[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i]; + du_max_1l[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i]; + } else { + du_min_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i]; + du_max_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i]; + } + du_pref_1l[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i]; + break; + case COMMAND_FLAPS: + if(RW.skew.deg > 50.0){ + du_min_1l[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i]; + du_max_1l[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i]; + } else { + du_min_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i]; + du_max_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i]; + } + du_pref_1l[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i]; + break; + case COMMAND_ELEVATOR: + du_min_1l[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i]; + du_max_1l[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i]; + u_pref[i] = RW.ele_pref; + du_pref_1l[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i]; + break; + case COMMAND_ROLL: + du_min_1l[i] = (act_min[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + du_max_1l[i] = (act_max[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + du_pref_1l[i] = (u_pref[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + break; + case COMMAND_PITCH: + if (rotwing_state_settings.stall_protection){ + du_min_1l[i] = (RadOfDeg(-17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + du_max_1l[i] = (RadOfDeg(17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + } else { + du_min_1l[i] = (act_min[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + du_max_1l[i] = (act_max[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + } + du_pref_1l[i] = (u_pref[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; + break; } } - - // WLS Control Allocator - number_iter = wls_alloc(andi_du_n, nu, du_min_1l, du_max_1l, bwls_1l, 0, 0, Wv_wls, Wu, du_pref_1l, gamma_wls, 10, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS); + // WLS Control Allocator + normalize_nu(); + number_iter = wls_alloc(andi_du_n, nu_n, du_min_1l, du_max_1l, bwls_1l, 0, 0, Wv_wls, Wu, du_pref_1l, gamma_wls, 10, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS); for (i = 0; i < ANDI_NUM_ACT_TOT; i++){ andi_du[i] = (float)(andi_du_n[i] * ratio_u_un[i]); } @@ -1395,18 +1580,20 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, if (in_flight_oneloop) { // Add the increments to the actuators float_vect_sum(andi_u, actuator_state_1l, andi_du, ANDI_NUM_ACT); - andi_u[ONELOOP_ANDI_PHI_IDX] = andi_du[ONELOOP_ANDI_PHI_IDX] + oneloop_andi.sta_state.att[0]; - andi_u[ONELOOP_ANDI_THETA_IDX] = andi_du[ONELOOP_ANDI_THETA_IDX] + oneloop_andi.sta_state.att[1]; + andi_u[COMMAND_ROLL] = andi_du[COMMAND_ROLL] + oneloop_andi.sta_state.att[0]; + andi_u[COMMAND_PITCH] = andi_du[COMMAND_PITCH] + oneloop_andi.sta_state.att[1]; } else { // Not in flight, so don't increment float_vect_copy(andi_u, andi_du, ANDI_NUM_ACT); - andi_u[ONELOOP_ANDI_PHI_IDX] = andi_du[ONELOOP_ANDI_PHI_IDX]; - andi_u[ONELOOP_ANDI_THETA_IDX] = andi_du[ONELOOP_ANDI_THETA_IDX]; + andi_u[COMMAND_ROLL] = andi_du[COMMAND_ROLL]; + andi_u[COMMAND_PITCH] = andi_du[COMMAND_PITCH]; } - if ((ONELOOP_ANDI_AC_HAS_PUSHER)&&(half_loop)){ - andi_u[ONELOOP_ANDI_PUSHER_IDX] = radio_control.values[RADIO_AUX4]; +#ifdef COMMAND_MOTOR_PUSHER + if ((half_loop)){ + andi_u[COMMAND_MOTOR_PUSHER] = radio_control.values[RADIO_AUX4]; } +#endif // TODO : USE THE PROVIDED MAX AND MIN and change limits for phi and theta // Bound the inputs to the actuators for (i = 0; i < ANDI_NUM_ACT_TOT; i++) { @@ -1416,25 +1603,25 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, /*Commit the actuator command*/ stabilization.cmd[COMMAND_THRUST] = 0; for (i = 0; i < ANDI_NUM_ACT; i++) { - actuators_pprz[i] = (int16_t) andi_u[i]; - stabilization.cmd[COMMAND_THRUST] += actuator_state_1l[i]; + //actuators_pprz[i] = (int16_t) andi_u[i]; + commands[i] = (int16_t) andi_u[i]; } - stabilization.cmd[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST]/num_thrusters_oneloop; - autopilot.throttle = stabilization_cmd[COMMAND_THRUST]; - + commands[COMMAND_THRUST] = (commands[COMMAND_MOTOR_FRONT] + commands[COMMAND_MOTOR_RIGHT] + commands[COMMAND_MOTOR_BACK] + commands[COMMAND_MOTOR_LEFT])/num_thrusters_oneloop; + autopilot.throttle = commands[COMMAND_THRUST]; if(autopilot.mode==AP_MODE_ATTITUDE_DIRECT){ - eulers_zxy_des.phi = andi_u[ONELOOP_ANDI_PHI_IDX]; - eulers_zxy_des.theta = andi_u[ONELOOP_ANDI_THETA_IDX]; + eulers_zxy_des.phi = andi_u[COMMAND_ROLL]; + eulers_zxy_des.theta = andi_u[COMMAND_PITCH]; } else { - eulers_zxy_des.phi = andi_u[ONELOOP_ANDI_PHI_IDX]; - eulers_zxy_des.theta = andi_u[ONELOOP_ANDI_THETA_IDX]; + eulers_zxy_des.phi = andi_u[COMMAND_ROLL]; + eulers_zxy_des.theta = andi_u[COMMAND_PITCH]; } if (heading_manual){ psi_des_deg = DegOfRad(psi_des_rad); } - stabilization_cmd[COMMAND_ROLL] = (int16_t) (DegOfRad(eulers_zxy_des.phi ) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_PHI )); - stabilization_cmd[COMMAND_PITCH] = (int16_t) (DegOfRad(eulers_zxy_des.theta) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_THETA)); - stabilization_cmd[COMMAND_YAW] = (int16_t) (psi_des_deg * MAX_PPRZ / 180.0); + + stabilization.cmd[COMMAND_ROLL] = (int16_t) (DegOfRad(eulers_zxy_des.phi ) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_PHI )); + stabilization.cmd[COMMAND_PITCH] = (int16_t) (DegOfRad(eulers_zxy_des.theta) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_THETA)); + stabilization.cmd[COMMAND_YAW] = (int16_t) (psi_des_deg * MAX_PPRZ / 180.0); } /** @brief Function to reconstruct actuator state using first order dynamics */ @@ -1448,106 +1635,64 @@ void get_act_state_oneloop(void) if(!autopilot_get_motors_on()){ actuator_state_1l[i] = 0.0; } - Bound(actuator_state_1l[i],0,MAX_PPRZ); + Bound(actuator_state_1l[i],act_min[i], act_max[i]); } } /** - * @brief Function that sums g1 and g2 to obtain the g1_g2 matrix. It also undoes the scaling that was done to make the values readable + * @brief Function that samples and scales the effectiveness matrix * FIXME: make this function into a for loop to make it more adaptable to different configurations */ -void sum_g1g2_1l(int ctrl_type) { +void G1G2_oneloop(int ctrl_type) { int i = 0; - - // Trig of attitude angles - float sphi = sinf(eulers_zxy.phi); - float cphi = cosf(eulers_zxy.phi); - float stheta = sinf(eulers_zxy.theta); - float ctheta = cosf(eulers_zxy.theta); - float spsi = sinf(eulers_zxy.psi); - float cpsi = cosf(eulers_zxy.psi); - // Trig of skew - // float skew = 0.0; - // if (ONELOOP_ANDI_SCHEDULING){ - // skew = rotwing_state_skewing.wing_angle_deg; - // } - // float sskew = sinf(skew); - // float cskew = cosf(skew); - // float s2skew = sskew * sskew; - // float c2skew = cskew * cskew; - // float s3skew = sskew * s2skew; - // float c3skew = cskew * c2skew; - - // Thrust and Pusher force estimation - float T = -g/(cphi*ctheta);//minus gravity is a guesstimate of the thrust force, thrust measurement would be better - float P = 0.0; - if (ONELOOP_ANDI_AC_HAS_PUSHER){ - P = actuator_state_1l[ONELOOP_ANDI_PUSHER_IDX] * g1_1l[2][ONELOOP_ANDI_PUSHER_IDX] / ANDI_G_SCALING; - } - float scaler = 1.0; - float sched_p = 1.0; // Scheduler variable for roll axis - float sched_q = 1.0; // Scheduler variable for pitch axis - //float I_com + float scaler = 1.0; for (i = 0; i < ANDI_NUM_ACT_TOT; i++) { - // Effectiveness vector for real actuators (e.g. motors, servos) - if (i < ANDI_NUM_ACT){ - if(ctrl_type == CTRL_ANDI){ - scaler = act_dynamics[i] * ratio_u_un[i] * ratio_vn_v[i] / ANDI_G_SCALING; - } else if (ctrl_type == CTRL_INDI){ - scaler = ratio_u_un[i] * ratio_vn_v[i] / ANDI_G_SCALING; - } - // switch (i){ - // case 1: - // sched_p = cskew; - // sched_q = sskew * g1_1l[4][0] / g1_1l[4][1] * 0.9645; // sin(skew)*mF_effctiveness*ratio_of_arms - // break; - // case 3: - // sched_p = cskew; - // sched_q = sskew * g1_1l[4][2] / g1_1l[4][3] * 0.9645; // sin(skew)*mF_effctiveness*ratio_of_arms - // break; - // default: - // sched_p = 1.0; - // sched_q = 1.0; - // break; - // } - g1g2_1l[0][i] = (cpsi * stheta + ctheta * sphi * spsi) * g1_1l[2][i] * scaler; - g1g2_1l[1][i] = (spsi * stheta - cpsi * ctheta * sphi) * g1_1l[2][i] * scaler; - g1g2_1l[2][i] = (cphi * ctheta ) * g1_1l[2][i] * scaler; - g1g2_1l[3][i] = (g1_1l[3][i]) * scaler * sched_p; - g1g2_1l[4][i] = (g1_1l[4][i]) * scaler * sched_q; - g1g2_1l[5][i] = (g1_1l[5][i] + g2_1l[i]) * scaler; - if ((ONELOOP_ANDI_AC_HAS_PUSHER)&&(i==ONELOOP_ANDI_PUSHER_IDX)){ - g1g2_1l[0][i] = (cpsi * ctheta - sphi * spsi * stheta) * g1_1l[2][i] * scaler; - g1g2_1l[1][i] = (ctheta * spsi + cpsi * sphi * stheta) * g1_1l[2][i] * scaler; - g1g2_1l[2][i] = (- cphi * stheta ) * g1_1l[2][i] * scaler; - g1g2_1l[3][i] = 0.0; - g1g2_1l[4][i] = 0.0; - g1g2_1l[5][i] = 0.0; + + switch (i) { + case (COMMAND_MOTOR_FRONT): + case (COMMAND_MOTOR_RIGHT): + case (COMMAND_MOTOR_BACK): + case (COMMAND_MOTOR_LEFT): + case (COMMAND_MOTOR_PUSHER): + case (COMMAND_ELEVATOR): + case (COMMAND_RUDDER): + case (COMMAND_AILERONS): + case (COMMAND_FLAPS): + if(ctrl_type == CTRL_ANDI){ + scaler = act_dynamics[i] * ratio_u_un[i]; + } else if (ctrl_type == CTRL_INDI){ + scaler = ratio_u_un[i]; + } + break; + case (COMMAND_ROLL): + case (COMMAND_PITCH): + if(ctrl_type == CTRL_ANDI){ + scaler = act_dynamics[i] * ratio_u_un[i]; + } else if (ctrl_type == CTRL_INDI){ + scaler = ratio_u_un[i]; + } + break; + default: + break; + } + int j = 0; + for (j = 0; j < ANDI_OUTPUTS; j++) { + EFF_MAT_G[j][i] = EFF_MAT_RW[j][i] * scaler * ratio_vn_v[j]; + if (airspeed_filt.o[0] < ELE_MIN_AS && i == COMMAND_ELEVATOR){ + EFF_MAT_G[j][i] = 0.0; } - }else{ - if(ctrl_type == CTRL_ANDI){ - scaler = act_dynamics[i] * ratio_u_un[i] * ratio_vn_v[i]; - } else if (ctrl_type == CTRL_INDI){ - scaler = ratio_u_un[i] * ratio_vn_v[i]; - } - // Effectiveness vector for Phi (virtual actuator) - if (i == ONELOOP_ANDI_PHI_IDX){ - g1g2_1l[0][i] = ( cphi * ctheta * spsi * T - cphi * spsi * stheta * P) * scaler; - g1g2_1l[1][i] = (-cphi * ctheta * cpsi * T + cphi * cpsi * stheta * P) * scaler; - g1g2_1l[2][i] = (-sphi * ctheta * T + sphi * stheta * P) * scaler; - g1g2_1l[3][i] = 0.0; - g1g2_1l[4][i] = 0.0; - g1g2_1l[5][i] = 0.0; - } - // Effectiveness vector for Theta (virtual actuator) - if (i == ONELOOP_ANDI_THETA_IDX){ - g1g2_1l[0][i] = ((ctheta*cpsi - sphi*stheta*spsi) * T - (cpsi * stheta + ctheta * sphi * spsi) * P) * scaler; - g1g2_1l[1][i] = ((ctheta*spsi + sphi*stheta*cpsi) * T - (spsi * stheta - cpsi * ctheta * sphi) * P) * scaler; - g1g2_1l[2][i] = (-stheta * cphi * T - cphi * ctheta * P) * scaler; - g1g2_1l[3][i] = 0.0; - g1g2_1l[4][i] = 0.0; - g1g2_1l[5][i] = 0.0; + if (!rotwing_state_settings.hover_motors_active && i < 4){ + EFF_MAT_G[j][i] = 0.0; } + if (ctrl_off && i < 4 && j == 5){ //hack test + EFF_MAT_G[j][i] = 0.0; + } + if (ctrl_off && i < 4 && j == 4){ //hack test + EFF_MAT_G[j][i] = 0.0; + } + if (ctrl_off && i < 4 && j == 3){ //hack test + EFF_MAT_G[j][i] = 0.0; + } } } } @@ -1558,7 +1703,6 @@ void calc_normalization(void){ for (i = 0; i < ANDI_NUM_ACT_TOT; i++){ act_dynamics_d[i] = 1.0-exp(-act_dynamics[i]*dt_1l); Bound(act_dynamics_d[i],0.00001,1.0); - ratio_vn_v[i] = 1.0; Bound(act_max[i],0,MAX_PPRZ); Bound(act_min[i],-MAX_PPRZ,0); float ratio_numerator = act_max[i]-act_min[i]; @@ -1568,36 +1712,60 @@ void calc_normalization(void){ ratio_u_un[i] = ratio_numerator/ratio_denominator; ratio_u_un[i] = positive_non_zero(ratio_u_un[i]);// make sure ratio is not zero } + for (i = 0; i < ANDI_OUTPUTS; i++){ + float ratio_numerator = positive_non_zero(nu_norm_max); + float ratio_denominator = 1.0; + switch (i) { + case (aN): + case (aE): + case (aD): + ratio_denominator = positive_non_zero(max_j_nav); + ratio_vn_v[i] = ratio_numerator/ratio_denominator; + break; + case (ap): + case (aq): + case (ar): + ratio_denominator = positive_non_zero(max_j_ang); + ratio_vn_v[i] = ratio_numerator/ratio_denominator; + break; + } + } +} + +/** @brief Function to normalize the pseudo control vector */ +void normalize_nu(void){ + int8_t i; + for (i = 0; i < ANDI_OUTPUTS; i++){ + nu_n[i] = nu[i] * ratio_vn_v[i]; + } } /** @brief Function that calculates the model prediction for the complementary filter. */ -void calc_model(int ctrl_type){ +void calc_model(void){ int8_t i; int8_t j; - // Absolute Model Prediction : + // // Absolute Model Prediction : float sphi = sinf(eulers_zxy.phi); float cphi = cosf(eulers_zxy.phi); float stheta = sinf(eulers_zxy.theta); float ctheta = cosf(eulers_zxy.theta); float spsi = sinf(eulers_zxy.psi); float cpsi = cosf(eulers_zxy.psi); + // Thrust and Pusher force estimation + float L = RW.wing.L / RW.m; // Lift specific force + float T = RW.T / RW.m; // Thrust specific force. Minus gravity is a guesstimate. + float P = RW.P / RW.m; // Pusher specific force - float T = -g/(cphi*ctheta); // -9.81; - float P = 0.0; - if (ONELOOP_ANDI_AC_HAS_PUSHER){ - P = actuator_state_1l[ONELOOP_ANDI_PUSHER_IDX] * g1_1l[2][ONELOOP_ANDI_PUSHER_IDX] / ANDI_G_SCALING; - } - model_pred[0] = (cpsi * stheta + ctheta * sphi * spsi) * T + (cpsi * ctheta - sphi * spsi * stheta) * P; - model_pred[1] = (spsi * stheta - cpsi * ctheta * sphi) * T + (ctheta * spsi + cpsi * sphi * stheta) * P; - model_pred[2] = g + cphi * ctheta * T - cphi * stheta * P; - + model_pred[0] = -(cpsi * stheta + ctheta * sphi * spsi) * T + (cpsi * ctheta - sphi * spsi * stheta) * P - sphi * spsi * L; + model_pred[1] = -(spsi * stheta - cpsi * ctheta * sphi) * T + (ctheta * spsi + cpsi * sphi * stheta) * P + cpsi * sphi * L; + model_pred[2] = g - cphi * ctheta * T - cphi * stheta * P - cphi * L; for (i = 3; i < ANDI_OUTPUTS; i++){ // For loop for prediction of angular acceleration model_pred[i] = 0.0; // for (j = 0; j < ANDI_NUM_ACT; j++){ - if (ctrl_type == CTRL_ANDI){ - model_pred[i] = model_pred[i] + actuator_state_1l[j] * g1g2_1l[i][j] / (act_dynamics[j] * ratio_u_un[j] * ratio_vn_v[j]); - } else if (ctrl_type == CTRL_INDI){ - model_pred[i] = model_pred[i] + actuator_state_1l[j] * g1g2_1l[i][j] / (ratio_u_un[j] * ratio_vn_v[j]); + if(j == COMMAND_ELEVATOR){ + model_pred[i] = model_pred[i] + (actuator_state_1l[j] - RW.ele_pref) * EFF_MAT_RW[i][j]; // Ele pref is incidence angle + } else { + model_pred[i] = model_pred[i] + actuator_state_1l[j] * EFF_MAT_RW[i][j]; } } } @@ -1646,15 +1814,15 @@ float oneloop_andi_sideslip(void) { // Coordinated turn // feedforward estimate angular rotation omega = g*tan(phi)/v - float omega; + float omega = 0.0; const float max_phi = RadOfDeg(ONELOOP_ANDI_MAX_BANK); float airspeed_turn = airspeed_filt.o[0]; - Bound(airspeed_turn, 10.0f, 30.0f); + Bound(airspeed_turn, 1.0f, 30.0f); // Use the current roll angle to determine the corresponding heading rate of change. float coordinated_turn_roll = eulers_zxy.phi; // Prevent flipping - if( (andi_u[ONELOOP_ANDI_THETA_IDX] > 0.0f) && ( fabs(andi_u[ONELOOP_ANDI_PHI_IDX]) < andi_u[ONELOOP_ANDI_THETA_IDX])) { - coordinated_turn_roll = ((andi_u[ONELOOP_ANDI_PHI_IDX] > 0.0f) - (andi_u[ONELOOP_ANDI_PHI_IDX] < 0.0f)) * andi_u[ONELOOP_ANDI_THETA_IDX]; + if( (eulers_zxy.theta > 0.0f) && ( fabs(eulers_zxy.phi) < eulers_zxy.theta)) { + coordinated_turn_roll = ((eulers_zxy.phi > 0.0f) - (eulers_zxy.phi < 0.0f)) * eulers_zxy.theta; } BoundAbs(coordinated_turn_roll, max_phi); omega = g / airspeed_turn * tanf(coordinated_turn_roll); @@ -1687,7 +1855,7 @@ static float chirp_pos_j_ref(float delta_t, float f0, float k, float A){ } /** - * @brief Function to perform position and attitude chirps + * @brief Reference Model Definition for 3rd order system specific to positioning with bounds * @param dt [s] time passed since start of the chirp * @param f0 [Hz] initial frequency of the chirp * @param f1 [Hz] final frequency of the chirp @@ -1782,4 +1950,86 @@ void chirp_call(bool *chirp_on, bool *chirp_first_call, float* t_0, float* time_ oneloop_andi_enter(false, oneloop_andi.ctrl_type); } } -} \ No newline at end of file +} + +/** Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts with all thrust off */ +bool autopilot_in_flight_end_detection(bool motors_on UNUSED) { + return ! motors_on; +} + + +void reshape_wind(void) +{ + float psi = eulers_zxy.psi; + float cpsi = cosf(psi); + float spsi = sinf(psi); + float airspeed = airspeed_filt.o[0]; + struct FloatVect2 NT_v_NE = {nav_target[0], nav_target[1]}; // Nav target in North and East frame + //struct FloatVect2 NT_v_B = {0.0, 0.0}; // Nav target in body frame (Overwritten later) + struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed }; + struct FloatVect2 windspeed; + struct FloatVect2 groundspeed = { oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.vel[1] }; + struct FloatVect2 des_as_NE; + struct FloatVect2 des_as_B; + struct FloatVect2 des_acc_B; + VECT2_DIFF(windspeed, groundspeed, airspeed_v); // Wind speed in North and East frame + VECT2_DIFF(des_as_NE, NT_v_NE, windspeed); // Desired airspeed in North and East frame + float norm_des_as = FLOAT_VECT2_NORM(des_as_NE); + //float norm_wind = FLOAT_VECT2_NORM(windspeed); + //float norm_gs = FLOAT_VECT2_NORM(groundspeed); + //float norm_nt = FLOAT_VECT2_NORM(NT_v_NE); + nav_target_new[0] = NT_v_NE.x; + nav_target_new[1] = NT_v_NE.y; + // if the desired airspeed is larger than the max airspeed or we are in force forward reshape gs des to cancel wind and fly at max airspeed + if ((norm_des_as > max_as)||(rotwing_state_settings.force_forward)){ + //printf("reshaping wind\n"); + float groundspeed_factor = 0.0f; + if (FLOAT_VECT2_NORM(windspeed) < max_as) { + //printf("we can achieve wind cancellation\n"); + float av = NT_v_NE.x * NT_v_NE.x + NT_v_NE.y * NT_v_NE.y; // norm squared of nav target + float bv = -2.f * (windspeed.x * NT_v_NE.x + windspeed.y * NT_v_NE.y); + float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - max_as * max_as; + float dv = bv * bv - 4.0f * av * cv; + // dv can only be positive, but just in case + if (dv < 0.0f) { + dv = fabsf(dv); + } + float d_sqrt = sqrtf(dv); + groundspeed_factor = (-bv + d_sqrt) / (2.0f * av); + } + des_as_NE.x = groundspeed_factor * NT_v_NE.x - windspeed.x; + des_as_NE.y = groundspeed_factor * NT_v_NE.y - windspeed.y; + NT_v_NE.x = groundspeed_factor * NT_v_NE.x; + NT_v_NE.y = groundspeed_factor * NT_v_NE.y; + } + norm_des_as = FLOAT_VECT2_NORM(des_as_NE); // Recalculate norm of desired airspeed + //NT_v_B.x = cpsi * NT_v_NE.x + spsi * NT_v_NE.y; // Nav target in body x frame + //NT_v_B.y = -spsi * NT_v_NE.x + cpsi * NT_v_NE.y; // Nav target in body y frame + des_as_B.x = norm_des_as; // Desired airspeed in body x frame + des_as_B.y = 0.0; // Desired airspeed in body y frame + // If flying fast or if in force forward mode, make turns instead of straight lines + if (((airspeed > ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD) && (norm_des_as > (ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD+2.0f)))|| (rotwing_state_settings.force_forward)){ + float delta_psi = atan2f(des_as_NE.y, des_as_NE.x) - psi; + FLOAT_ANGLE_NORMALIZE(delta_psi); + des_acc_B.y = delta_psi * 5.0;//gih_params.heading_bank_gain; + des_acc_B.x = (des_as_B.x - airspeed) * k_pos_rm.k2[0];//gih_params.speed_gain; + //printf("des_acc_B: %f, des_acc_B.x: %f, des_acc_B.y: %f\n", sqrtf(des_acc_B.x*des_acc_B.x+des_acc_B.y*des_acc_B.y),des_acc_B.x, des_acc_B.y); + //printf("max_a_nav: %f\n", max_a_nav); + acc_body_bound(&des_acc_B, max_a_nav); // Scale down side acceleration if norm is too large + //printf("bnd_acc_B: %f, des_acc_B.x: %f, des_acc_B.y: %f\n", sqrtf(des_acc_B.x*des_acc_B.x+des_acc_B.y*des_acc_B.y),des_acc_B.x, des_acc_B.y); + nav_target_new[0] = cpsi * des_acc_B.x - spsi * des_acc_B.y; + nav_target_new[1] = spsi * des_acc_B.x + cpsi * des_acc_B.y; + } else { + nav_target_new[0] = (NT_v_NE.x - groundspeed.x) * k_pos_rm.k2[0]; + nav_target_new[1] = (NT_v_NE.y - groundspeed.y) * k_pos_rm.k2[1]; + } + vect_bound_nd(nav_target_new, max_a_nav, 2); + //printf("norm_as : %f, as_x : %f, as_y : %f\n", airspeed, airspeed_v.x, airspeed_v.y); + //printf("norm_des_as: %f, des_as_x: %f, des_as_y: %f\n", sqrtf(des_as_NE.x*des_as_NE.x+des_as_NE.y*des_as_NE.y), des_as_NE.x, des_as_NE.y); + //printf("norm_wind : %f, wind_x : %f, wind_y : %f\n", FLOAT_VECT2_NORM(windspeed), windspeed.x, windspeed.y); + //printf("norm_gs : %f, gs_x : %f, gs_y : %f\n", FLOAT_VECT2_NORM(groundspeed), groundspeed.x, groundspeed.y); + //printf("norm_nt : %f, nt_x : %f, nt_y : %f\n", FLOAT_VECT2_NORM(NT_v_NE), NT_v_NE.x, NT_v_NE.y); + //printf("norm_nt_new: %f, nt_new_x: %f, nt_new_y: %f\n", sqrtf(nav_target_new[0]*nav_target_new[0] + nav_target_new[1]*nav_target_new[1]), nav_target_new[0], nav_target_new[1]); +} + + diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h index 5dd313bdcd..9a76793245 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h @@ -57,6 +57,7 @@ #define CTRL_ANDI 0 #define CTRL_INDI 1 +extern bool ctrl_off; extern float act_state_filt_vect_1l[ANDI_NUM_ACT]; extern float actuator_state_1l[ANDI_NUM_ACT]; extern float nu[6]; @@ -69,6 +70,8 @@ extern bool yaw_stick_in_auto; extern float fwd_sideslip_gain; extern struct FloatEulers eulers_zxy_des; extern float psi_des_rad; +extern float k_as; +extern float max_as; /*Chirp test Variables*/ extern bool chirp_on; @@ -90,7 +93,7 @@ struct guidance_indi_hybrid_params { float liftd_p50; }; extern struct guidance_indi_hybrid_params gih_params; -extern bool force_forward; +//extern bool force_forward; //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// struct OneloopGuidanceRef { float pos[3]; @@ -144,6 +147,7 @@ struct Gains2ndOrder{ float k3; }; +extern int16_t temp_pitch; /*Declaration of Reference Model and Error Controller Gains*/ extern struct PolePlacement p_att_e; extern struct PolePlacement p_att_rm; @@ -167,7 +171,7 @@ extern void oneloop_andi_init(void); extern void oneloop_andi_enter(bool half_loop_sp, int ctrl_type); extern void oneloop_andi_set_failsafe_setpoint(void); extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v); -extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v); +extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop); extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn); extern void oneloop_from_nav(bool in_flight); #endif // ONELOOP_ANDI_H