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 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
-
+
+
-
-
-
-
-
-
-
+
@@ -272,21 +272,52 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
-
-
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -128,6 +168,56 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -135,57 +225,41 @@
+
+
+
-
+
+
+
+
+
+
+
+
-
+
+
+
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
diff --git a/conf/autopilot/rotorcraft_oneloop_switch.xml b/conf/autopilot/rotorcraft_oneloop_switch.xml
new file mode 100644
index 0000000000..999d162fc2
--- /dev/null
+++ b/conf/autopilot/rotorcraft_oneloop_switch.xml
@@ -0,0 +1,258 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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/right_motor
+
+ fcs/MOTOR_RIGHT
10.1
-
+
-
- fcs/back_motor
+
+ fcs/MOTOR_BACK
10.1
-
+
-
- fcs/left_motor
+
+ fcs/MOTOR_LEFT
10.1
-
+
-
- fcs/pusher
+
+ fcs/MOTOR_PUSHER
24.07
-
+
+
+
+ fcs/ELEVATOR
+ 50.00
+
+
+
+ fcs/RUDDER
+ 50.00
+
+
+
+ fcs/AILERONS
+ 50.00
+
+
+
+ fcs/FLAPS
+ 50.00
+
-
-
- fcs/front_motor
+
+ fcs/MOTOR_FRONT
10.1
-
+
-
- fcs/right_motor
+
+ fcs/MOTOR_RIGHT
10.1
-
+
-
- fcs/back_motor
+
+ fcs/MOTOR_BACK
10.1
-
+
-
- fcs/left_motor
+
+ fcs/MOTOR_LEFT
10.1
-
+
+
+
+
+
+
+
+ fcs/ROT_MECH
+ 0.5
+
+
+
+
+
+
+
+
+
+
+ fcs/cosskew
+ 2.0
+
+
+
+
+
+
+
+
+ fcs/cosskew
+ 3.0
+
+
+
+
+
+
+
+
+
+ fcs/ROT_MECH
+ 0.5
+
+
+
+
+
+
+
+
+
+
+ fcs/sinskew
+ 2.0
+
+
+
+
+
+
+
+
+ fcs/sinskew
+ 3.0
+
+
+
+
+
+
+
+
+ fcs/cosskew
+
+ fcs/cos3skew
+ -1
+
+
+
+
+
+
+
+
+
+
+
+ aero/qbar-psf
+ 47.880259
+ 1.6327
+
+
+
+
+
+
+
+
+ fcs/airspeed2_ms
+
+
+
+
+
+
+
+
+
+
+ fcs/airspeed_ms
+
+ 0.0 0.0
+ 8.8 0.0
+ 16.0 -0.8333
+ 25.0 -0.8333
+
+
+ fcs/ELEVATOR_lag
+
+
+
+
+
+
+
+ 0.129
+
+
+
+
+
+
+ 0.950
+
+
+
+
+
+
+ 0.0478
+
+
+
+
+
+
+ 0.7546
+
+
+
+
+
+
+ 0.08099
+
+
+
+
+
+
+ 0.1949
+
+
+
+
+
+
+
+ fcs/Ixx_body
+
+ fcs/Ixx_wing
+ fcs/cos2skew
+
+
+ fcs/Iyy_wing
+ fcs/sin2skew
+
+
+
+
+
+
+
+
+
+ fcs/Iyy_body
+
+ fcs/Ixx_wing
+ fcs/sin2skew
+
+
+ fcs/Iyy_wing
+ fcs/cos2skew
+
+
+
+
+
+
+
+
+
+ fcs/Ixx_quad
+ fcs/Ixx_sched
+
+
+
+
+
+
+
+
+ fcs/Iyy_quad
+ fcs/Iyy_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 0000000000..2a4a57c1a1
Binary files /dev/null and b/data/pictures/gcs_icons/transition_left.png differ
diff --git a/data/pictures/gcs_icons/transition_right.png b/data/pictures/gcs_icons/transition_right.png
new file mode 100644
index 0000000000..23c1a9eb0a
Binary files /dev/null and b/data/pictures/gcs_icons/transition_right.png differ
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c
index f88af1b727..b1561fb02c 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c
@@ -30,6 +30,7 @@
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_oneloop.h"
#include "firmwares/rotorcraft/oneloop/oneloop_andi.h"
+struct ThrustSetpoint thrust_sp;
void guidance_h_run_enter(void)
{
@@ -59,25 +60,25 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
return guidance_oneloop_run_mode(in_flight, gh, _gv, GUIDANCE_ONELOOP_H_ACCEL, _v_mode);
}
-int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_POS;
- return 0; // nothing to do
+ return thrust_sp; // nothing to do
}
-int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_SPEED;
- return 0; // nothing to do
+ return thrust_sp; // nothing to do
}
-int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
+struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_ACCEL;
- return 0; // nothing to do
+ return thrust_sp;; // nothing to do
}
struct StabilizationSetpoint guidance_oneloop_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceOneloop_HMode h_mode, enum GuidanceOneloop_VMode v_mode)
diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
index 907b64a449..055c145e07 100644
--- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
+++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
@@ -87,11 +87,15 @@
#include "math/wls/wls_alloc.h"
#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
-#include "modules/rot_wing_drone/rotwing_state.h"
+#include "modules/rot_wing_drone/rotwing_state_V2.h"
+#include "modules/core/commands.h"
+#include "modules/ctrl/eff_scheduling_rot_wing_V2.h"
#include
#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