diff --git a/Makefile b/Makefile
index 7d65a55964..360761cbfe 100644
--- a/Makefile
+++ b/Makefile
@@ -327,7 +327,7 @@ test_tudelft: all
# test GVF conf
test_gvf: all
- CONF_XML=conf/userconf/GVF/gvf_conf.xml prove tests/aircrafts/
+ CONF_XML=conf/userconf/conf_example_gvf.xml prove tests/aircrafts/
# compiles all aircrafts in conf_tests.xml
diff --git a/conf/airframes/UCM/rover_steering.xml b/conf/airframes/UCM/rover_steering.xml
index 0d700291be..a9ff16a0e6 100644
--- a/conf/airframes/UCM/rover_steering.xml
+++ b/conf/airframes/UCM/rover_steering.xml
@@ -44,10 +44,8 @@
-
-
-
-
+
+
diff --git a/conf/airframes/UGR/sonic_n1.xml b/conf/airframes/UGR/sonic_n1.xml
new file mode 100644
index 0000000000..39636fc2d0
--- /dev/null
+++ b/conf/airframes/UGR/sonic_n1.xml
@@ -0,0 +1,307 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/UGR/sonic_n2.xml b/conf/airframes/UGR/sonic_n2.xml
new file mode 100644
index 0000000000..dec548afdd
--- /dev/null
+++ b/conf/airframes/UGR/sonic_n2.xml
@@ -0,0 +1,307 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/UGR/steering_rover_n1.xml b/conf/airframes/UGR/steering_rover_n1.xml
new file mode 100644
index 0000000000..9d51439045
--- /dev/null
+++ b/conf/airframes/UGR/steering_rover_n1.xml
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/UGR/steering_rover_n2.xml b/conf/airframes/UGR/steering_rover_n2.xml
new file mode 100644
index 0000000000..6fa5fbeef6
--- /dev/null
+++ b/conf/airframes/UGR/steering_rover_n2.xml
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/UGR/steering_rover_n3.xml b/conf/airframes/UGR/steering_rover_n3.xml
new file mode 100644
index 0000000000..b670a96071
--- /dev/null
+++ b/conf/airframes/UGR/steering_rover_n3.xml
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/bebop2_gvf.xml b/conf/airframes/examples/bebop2_gvf.xml
index c10262e126..9ef32a8f86 100644
--- a/conf/airframes/examples/bebop2_gvf.xml
+++ b/conf/airframes/examples/bebop2_gvf.xml
@@ -13,7 +13,7 @@
-
+
@@ -23,8 +23,7 @@
-
-
+
diff --git a/conf/autopilot/rover_steering.xml b/conf/autopilot/rover_steering.xml
index b27a8e98b4..62f48ba1f0 100644
--- a/conf/autopilot/rover_steering.xml
+++ b/conf/autopilot/rover_steering.xml
@@ -78,7 +78,7 @@
-
+
diff --git a/conf/autopilot/rover_steering_cruise.xml b/conf/autopilot/rover_steering_cruise.xml
new file mode 100644
index 0000000000..c246d422d6
--- /dev/null
+++ b/conf/autopilot/rover_steering_cruise.xml
@@ -0,0 +1,147 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index 7acfb71b91..256af912c2 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -117,8 +117,8 @@
telemetry="telemetry/GVF/gvf_default_rotorcraft.xml"
flight_plan="flight_plans/demo_gvf_rotorcraft.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
- settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gvf_module.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
- gui_color="red"
+ settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gvf_classic.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
+ gui_color="#6200a000ea00"
/>
-
+
@@ -76,43 +76,43 @@
-
+
-
+
-
+
-
-
+
+
-
+
-
+
-
+
-
+
-
+
-
+
diff --git a/conf/flight_plans/UCM/steering_rover_gvfMission.xml b/conf/flight_plans/UCM/steering_rover_gvfMission.xml
index af2f9aa9c4..3b86b7e8b7 100644
--- a/conf/flight_plans/UCM/steering_rover_gvfMission.xml
+++ b/conf/flight_plans/UCM/steering_rover_gvfMission.xml
@@ -79,36 +79,36 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
diff --git a/conf/flight_plans/UGR/fixedwing_clubalhambra_gvf.xml b/conf/flight_plans/UGR/fixedwing_clubalhambra_gvf.xml
new file mode 100644
index 0000000000..33772d1ea2
--- /dev/null
+++ b/conf/flight_plans/UGR/fixedwing_clubalhambra_gvf.xml
@@ -0,0 +1,188 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/UGR/fixedwing_clubalhambra_ik.xml b/conf/flight_plans/UGR/fixedwing_clubalhambra_ik.xml
new file mode 100644
index 0000000000..8059e1fa91
--- /dev/null
+++ b/conf/flight_plans/UGR/fixedwing_clubalhambra_ik.xml
@@ -0,0 +1,129 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/UGR/fixedwing_clubalhambra_param.xml b/conf/flight_plans/UGR/fixedwing_clubalhambra_param.xml
new file mode 100644
index 0000000000..b1cce922b1
--- /dev/null
+++ b/conf/flight_plans/UGR/fixedwing_clubalhambra_param.xml
@@ -0,0 +1,150 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/UGR/steering_rover_clubalhambra.xml b/conf/flight_plans/UGR/steering_rover_clubalhambra.xml
new file mode 100644
index 0000000000..0881ea05e5
--- /dev/null
+++ b/conf/flight_plans/UGR/steering_rover_clubalhambra.xml
@@ -0,0 +1,124 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/demo_gvf.xml b/conf/flight_plans/demo_gvf.xml
index 46ea459886..fa1a54493e 100644
--- a/conf/flight_plans/demo_gvf.xml
+++ b/conf/flight_plans/demo_gvf.xml
@@ -6,16 +6,23 @@
-
-
-
+
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -28,90 +35,222 @@
+
+
+
+
-
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
-
+
+
-
+
+
-
-
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/demo_gvf_rotorcraft.xml b/conf/flight_plans/demo_gvf_rotorcraft.xml
index 67384f08c3..acd8fd8762 100644
--- a/conf/flight_plans/demo_gvf_rotorcraft.xml
+++ b/conf/flight_plans/demo_gvf_rotorcraft.xml
@@ -5,26 +5,32 @@
-
-
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
@@ -41,48 +47,55 @@
+
+
-
+
-
-
+
+
+
+
+
+
+
+
+
+
-
-
+
+
+
-
-
-
-
-
-
-
-
+
-
+
-
+
-
+
+
+
+
diff --git a/conf/modules/distributed_circular_formation.xml b/conf/modules/distributed_circular_formation.xml
index 8f0646b1b0..f5e3e0c46d 100644
--- a/conf/modules/distributed_circular_formation.xml
+++ b/conf/modules/distributed_circular_formation.xml
@@ -28,7 +28,7 @@
- gvf_module
+ gvf_classic
-
diff --git a/conf/modules/guidance_rover_steering.xml b/conf/modules/guidance_rover_steering.xml
index 3faaa80caf..720a17b015 100644
--- a/conf/modules/guidance_rover_steering.xml
+++ b/conf/modules/guidance_rover_steering.xml
@@ -30,7 +30,7 @@
- @navigation
+ @navigation,gvf_common
guidance,commands
diff --git a/conf/modules/gvf_module.xml b/conf/modules/gvf_classic.xml
similarity index 75%
rename from conf/modules/gvf_module.xml
rename to conf/modules/gvf_classic.xml
index e9dbbd059a..34c2d464e2 100644
--- a/conf/modules/gvf_module.xml
+++ b/conf/modules/gvf_classic.xml
@@ -1,9 +1,10 @@
-
+
- Guidance algorithm for tracking smooth trajectories. The algorithm is based on the idea of stearing the vehicle to a vector field that smoothly converges to the desired trajectory.
-For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
+
+ Guidance algorithm for tracking smooth trajectories. The algorithm is based on the idea of stearing the vehicle to a vector field that smoothly converges to the desired trajectory.
+ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
@@ -37,21 +38,21 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
-
+
-
-
+
+
-
+
@@ -68,10 +69,9 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
@@ -79,28 +79,24 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
-
-
-
-
+
+
+
-
-
-
-
+
+
+
-
-
-
-
-
+
+
+
diff --git a/conf/modules/gvf_common.xml b/conf/modules/gvf_common.xml
index f7afed5ea1..6ea0a74895 100644
--- a/conf/modules/gvf_common.xml
+++ b/conf/modules/gvf_common.xml
@@ -3,22 +3,36 @@
- Common file to allow both gvf to work together.
- Still requires at least one module providing the actual gvf implementation
+ A common module that enables all GVF variants to operate within a unified framework.
+ It also provides a shared low-level control system.
+ However, at least one additional module is required to implement the actual GVF functionality.
-
-
+
+
-
-
-
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
+
+
-
diff --git a/conf/modules/gvf_ik.xml b/conf/modules/gvf_ik.xml
new file mode 100644
index 0000000000..985cfe4e2a
--- /dev/null
+++ b/conf/modules/gvf_ik.xml
@@ -0,0 +1,99 @@
+
+
+
+
+
+ Guidance algorithm based on inverse kinematics for path following using guiding vector fields.
+ The path is defined implicitly by level sets, enabling precise transient control and feed-forward motion shaping.
+ For more details, see https://arxiv.org/pdf/2502.17313 .
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ gvf_common
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/gvf_parametric.xml b/conf/modules/gvf_parametric.xml
index 319b7489e2..1194163414 100644
--- a/conf/modules/gvf_parametric.xml
+++ b/conf/modules/gvf_parametric.xml
@@ -2,7 +2,8 @@
- Guidance algorithm for tracking 2D and 3D smooth trajectories that are defined by a parametric equation.
+
+ Guidance algorithm for tracking 2D and 3D smooth trajectories that are defined by a parametric equation.
For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
This algorithm needs the Eigen library, be sure you have it installed, e.g., git submodule update --init --recursive sw/ext/eigen
@@ -14,8 +15,8 @@
-
-
+
+
@@ -43,8 +44,8 @@
-
-
+
+
@@ -68,11 +69,10 @@
@@ -84,11 +84,10 @@
-
-
-
-
-
+
+
+
+
@@ -99,11 +98,10 @@
-
-
-
-
-
+
+
+
+
diff --git a/conf/radios/T7C_SBUS.xml b/conf/radios/T7C_SBUS.xml
new file mode 100644
index 0000000000..632dc06cd8
--- /dev/null
+++ b/conf/radios/T7C_SBUS.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/radios/UGR/FrSkyX20.xml b/conf/radios/UGR/FrSkyX20.xml
new file mode 100644
index 0000000000..bf26acecea
--- /dev/null
+++ b/conf/radios/UGR/FrSkyX20.xml
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/radios/UGR/T16SZ_SBUS_rover.xml b/conf/radios/UGR/T16SZ_SBUS_rover.xml
new file mode 100644
index 0000000000..6d51f5f357
--- /dev/null
+++ b/conf/radios/UGR/T16SZ_SBUS_rover.xml
@@ -0,0 +1,52 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/radios/UGR/T16SZ_SBUS_sonic.xml b/conf/radios/UGR/T16SZ_SBUS_sonic.xml
new file mode 100644
index 0000000000..0b0143f144
--- /dev/null
+++ b/conf/radios/UGR/T16SZ_SBUS_sonic.xml
@@ -0,0 +1,60 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/UGR/fixedwing_basic.xml b/conf/settings/UGR/fixedwing_basic.xml
new file mode 100644
index 0000000000..10c2238660
--- /dev/null
+++ b/conf/settings/UGR/fixedwing_basic.xml
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/UGR/gvf_fixedwing.xml b/conf/telemetry/UGR/gvf_fixedwing.xml
new file mode 100644
index 0000000000..9d9c365cdb
--- /dev/null
+++ b/conf/telemetry/UGR/gvf_fixedwing.xml
@@ -0,0 +1,108 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/userconf/GVF/gvf_conf.xml b/conf/userconf/UCM/gvf_conf.xml
similarity index 76%
rename from conf/userconf/GVF/gvf_conf.xml
rename to conf/userconf/UCM/gvf_conf.xml
index be8cf61d72..b57a04fff8 100644
--- a/conf/userconf/GVF/gvf_conf.xml
+++ b/conf/userconf/UCM/gvf_conf.xml
@@ -7,7 +7,7 @@
telemetry="telemetry/GVF/gvf_default_fixedwing.xml"
flight_plan="flight_plans/UCM/fixedwing_gvfMission.xml"
settings="settings/fixedwing_basic.xml"
- settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_full_pid_fw.xml modules/gvf_module.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
+ settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_full_pid_fw.xml modules/gvf_classic.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
gui_color="blue"
/>
diff --git a/conf/userconf/UGR/conf.xml b/conf/userconf/UGR/conf.xml
new file mode 100644
index 0000000000..77f2b8b092
--- /dev/null
+++ b/conf/userconf/UGR/conf.xml
@@ -0,0 +1,57 @@
+
+
+
+
+
+
+
diff --git a/conf/userconf/UGR/control_panel.xml b/conf/userconf/UGR/control_panel.xml
new file mode 100644
index 0000000000..0ec5a7ee0f
--- /dev/null
+++ b/conf/userconf/UGR/control_panel.xml
@@ -0,0 +1,209 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/userconf/conf_example_gvf.xml b/conf/userconf/conf_example_gvf.xml
new file mode 100644
index 0000000000..87c13b0689
--- /dev/null
+++ b/conf/userconf/conf_example_gvf.xml
@@ -0,0 +1,35 @@
+
+
+
+
+
diff --git a/doc/sphinx/source/user_guide/gvf_viewer.rst b/doc/sphinx/source/user_guide/gvf_viewer.rst
index c677d141a4..01136640b5 100644
--- a/doc/sphinx/source/user_guide/gvf_viewer.rst
+++ b/doc/sphinx/source/user_guide/gvf_viewer.rst
@@ -56,7 +56,7 @@ The GVF Viewer tool is built inside the GCS map widget but will be deactivated u
-Once we loaded GVF Viewer, we need an aircraft working with GVF. In order to test some features of this tool, the user can load with ``start.py`` the GVF configurations located in ``conf/userconf/GVF`` and compile the simulation target.
+Once we loaded GVF Viewer, we need an aircraft working with GVF. In order to test some features of this tool, the user can load with ``start.py`` the GVF configurations located in ``conf/userconf/conf_example_gvf.xml`` and compile the simulation target.
How GVF Viewer works
--------------------
diff --git a/sw/airborne/modules/guidance/gvf/gvf.c b/sw/airborne/modules/guidance/gvf/gvf.c
index 81e25239ea..95d42899de 100644
--- a/sw/airborne/modules/guidance/gvf/gvf.c
+++ b/sw/airborne/modules/guidance/gvf/gvf.c
@@ -24,30 +24,18 @@
#include "std.h"
#include "gvf.h"
-#include "gvf_low_level_control.h"
-#include "trajectories/gvf_ellipse.h"
-#include "trajectories/gvf_line.h"
-#include "trajectories/gvf_sin.h"
#include "autopilot.h"
-#include "../gvf_common.h"
-
// Control
gvf_con gvf_control;
-// State
-gvf_st gvf_state;
-
-// Trajectory
-gvf_tra gvf_trajectory;
-gvf_seg gvf_segment;
+// Telemetry
+gvf_tel gvf_telemetry = {0};
// Time variables to check if GVF is active
-uint32_t gvf_t0 = 0;
+static uint32_t gvf_t0 = 0;
-// Param array lenght
-int gvf_plen = 1;
-int gvf_plen_wps = 0;
+/** TELEMETRY -------------------------------------------------------------- **/
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -59,8 +47,18 @@ static void send_gvf(struct transport_tx *trans, struct link_device *dev)
uint32_t delta_T = now - gvf_t0;
if (delta_T < 200) {
- pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type,
- &gvf_control.s, &gvf_control.ke, gvf_plen, gvf_trajectory.p);
+ pprz_msg_send_GVF(
+ trans, dev, AC_ID,
+ &gvf_control.error,
+ &traj_type,
+ &gvf_control.s,
+ &gvf_control.ke,
+ gvf_trajectory.p_len, gvf_trajectory.p,
+ &gvf_control.error_n,
+ &gvf_telemetry.n_norm,
+ &gvf_telemetry.t_norm,
+ &gvf_telemetry.omega_d,
+ &gvf_telemetry.omega);
#if GVF_OCAML_GCS
if (gvf_trajectory.type == ELLIPSE &&
@@ -82,43 +80,14 @@ static void send_gvf(struct transport_tx *trans, struct link_device *dev)
#endif // PERIODIC_TELEMETRY
-static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
-{
- struct EnuCoor_f *p = stateGetPositionEnu_f();
- float px = p->x - x1;
- float py = p->y - y1;
-
- float zx = x2 - x1;
- float zy = y2 - y1;
- float alpha = atan2f(zy, zx);
-
- float cosa = cosf(-alpha);
- float sina = sinf(-alpha);
-
- float pxr = px * cosa - py * sina;
- float zxr = zx * cosa - zy * sina;
-
- int s = 0;
-
- if (pxr < -d1) {
- s = 1;
- } else if (pxr > (zxr + d2)) {
- s = -1;
- }
-
- if (zy < 0) {
- s *= -1;
- }
-
- return s;
-}
+/** ------------------------------------------------------------------------ **/
void gvf_init(void)
{
gvf_control.ke = 1;
gvf_control.kn = 1;
gvf_control.s = 1;
- gvf_control.speed = 1.0; // Rotorcraft only (for now)
+ gvf_control.speed = 1; // Rotorcraft only (for now)
gvf_control.align = false; // Rotorcraft only
gvf_trajectory.type = NONE;
@@ -132,11 +101,11 @@ void gvf_control_2D(float ke, float kn __attribute__((unused)), float e,
struct gvf_grad *grad, struct gvf_Hess *hess)
{
gvf_t0 = get_sys_time_msec();
-
+
gvf_low_level_getState();
- float course __attribute__((unused)) = gvf_state.course;
- float px_dot = gvf_state.px_dot;
- float py_dot = gvf_state.py_dot;
+ float course __attribute__((unused)) = gvf_c_state.course;
+ float px_dot __attribute__((unused)) = gvf_c_state.px_dot;
+ float py_dot __attribute__((unused)) = gvf_c_state.py_dot;
int s = gvf_control.s;
@@ -149,37 +118,22 @@ void gvf_control_2D(float ke, float kn __attribute__((unused)), float e,
float ty = -s * grad->nx;
// Hessian
- float H11 = hess->H11;
- float H12 = hess->H12;
- float H21 = hess->H21;
- float H22 = hess->H22;
+ float H11 __attribute__((unused)) = hess->H11;
+ float H12 __attribute__((unused)) = hess->H12;
+ float H21 __attribute__((unused)) = hess->H21;
+ float H22 __attribute__((unused)) = hess->H22;
// Calculation of the desired angular velocity in the vector field
float pdx_dot = tx - ke * e * nx;
float pdy_dot = ty - ke * e * ny;
- float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
- float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
-
- float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
- + ((-ke * e * H12) + s * H22) * py_dot;
- float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
- - (s * H12 + (ke * e * H22)) * py_dot;
-
- float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
- float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
-
float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
float md_x = pdx_dot / norm_pd_dot;
float md_y = pdy_dot / norm_pd_dot;
- float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
- / norm_pd_dot;
+ #if defined(ROTORCRAFT_FIRMWARE) // TODO: new accel GVF module just for rotorcrafts
- float md_dot_x __attribute__((unused)) = md_y * md_dot_const;
- float md_dot_y __attribute__((unused))= -md_x * md_dot_const;
-
- #if defined(ROTORCRAFT_FIRMWARE)
+ (void)(course);
// Use accel based control. Not recommended as of current implementation
#if defined(GVF_ROTORCRAFT_USE_ACCEL)
@@ -212,7 +166,7 @@ void gvf_control_2D(float ke, float kn __attribute__((unused)), float e,
nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
nav.heading = atan2f(md_x,md_y);
- #else // SPEED_BASED_GVF
+ #else // SPEED_BASED_GVF // TODO: move to low level control??
nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
@@ -228,26 +182,58 @@ void gvf_control_2D(float ke, float kn __attribute__((unused)), float e,
#endif
- #else // FIXEDWING / ROVER FIRMWARE
+ #else // FIXEDWING & ROVER FIRMWARE
+
+ float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
+ float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
+
+ float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
+ + ((-ke * e * H12) + s * H22) * py_dot;
+ float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
+ - (s * H12 + (ke * e * H22)) * py_dot;
+
+ float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
+ float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
+
+ float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
+ / norm_pd_dot;
+
+ float md_dot_x = md_y * md_dot_const;
+ float md_dot_y = -md_x * md_dot_const;
float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
float mr_x = sinf(course);
float mr_y = cosf(course);
- float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
+ float e_n = (mr_x * md_y - mr_y * md_x);
+ float omega = omega_d + kn * e_n;
+
+ gvf_control.error_n = e_n;
+
+ // ------------------------------------------------
- gvf_control.omega = omega;
+ // Telemetry data
+ gvf_telemetry.n_norm = ke*e*sqrtf(nx*nx + ny*ny);
+ gvf_telemetry.t_norm = sqrtf(tx*tx + ty*ty);
+ gvf_telemetry.omega_d = omega_d;
+ gvf_telemetry.omega = omega;
- // From gvf_common.h TODO: derivative of curvature and ori_err
- gvf_c_omega.omega = omega;
+ // Set GVF common info
gvf_c_info.kappa = (nx*(H12*ny - nx*H22) + ny*(H21*nx - H11*ny))/powf(nx*nx + ny*ny,1.5);
- gvf_c_info.ori_err = 1 - (md_x*cosf(course) + md_y*sinf(course));
+ gvf_c_info.ori_err = e_n;
+
+ // Call the low level control
gvf_low_level_control_2D(omega);
#endif
}
+void gvf_set_direction(int8_t s)
+{
+ gvf_control.s = s;
+}
+
// BEGIN ROTORCRAFT
void gvf_set_speed(float speed)
@@ -263,284 +249,4 @@ void gvf_set_align(bool align)
// END ROTORCRAFT
-void gvf_set_direction(int8_t s)
-{
- gvf_control.s = s;
-}
-
-// STRAIGHT LINE
-
-static void gvf_line(float a, float b, float heading)
-{
- float e;
- struct gvf_grad grad_line;
- struct gvf_Hess Hess_line;
-
- gvf_trajectory.type = 0;
- gvf_trajectory.p[0] = a;
- gvf_trajectory.p[1] = b;
- gvf_trajectory.p[2] = heading;
- gvf_plen = 3 + gvf_plen_wps;
- gvf_plen_wps = 0;
-
- gvf_line_info(&e, &grad_line, &Hess_line);
- gvf_control.ke = gvf_line_par.ke;
- gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
-
- gvf_control.error = e;
-
- gvf_setNavMode(GVF_MODE_WAYPOINT);
-
- gvf_segment.seg = 0;
-}
-
-bool gvf_line_XY_heading(float a, float b, float heading)
-{
- gvf_set_direction(1);
- gvf_line(a, b, heading);
- return true;
-}
-
-bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
-{
- if (gvf_plen_wps != 2) {
- gvf_trajectory.p[3] = x2;
- gvf_trajectory.p[4] = y2;
- gvf_trajectory.p[5] = 0;
- gvf_plen_wps = 3;
- }
-
- float zx = x2 - x1;
- float zy = y2 - y1;
-
- gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
-
- gvf_setNavMode(GVF_MODE_ROUTE);
- gvf_segment.seg = 1;
- gvf_segment.x1 = x1;
- gvf_segment.y1 = y1;
- gvf_segment.x2 = x2;
- gvf_segment.y2 = y2;
-
- return true;
-}
-
-bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
-{
- gvf_trajectory.p[3] = wp1;
- gvf_trajectory.p[4] = wp2;
- gvf_plen_wps = 2;
-
- float x1 = WaypointX(wp1);
- float y1 = WaypointY(wp1);
- float x2 = WaypointX(wp2);
- float y2 = WaypointY(wp2);
-
- return gvf_line_XY1_XY2(x1, y1, x2, y2);
-}
-
-bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
-{
- int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
- if (s != 0) {
- gvf_set_direction(s);
- }
-
- float zx = x2 - x1;
- float zy = y2 - y1;
- float alpha = atanf(zx / zy);
-
- gvf_line(x1, y1, alpha);
-
- gvf_setNavMode(GVF_MODE_ROUTE);
-
- gvf_segment.seg = 1;
- gvf_segment.x1 = x1;
- gvf_segment.y1 = y1;
- gvf_segment.x2 = x2;
- gvf_segment.y2 = y2;
-
- return true;
-}
-
-bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
-{
- gvf_trajectory.p[3] = wp1;
- gvf_trajectory.p[4] = wp2;
- gvf_trajectory.p[5] = d1;
- gvf_trajectory.p[6] = d2;
- gvf_plen_wps = 4;
-
- float x1 = WaypointX(wp1);
- float y1 = WaypointY(wp1);
- float x2 = WaypointX(wp2);
- float y2 = WaypointY(wp2);
-
- return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
-}
-
-bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
-{
- struct EnuCoor_f *p = stateGetPositionEnu_f();
- float px = p->x - x1;
- float py = p->y - y1;
-
- float zx = x2 - x1;
- float zy = y2 - y1;
-
- float beta = atan2f(zy, zx);
- float cosb = cosf(-beta);
- float sinb = sinf(-beta);
- float zxr = zx * cosb - zy * sinb;
- float pxr = px * cosb - py * sinb;
-
- if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
- return false;
- }
-
- return gvf_line_XY1_XY2(x1, y1, x2, y2);
-}
-
-bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
-{
- gvf_trajectory.p[3] = wp1;
- gvf_trajectory.p[4] = wp2;
- gvf_plen_wps = 2;
-
- float x1 = WaypointX(wp1);
- float y1 = WaypointY(wp1);
- float x2 = WaypointX(wp2);
- float y2 = WaypointY(wp2);
-
- return gvf_segment_XY1_XY2(x1, y1, x2, y2);
-}
-
-bool gvf_line_wp_heading(uint8_t wp, float heading)
-{
- gvf_trajectory.p[3] = wp;
- gvf_plen_wps = 1;
-
- heading = RadOfDeg(heading);
-
- float a = WaypointX(wp);
- float b = WaypointY(wp);
-
- return gvf_line_XY_heading(a, b, heading);
-}
-
-// ELLIPSE
-
-bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
-{
- float e;
- struct gvf_grad grad_ellipse;
- struct gvf_Hess Hess_ellipse;
-
- gvf_trajectory.type = 1;
- gvf_trajectory.p[0] = x;
- gvf_trajectory.p[1] = y;
- gvf_trajectory.p[2] = a;
- gvf_trajectory.p[3] = b;
- gvf_trajectory.p[4] = alpha;
- gvf_plen = 5 + gvf_plen_wps;
- gvf_plen_wps = 0;
-
- // SAFE MODE
- if (a < 1 || b < 1) {
- gvf_trajectory.p[2] = 60;
- gvf_trajectory.p[3] = 60;
- }
-
- if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
- gvf_setNavMode(GVF_MODE_CIRCLE);
-
- } else {
- gvf_setNavMode(GVF_MODE_WAYPOINT);
- }
-
- gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
- gvf_control.ke = gvf_ellipse_par.ke;
- gvf_control_2D(gvf_ellipse_par.ke, gvf_ellipse_par.kn,
- e, &grad_ellipse, &Hess_ellipse);
-
- gvf_control.error = e;
-
- return true;
-}
-
-
-bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
-{
- gvf_trajectory.p[5] = wp;
- gvf_plen_wps = 1;
-
- gvf_ellipse_XY(WaypointX(wp), WaypointY(wp), a, b, alpha);
- return true;
-}
-
-// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
-
-bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
-{
- float e;
- struct gvf_grad grad_line;
- struct gvf_Hess Hess_line;
-
- gvf_trajectory.type = 2;
- gvf_trajectory.p[0] = a;
- gvf_trajectory.p[1] = b;
- gvf_trajectory.p[2] = alpha;
- gvf_trajectory.p[3] = w;
- gvf_trajectory.p[4] = off;
- gvf_trajectory.p[5] = A;
- gvf_plen = 6 + gvf_plen_wps;
- gvf_plen_wps = 0;
-
- gvf_sin_info(&e, &grad_line, &Hess_line);
- gvf_control.ke = gvf_sin_par.ke;
- gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
-
- gvf_control.error = e;
-
- return true;
-}
-
-bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
-{
- w = 2 * M_PI * w;
-
- gvf_trajectory.p[6] = wp1;
- gvf_trajectory.p[7] = wp2;
- gvf_plen_wps = 2;
-
- float x1 = WaypointX(wp1);
- float y1 = WaypointY(wp1);
- float x2 = WaypointX(wp2);
- float y2 = WaypointY(wp2);
-
- float zx = x1 - x2;
- float zy = y1 - y2;
-
- float alpha = atanf(zy / zx);
-
- gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
-
- return true;
-}
-
-bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
-{
- w = 2 * M_PI * w;
- alpha = RadOfDeg(alpha);
-
- gvf_trajectory.p[6] = wp;
- gvf_plen_wps = 1;
-
- float x = WaypointX(wp);
- float y = WaypointY(wp);
-
- gvf_sin_XY_alpha(x, y, alpha, w, off, A);
-
- return true;
-}
-
+/** ------------------------------------------------------------------------ **/
diff --git a/sw/airborne/modules/guidance/gvf/gvf.h b/sw/airborne/modules/guidance/gvf/gvf.h
index eaddc029f8..0661fe08af 100644
--- a/sw/airborne/modules/guidance/gvf/gvf.h
+++ b/sw/airborne/modules/guidance/gvf/gvf.h
@@ -28,119 +28,55 @@
#ifndef GVF_H
#define GVF_H
-#define GVF_GRAVITY 9.806
-
/*! Default GCS trajectory painter */
#ifndef GVF_OCAML_GCS
#define GVF_OCAML_GCS true
#endif
#include "std.h"
+#include "modules/guidance/gvf_common.h"
+#include "modules/guidance/trajectories/gvf_traj.h"
+
+/** STRUCTS ---------------------------------------------------------------- **/
/** @typedef gvf_con
* @brief Control parameters for the GVF
* @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field
* @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory.
-* @param omega Angular velocity of the vehicle
+* @param error_n Error of alignment with the GVF
* @param speed Speed of the vehicle. It is only used in rotorcrafts for now.
* @param s Defines the direction to be tracked. Its meaning depends on the trajectory and its implementation. Check the wiki entry of the GVF. It takes the values -1 or 1.
* @param align Align the vehicle with the direction of the vector field. Can only used in rotorcrafts.
*/
+
typedef struct {
float ke;
float kn;
float error;
- float omega;
+ float error_n;
float speed;
int8_t s;
bool align;
} gvf_con;
+typedef struct {
+ float n_norm;
+ float t_norm;
+ float omega_d;
+ float omega;
+} gvf_tel;
+
+// Extern structs
extern gvf_con gvf_control;
-typedef struct {
- float course;
- float px_dot;
- float py_dot;
-} gvf_st;
-
-extern gvf_st gvf_state;
-
-enum trajectories {
- LINE = 0,
- ELLIPSE,
- SIN,
- NONE = 255,
-};
-
-typedef struct {
- enum trajectories type;
- float p[16];
-} gvf_tra;
-
-/** @typedef gvf_seg
-* @brief Struct employed by the LINE trajectory for the special case of trackinga segment, which is described by the coordinates x1, y1, x2, y2
-* @param seg Tracking a segment or not
-* @param x1 coordinate w.r.t. HOME
-* @param y1 coordinate w.r.t. HOME
-* @param x2 coordinate w.r.t. HOME
-* @param y2 coordinate w.r.t. HOME
-*/
-typedef struct {
- int seg;
- float x1;
- float y1;
- float x2;
- float y2;
-} gvf_seg;
-
-extern gvf_tra gvf_trajectory;
-
-struct gvf_grad {
- float nx;
- float ny;
- float nz;
-};
-
-struct gvf_Hess {
- float H11;
- float H12;
- float H13;
- float H21;
- float H22;
- float H23;
- float H31;
- float H32;
- float H33;
-};
+/** EXTERN FUNCTIONS ------------------------------------------------------- **/
extern void gvf_init(void);
-void gvf_control_2D(float ke, float kn, float e,
+extern void gvf_control_2D(float ke, float kn, float e,
struct gvf_grad *, struct gvf_Hess *);
extern void gvf_set_speed(float speed); // Rotorcraft only (for now)
extern void gvf_set_align(bool align); // Rotorcraft only
extern void gvf_set_direction(int8_t s);
-// Straight line
-extern bool gvf_line_XY_heading(float x, float y, float heading);
-extern bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2);
-extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
-extern bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2);
-extern bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2);
-extern bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2);
-extern bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
-extern bool gvf_line_wp_heading(uint8_t wp, float heading);
-
-// Ellipse
-extern bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha);
-extern bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha);
-
-// Sinusoidal
-extern bool gvf_sin_XY_alpha(float x, float y, float alpha, float w, float off, float A);
-extern bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off,
- float A);
-extern bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off,
- float A);
-
#endif // GVF_H
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.c b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.c
deleted file mode 100644
index 5698906fd7..0000000000
--- a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.c
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * Copyright (C) 2020 Hector Garcia de Marina
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file modules/guidance/gvf/gvf_low_level_control.c
- *
- * Firmware dependent file for the guiding vector field algorithm for 2D trajectories.
- */
-
-#include "autopilot.h"
-#include "modules/guidance/gvf/gvf_low_level_control.h"
-#include "modules/guidance/gvf/gvf.h"
-
-#if defined(FIXEDWING_FIRMWARE)
-#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-#endif
-
-void gvf_low_level_getState(void)
-{
- #if defined(FIXEDWING_FIRMWARE)
- float ground_speed = stateGetHorizontalSpeedNorm_f();
- gvf_state.course = stateGetHorizontalSpeedDir_f();
- gvf_state.px_dot = ground_speed * sinf(gvf_state.course);
- gvf_state.py_dot = ground_speed * cosf(gvf_state.course);
-
- #elif defined(ROVER_FIRMWARE) || defined(ROTORCRAFT_FIRMWARE)
- // We assume that the course and psi
- // of the rover (steering wheel) are the same
- gvf_state.course = stateGetNedToBodyEulers_f()->psi;
- gvf_state.px_dot = stateGetSpeedEnu_f()->x;
- gvf_state.py_dot = stateGetSpeedEnu_f()->y;
- #endif
-}
-
-void gvf_low_level_control_2D(float omega)
-{
-
-#if defined(FIXEDWING_FIRMWARE)
- if (autopilot_get_mode() == AP_MODE_AUTO2) {
-
- // Coordinated turn
- struct FloatEulers *att = stateGetNedToBodyEulers_f();
- float ground_speed = stateGetHorizontalSpeedNorm_f();
-
- lateral_mode = LATERAL_MODE_ROLL;
-
- h_ctl_roll_setpoint =
- -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
- BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
- }
-#endif
-
- (void)(omega); // Avoid unused parameter warning
-}
-
diff --git a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h
deleted file mode 100644
index a3ee7135c9..0000000000
--- a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * Copyright (C) 2020 Hector Garcia de Marina
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file modules/guidance/gvf/gvf_low_level_control.h
- *
- * Firmware dependent file for the guiding vector field algorithm for 2D trajectories.
- */
-
-#ifndef GVF_LOW_LEVEL_CONTROL_H
-#define GVF_LOW_LEVEL_CONTROL_H
-
-#ifdef FIXEDWING_FIRMWARE
-#include "firmwares/fixedwing/nav.h"
-#include "modules/nav/common_nav.h"
-#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-#define gvf_setNavMode(_navMode) (horizontal_mode = _navMode)
-#define GVF_MODE_ROUTE HORIZONTAL_MODE_ROUTE
-#define GVF_MODE_WAYPOINT HORIZONTAL_MODE_WAYPOINT
-#define GVF_MODE_CIRCLE HORIZONTAL_MODE_CIRCLE
-
-#elif defined(ROVER_FIRMWARE)
-#include "state.h"
-#include "firmwares/rover/navigation.h"
-#define gvf_setNavMode(_navMode) (nav.mode = _navMode)
-#define GVF_MODE_ROUTE NAV_MODE_ROUTE
-#define GVF_MODE_WAYPOINT NAV_MODE_WAYPOINT
-#define GVF_MODE_CIRCLE NAV_MODE_CIRCLE
-
-#elif defined(ROTORCRAFT_FIRMWARE)
-#include "firmwares/rotorcraft/navigation.h"
-#define gvf_setNavMode(_navMode) (nav.horizontal_mode = _navMode)
-#define GVF_MODE_ROUTE NAV_HORIZONTAL_MODE_ROUTE
-#define GVF_MODE_WAYPOINT NAV_HORIZONTAL_MODE_WAYPOINT
-#define GVF_MODE_CIRCLE NAV_HORIZONTAL_MODE_CIRCLE
-
-//NAV_SETPOINT_MODE_POS
-//NAV_SETPOINT_MODE_SPEED
-//NAV_SETPOINT_MODE_ACCEL
-
-#else
-#error "GVF does not support your firmware yet!"
-#endif
-
-// Low level control functions
-extern void gvf_low_level_getState(void);
-extern void gvf_low_level_control_2D(float);
-
-#endif // GVF_LOW_LEVEL_CONTROL_H
-
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.c b/sw/airborne/modules/guidance/gvf/nav/nav_ellipse.c
similarity index 55%
rename from sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.c
rename to sw/airborne/modules/guidance/gvf/nav/nav_ellipse.c
index f837e6058b..72457d5c9d 100644
--- a/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.c
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_ellipse.c
@@ -20,16 +20,10 @@
*
*/
-/** \file gvf_ellipse.c
- *
- * Guidance algorithm based on vector fields
- * 2D Ellipse trajectory
- */
+#include "nav_ellipse.h"
-
-#include "modules/nav/common_nav.h"
-#include "gvf_ellipse.h"
#include "generated/airframe.h"
+#include "modules/guidance/gvf/gvf.h"
/*! Default gain ke for the ellipse trajectory */
#ifndef GVF_ELLIPSE_KE
@@ -60,36 +54,57 @@ gvf_ell_par gvf_ellipse_par = {GVF_ELLIPSE_KE, GVF_ELLIPSE_KN,
GVF_ELLIPSE_A, GVF_ELLIPSE_B, GVF_ELLIPSE_ALPHA
};
-void gvf_ellipse_info(float *phi, struct gvf_grad *grad,
- struct gvf_Hess *hess)
+// Param array lenght
+static int gvf_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+// ELLIPSE
+
+bool nav_gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
{
+ float e;
+ struct gvf_grad grad_ellipse;
+ struct gvf_Hess Hess_ellipse;
- struct EnuCoor_f *p = stateGetPositionEnu_f();
- float px = p->x;
- float py = p->y;
- float wx = gvf_trajectory.p[0];
- float wy = gvf_trajectory.p[1];
- float a = gvf_trajectory.p[2];
- float b = gvf_trajectory.p[3];
- float alpha = gvf_trajectory.p[4];
+ gvf_trajectory.type = 1;
+ gvf_trajectory.p[0] = x;
+ gvf_trajectory.p[1] = y;
+ gvf_trajectory.p[2] = a;
+ gvf_trajectory.p[3] = b;
+ gvf_trajectory.p[4] = alpha;
+ gvf_trajectory.p_len= 5 + gvf_p_len_wps;
+ gvf_p_len_wps = 0;
- float cosa = cosf(alpha);
- float sina = sinf(alpha);
+ // SAFE MODE
+ if (a < 1 || b < 1) {
+ gvf_trajectory.p[2] = 60;
+ gvf_trajectory.p[3] = 60;
+ }
- // Phi(x,y)
- float xel = (px - wx) * cosa - (py - wy) * sina;
- float yel = (px - wx) * sina + (py - wy) * cosa;
- *phi = (xel / a) * (xel / a) + (yel / b) * (yel / b) - 1;
+ if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
+ gvf_setNavMode(GVF_MODE_CIRCLE);
- // grad Phi
- grad->nx = (2 * xel / (a * a)) * cosa + (2 * yel / (b * b)) * sina;
- grad->ny = (2 * yel / (b * b)) * cosa - (2 * xel / (a * a)) * sina;
+ } else {
+ gvf_setNavMode(GVF_MODE_WAYPOINT);
+ }
- // Hessian Phi
- hess->H11 = 2 * (cosa * cosa / (a * a)
- + sina * sina / (b * b));
- hess->H12 = 2 * sina * cosa * (1 / (b * b) - 1 / (a * a));
- hess->H21 = hess->H12;
- hess->H22 = 2 * (sina * sina / (a * a)
- + cosa * cosa / (b * b));
+ gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
+ gvf_control.ke = gvf_ellipse_par.ke;
+ gvf_control_2D(gvf_ellipse_par.ke, gvf_ellipse_par.kn,
+ e, &grad_ellipse, &Hess_ellipse);
+
+ gvf_control.error = e;
+
+ return true;
+}
+
+
+bool nav_gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
+{
+ gvf_trajectory.p[5] = wp;
+ gvf_p_len_wps = 1;
+
+ nav_gvf_ellipse_XY(WaypointX(wp), WaypointY(wp), a, b, alpha);
+ return true;
}
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.h b/sw/airborne/modules/guidance/gvf/nav/nav_ellipse.h
similarity index 81%
rename from sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.h
rename to sw/airborne/modules/guidance/gvf/nav/nav_ellipse.h
index 2fa7e9cfbb..7b93c087f4 100644
--- a/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.h
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_ellipse.h
@@ -20,16 +20,10 @@
*
*/
-/** @file gvf_ellipse.h
- *
- * Guidance algorithm based on vector fields
- * 2D Ellipse trajectory
- */
-
#ifndef GVF_ELLIPSE_H
#define GVF_ELLIPSE_H
-#include "modules/guidance/gvf/gvf.h"
+#include "modules/guidance/trajectories/gvf_traj.h"
/** @typedef gvf_ell_par
* @brief Parameters for the GVF line trajectory
@@ -47,8 +41,12 @@ typedef struct {
float alpha;
} gvf_ell_par;
+/** ------------------------------------------------------------------------ **/
+
extern gvf_ell_par gvf_ellipse_par;
-extern void gvf_ellipse_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+// Ellipse
+extern bool nav_gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha);
+extern bool nav_gvf_ellipse_XY(float x, float y, float a, float b, float alpha);
#endif // GVF_ELLIPSE_H
diff --git a/sw/airborne/modules/guidance/gvf/nav/nav_line.c b/sw/airborne/modules/guidance/gvf/nav/nav_line.c
new file mode 100644
index 0000000000..5677d6393c
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_line.c
@@ -0,0 +1,256 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "nav_line.h"
+
+#include "generated/airframe.h"
+#include "modules/guidance/gvf/gvf.h"
+
+/*! Gain ke for the line trajectory*/
+#ifndef GVF_LINE_KE
+#define GVF_LINE_KE 1
+#endif
+
+/*! Gain kn for the line trajectory*/
+#ifndef GVF_LINE_KN
+#define GVF_LINE_KN 1
+#endif
+
+/*! Default heading in degrees for a trajectory called from gvf_line_**_HEADING */
+#ifndef GVF_LINE_HEADING
+#define GVF_LINE_HEADING 0
+#endif
+
+/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x1,y1 before turning back */
+#ifndef GVF_SEGMENT_D1
+#define GVF_SEGMENT_D1 0
+#endif
+
+/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x2,y2 before turning back */
+#ifndef GVF_SEGMENT_D2
+#define GVF_SEGMENT_D2 0
+#endif
+
+gvf_li_par gvf_line_par = {GVF_LINE_KE, GVF_LINE_KN, GVF_LINE_HEADING};
+gvf_seg_par gvf_segment_par = {GVF_SEGMENT_D1, GVF_SEGMENT_D2};
+
+// Param array lenght
+static int gvf_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+static void gvf_line(float a, float b, float heading)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 0;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = heading;
+ gvf_trajectory.p_len= 3 + gvf_p_len_wps;
+ gvf_p_len_wps = 0;
+
+ gvf_line_info(&e, &grad_line, &Hess_line);
+ gvf_control.ke = gvf_line_par.ke;
+ gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
+
+ gvf_control.error = e;
+
+ gvf_setNavMode(GVF_MODE_WAYPOINT);
+
+ gvf_segment.seg = 0;
+}
+
+static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
+{
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x - x1;
+ float py = p->y - y1;
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+ float alpha = atan2f(zy, zx);
+
+ float cosa = cosf(-alpha);
+ float sina = sinf(-alpha);
+
+ float pxr = px * cosa - py * sina;
+ float zxr = zx * cosa - zy * sina;
+
+ int s = 0;
+
+ if (pxr < -d1) {
+ s = 1;
+ } else if (pxr > (zxr + d2)) {
+ s = -1;
+ }
+
+ if (zy < 0) {
+ s *= -1;
+ }
+
+ return s;
+}
+
+/** ------------------------------------------------------------------------ **/
+
+// STRAIGHT LINE
+
+bool nav_gvf_line_XY_heading(float a, float b, float heading)
+{
+ gvf_set_direction(1);
+ gvf_line(a, b, heading);
+ return true;
+}
+
+bool nav_gvf_line_wp_heading(uint8_t wp, float heading)
+{
+ heading = RadOfDeg(heading);
+
+ float a = WaypointX(wp);
+ float b = WaypointY(wp);
+
+ return nav_gvf_line_XY_heading(a, b, heading);
+}
+
+bool nav_gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
+{
+ if (gvf_p_len_wps != 2) {
+ gvf_trajectory.p[3] = x2;
+ gvf_trajectory.p[4] = y2;
+ gvf_trajectory.p[5] = 0;
+ gvf_p_len_wps = 3;
+ }
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+
+ nav_gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
+
+ gvf_setNavMode(GVF_MODE_ROUTE);
+ gvf_segment.seg = 1;
+ gvf_segment.x1 = x1;
+ gvf_segment.y1 = y1;
+ gvf_segment.x2 = x2;
+ gvf_segment.y2 = y2;
+
+ return true;
+}
+
+bool nav_gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
+{
+ gvf_trajectory.p[3] = wp1;
+ gvf_trajectory.p[4] = wp2;
+ gvf_p_len_wps = 2;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ return nav_gvf_line_XY1_XY2(x1, y1, x2, y2);
+}
+
+// SEGMENT
+
+bool nav_gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
+{
+ int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
+ if (s != 0) {
+ gvf_set_direction(s);
+ }
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+ float alpha = atanf(zx / zy);
+
+ gvf_line(x1, y1, alpha);
+
+ gvf_setNavMode(GVF_MODE_ROUTE);
+
+ gvf_segment.seg = 1;
+ gvf_segment.x1 = x1;
+ gvf_segment.y1 = y1;
+ gvf_segment.x2 = x2;
+ gvf_segment.y2 = y2;
+
+ return true;
+}
+
+bool nav_gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
+{
+ gvf_trajectory.p[3] = wp1;
+ gvf_trajectory.p[4] = wp2;
+ gvf_trajectory.p[5] = d1;
+ gvf_trajectory.p[6] = d2;
+ gvf_p_len_wps = 4;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ return nav_gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
+}
+
+bool nav_gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
+{
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x - x1;
+ float py = p->y - y1;
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+
+ float beta = atan2f(zy, zx);
+ float cosb = cosf(-beta);
+ float sinb = sinf(-beta);
+ float zxr = zx * cosb - zy * sinb;
+ float pxr = px * cosb - py * sinb;
+
+ if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
+ return false;
+ }
+
+ return nav_gvf_line_XY1_XY2(x1, y1, x2, y2);
+}
+
+bool nav_gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
+{
+ gvf_trajectory.p[3] = wp1;
+ gvf_trajectory.p[4] = wp2;
+ gvf_p_len_wps = 2;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ return nav_gvf_segment_XY1_XY2(x1, y1, x2, y2);
+}
+
+bool nav_gvf_segment_points(struct FloatVect2 start, struct FloatVect2 end)
+{
+ return nav_gvf_segment_XY1_XY2(start.x, start.y, end.x, end.y);
+}
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h b/sw/airborne/modules/guidance/gvf/nav/nav_line.h
similarity index 66%
rename from sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h
rename to sw/airborne/modules/guidance/gvf/nav/nav_line.h
index cc230a5b59..0afdfb62a0 100644
--- a/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_line.h
@@ -29,7 +29,8 @@
#ifndef GVF_LINE_H
#define GVF_LINE_H
-#include "modules/guidance/gvf/gvf.h"
+#include "modules/guidance/trajectories/gvf_traj.h"
+#include "math/pprz_algebra_float.h"
/** @typedef gvf_li_par
* @brief Parameters for the GVF line trajectory
@@ -53,9 +54,20 @@ typedef struct {
float d2;
} gvf_seg_par;
+/** ------------------------------------------------------------------------ **/
+
extern gvf_li_par gvf_line_par;
extern gvf_seg_par gvf_segment_par;
-extern void gvf_line_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+// Straight line
+extern bool nav_gvf_line_XY_heading(float x, float y, float heading);
+extern bool nav_gvf_line_wp_heading(uint8_t wp, float heading);
+extern bool nav_gvf_line_XY1_XY2(float x1, float y1, float x2, float y2);
+extern bool nav_gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
+extern bool nav_gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2);
+extern bool nav_gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2);
+extern bool nav_gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2);
+extern bool nav_gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
+extern bool nav_gvf_segment_points(struct FloatVect2 start, struct FloatVect2 end);
#endif // GVF_LINE_H
diff --git a/sw/airborne/modules/guidance/gvf/nav/nav_sin.c b/sw/airborne/modules/guidance/gvf/nav/nav_sin.c
new file mode 100644
index 0000000000..75372e33ed
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_sin.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "nav_sin.h"
+
+#include "generated/airframe.h"
+#include "modules/guidance/gvf/gvf.h"
+
+/*! Default gain ke for the sin trajectory*/
+#ifndef GVF_SIN_KE
+#define GVF_SIN_KE 1
+#endif
+
+/*! Default gain kn for the sin trajectory*/
+#ifndef GVF_SIN_KN
+#define GVF_SIN_KN 1
+#endif
+
+/*! Default orientation in rads for the sin trajectory function gvf_sin_**_alpha*/
+#ifndef GVF_SIN_ALPHA
+#define GVF_SIN_ALPHA 0
+#endif
+
+/*! Default frequency for the sin trajectory in rads*/
+#ifndef GVF_SIN_W
+#define GVF_SIN_W 0
+#endif
+
+/*! Default off-set in rads for the sin trajectory in rads*/
+#ifndef GVF_SIN_OFF
+#define GVF_SIN_OFF 0
+#endif
+
+/*! Default amplitude for the sin trajectory in meters*/
+#ifndef GVF_SIN_A
+#define GVF_SIN_A 0
+#endif
+
+gvf_s_par gvf_sin_par = {GVF_SIN_KE, GVF_SIN_KN,
+ GVF_SIN_ALPHA, GVF_SIN_W, GVF_SIN_OFF, GVF_SIN_A
+ };
+
+// Param array lenght
+static int gvf_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
+
+bool nav_gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 2;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = alpha;
+ gvf_trajectory.p[3] = w;
+ gvf_trajectory.p[4] = off;
+ gvf_trajectory.p[5] = A;
+ gvf_trajectory.p_len= 6 + gvf_p_len_wps;
+ gvf_p_len_wps = 0;
+
+ gvf_sin_info(&e, &grad_line, &Hess_line);
+ gvf_control.ke = gvf_sin_par.ke;
+ gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
+
+ gvf_control.error = e;
+
+ return true;
+}
+
+bool nav_gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+
+ gvf_trajectory.p[6] = wp1;
+ gvf_trajectory.p[7] = wp2;
+ gvf_p_len_wps = 2;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ float zx = x1 - x2;
+ float zy = y1 - y2;
+
+ float alpha = atanf(zy / zx);
+
+ nav_gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
+
+ return true;
+}
+
+bool nav_gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+ alpha = RadOfDeg(alpha);
+
+ gvf_trajectory.p[6] = wp;
+ gvf_p_len_wps = 1;
+
+ float x = WaypointX(wp);
+ float y = WaypointY(wp);
+
+ nav_gvf_sin_XY_alpha(x, y, alpha, w, off, A);
+
+ return true;
+}
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.h b/sw/airborne/modules/guidance/gvf/nav/nav_sin.h
similarity index 78%
rename from sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.h
rename to sw/airborne/modules/guidance/gvf/nav/nav_sin.h
index 0819f39666..2d3ae5eceb 100644
--- a/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.h
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_sin.h
@@ -29,7 +29,7 @@
#ifndef GVF_SIN_H
#define GVF_SIN_H
-#include "modules/guidance/gvf/gvf.h"
+#include "modules/guidance/trajectories/gvf_traj.h"
/** @typedef gvf_s_par
* @brief Parameters for the GVF line trajectory
@@ -49,8 +49,13 @@ typedef struct {
float A;
} gvf_s_par;
+/** ------------------------------------------------------------------------ **/
+
extern gvf_s_par gvf_sin_par;
-extern void gvf_sin_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+// Sinusoidal
+extern bool nav_gvf_sin_XY_alpha(float x, float y, float alpha, float w, float off, float A);
+extern bool nav_gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A);
+extern bool nav_gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A);
#endif // GVF_SIN_H
diff --git a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c
index bf571d71f3..f9a6ecc47a 100644
--- a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c
@@ -27,6 +27,8 @@
*
*/
+#include "nav_survey_polygon_gvf.h"
+
#include "firmwares/fixedwing/nav.h"
#include "state.h"
#include "autopilot.h"
@@ -39,19 +41,25 @@
struct gvf_SurveyPolyAdv gvf_survey;
-static void gvf_nav_points(struct FloatVect2 start, struct FloatVect2 end)
+/** ------------------------------------------------------------------------ **/
+
+static void gvf_circle_direction(float rad)
{
- gvf_segment_XY1_XY2(start.x, start.y, end.x, end.y);
+ if (rad > 0) {
+ gvf_set_direction(-1);
+ } else {
+ gvf_set_direction(1);
+ }
}
/**
- * intercept two lines and give back the point of intersection
- * @return FALSE if no intersection can be found or intersection does not lie between points a and b
- * else TRUE
+ * Intercept two lines and give back the point of intersection
+ * @return FALSE if no intersection can be found or intersection does not lie
+ * between points a and b else TRUE
* @param p returns intersection
* @param x, y first line is defined by point x and y (goes through this points)
* @param a1, a2, b1, b2 second line by coordinates a1/a2, b1/b2
- */
+ **/
static bool gvf_intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2,
float b1, float b2)
{
@@ -70,11 +78,11 @@ static bool gvf_intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, s
}
/**
- * intersects a line with the polygon and gives back the two intersection points
- * @return TRUE if two intersection can be found, else FALSE
+ * Intersects a line with the polygon and gives back the two intersection points
+ * @return TRUE if two intersection can be found, else FALSE
* @param x, y intersection points
* @param a, b define the line to intersection
- */
+ **/
static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, struct FloatVect2 a, struct FloatVect2 b)
{
int i, count = 0;
@@ -122,8 +130,10 @@ static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, s
return true;
}
+/** ------------------------------------------------------------------------ **/
+
/**
- * initializes the variables needed for the survey to start
+ * Initializes the variables needed for the survey to start
* @param first_wp the first Waypoint of the polygon
* @param size the number of points that make up the polygon
* @param angle angle in which to do the flyovers
@@ -132,7 +142,7 @@ static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, s
* @param min_rad minimal radius when navigating
* @param altitude the altitude that must be reached before the flyover starts
**/
-void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
+void nav_gvf_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude)
{
int i;
@@ -229,28 +239,19 @@ void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, f
}
/**
- * main navigation routine. This is called periodically evaluates the current
+ * Main navigation routine. This is called periodically evaluates the current
* Position and stage and navigates accordingly.
* @returns True until the survey is finished
- */
-void gvf_nav_direction_circle(float rad)
-{
- if (rad > 0) {
- gvf_set_direction(-1);
- } else {
- gvf_set_direction(1);
- }
-}
-
-bool gvf_nav_survey_polygon_run(void)
+ **/
+bool nav_gvf_survey_polygon_run(void)
{
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(gvf_survey.psa_altitude, 0.0);
//entry circle around entry-center until the desired altitude is reached
if (gvf_survey.stage == gENTRY) {
- gvf_nav_direction_circle(gvf_survey.psa_min_rad);
- gvf_ellipse_XY(gvf_survey.entry_center.x, gvf_survey.entry_center.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
+ gvf_circle_direction(gvf_survey.psa_min_rad);
+ nav_gvf_ellipse_XY(gvf_survey.entry_center.x, gvf_survey.entry_center.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
if (NavCourseCloseTo(gvf_survey.segment_angle)
&& nav_approaching_xy(gvf_survey.seg_start.x, gvf_survey.seg_start.y, last_x, last_y, CARROT)
&& fabs(stateGetPositionUtm_f()->alt - gvf_survey.psa_altitude) <= 20) {
@@ -264,7 +265,7 @@ bool gvf_nav_survey_polygon_run(void)
}
//fly the segment until seg_end is reached
if (gvf_survey.stage == gSEG) {
- gvf_nav_points(gvf_survey.seg_start, gvf_survey.seg_end);
+ nav_gvf_segment_points(gvf_survey.seg_start, gvf_survey.seg_end);
//calculate all needed points for the next flyover
if (nav_approaching_xy(gvf_survey.seg_end.x, gvf_survey.seg_end.y, gvf_survey.seg_start.x, gvf_survey.seg_start.y, 0)) {
#ifdef DIGITAL_CAM
@@ -295,15 +296,15 @@ bool gvf_nav_survey_polygon_run(void)
}
//turn from stage to return
else if (gvf_survey.stage == gTURN1) {
- gvf_nav_direction_circle(gvf_survey.psa_min_rad);
- gvf_ellipse_XY(gvf_survey.seg_center1.x, gvf_survey.seg_center1.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
+ gvf_circle_direction(gvf_survey.psa_min_rad);
+ nav_gvf_ellipse_XY(gvf_survey.seg_center1.x, gvf_survey.seg_center1.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
if (NavCourseCloseTo(gvf_survey.return_angle)) {
gvf_survey.stage = gRET;
nav_init_stage();
}
//return
} else if (gvf_survey.stage == gRET) {
- gvf_nav_points(gvf_survey.ret_start, gvf_survey.ret_end);
+ nav_gvf_segment_points(gvf_survey.ret_start, gvf_survey.ret_end);
if (nav_approaching_xy(gvf_survey.ret_end.x, gvf_survey.ret_end.y, gvf_survey.ret_start.x, gvf_survey.ret_start.y, 0)) {
gvf_survey.stage = gTURN2;
nav_init_stage();
@@ -311,8 +312,8 @@ bool gvf_nav_survey_polygon_run(void)
//turn from return to stage
} else if (gvf_survey.stage == gTURN2) {
float rad_sur = (2 * gvf_survey.psa_min_rad + gvf_survey.psa_sweep_width) * 0.5;
- gvf_nav_direction_circle(rad_sur);
- gvf_ellipse_XY(gvf_survey.seg_center2.x, gvf_survey.seg_center2.y, rad_sur, rad_sur, 0);
+ gvf_circle_direction(rad_sur);
+ nav_gvf_ellipse_XY(gvf_survey.seg_center2.x, gvf_survey.seg_center2.y, rad_sur, rad_sur, 0);
if (NavCourseCloseTo(gvf_survey.segment_angle)) {
gvf_survey.stage = gSEG;
nav_init_stage();
diff --git a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h
index 634d27b37c..5f3bf88626 100644
--- a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h
+++ b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h
@@ -27,8 +27,8 @@
*
*/
-#ifndef NAV_SURVEY_POLYGON_GVF_H
-#define NAV_SURVEY_POLYGON_GVF_H
+#ifndef SURVEY_POLYGON_GVF_H
+#define SURVEY_POLYGON_GVF_H
#include "std.h"
#include "math/pprz_algebra_float.h"
@@ -83,11 +83,11 @@ struct gvf_SurveyPolyAdv {
struct FloatVect2 ret_end;
};
-// external setting
-extern void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
+/** ------------------------------------------------------------------------ **/
+
+// Polygon survey
+extern void nav_gvf_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude);
+extern bool nav_gvf_survey_polygon_run(void);
-void gvf_nav_direction_circle(float rad);
-extern bool gvf_nav_survey_polygon_run(void);
-
-#endif
+#endif // SURVEY_POLYGON_GVF_H
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c b/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c
deleted file mode 100644
index fc42c1db0c..0000000000
--- a/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * Copyright (C) 2016 Hector Garcia de Marina
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file gvf_line.c
- *
- * Guidance algorithm based on vector fields
- * 2D straight line trajectory
- */
-
-
-#include "modules/nav/common_nav.h"
-#include "gvf_line.h"
-#include "generated/airframe.h"
-
-/*! Gain ke for the line trajectory*/
-#ifndef GVF_LINE_KE
-#define GVF_LINE_KE 1
-#endif
-
-/*! Gain kn for the line trajectory*/
-#ifndef GVF_LINE_KN
-#define GVF_LINE_KN 1
-#endif
-
-/*! Default heading in degrees for a trajectory called from gvf_line_**_HEADING */
-#ifndef GVF_LINE_HEADING
-#define GVF_LINE_HEADING 0
-#endif
-
-/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x1,y1 before turning back */
-#ifndef GVF_SEGMENT_D1
-#define GVF_SEGMENT_D1 0
-#endif
-
-/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x2,y2 before turning back */
-#ifndef GVF_SEGMENT_D2
-#define GVF_SEGMENT_D2 0
-#endif
-
-gvf_li_par gvf_line_par = {GVF_LINE_KE, GVF_LINE_KN, GVF_LINE_HEADING};
-gvf_seg_par gvf_segment_par = {GVF_SEGMENT_D1, GVF_SEGMENT_D2};
-
-void gvf_line_info(float *phi, struct gvf_grad *grad,
- struct gvf_Hess *hess)
-{
-
- struct EnuCoor_f *p = stateGetPositionEnu_f();
- float px = p->x;
- float py = p->y;
- float a = gvf_trajectory.p[0];
- float b = gvf_trajectory.p[1];
- float alpha = gvf_trajectory.p[2];
-
- // Phi(x,y)
- *phi = -(px - a) * cosf(alpha) + (py - b) * sinf(alpha);
-
- // grad Phi
- grad->nx = -cosf(alpha);
- grad->ny = sinf(alpha);
-
- // Hessian Phi
- hess->H11 = 0;
- hess->H12 = 0;
- hess->H21 = 0;
- hess->H22 = 0;
-}
diff --git a/sw/airborne/modules/guidance/gvf_common.c b/sw/airborne/modules/guidance/gvf_common.c
index 4e277fbe42..430da9500e 100644
--- a/sw/airborne/modules/guidance/gvf_common.c
+++ b/sw/airborne/modules/guidance/gvf_common.c
@@ -18,7 +18,107 @@
* .
*/
-#include "./gvf_common.h"
+#include "gvf_common.h"
-gvf_common_omega gvf_c_omega;
-gvf_common_params gvf_c_info;
+// Get MODULE_H from loaded modules
+#include "generated/modules.h"
+
+/** ------------------------------------------------------------------------ **/
+
+gvf_common_ctrl gvf_c_ctrl;
+gvf_common_info gvf_c_info;
+gvf_common_params gvf_c_params = {1,1};
+
+// State
+gvf_common_state gvf_c_state;
+
+/** ------------------------------------------------------------------------ **/
+
+void gvf_low_level_getState(void)
+{
+
+ gvf_c_state.px = stateGetPositionEnu_f()->x;
+ gvf_c_state.py = stateGetPositionEnu_f()->y;
+
+ #if defined(FIXEDWING_FIRMWARE)
+ float ground_speed = stateGetHorizontalSpeedNorm_f();
+ gvf_c_state.course = stateGetHorizontalSpeedDir_f();
+ gvf_c_state.px_dot = ground_speed * sinf(gvf_c_state.course);
+ gvf_c_state.py_dot = ground_speed * cosf(gvf_c_state.course);
+
+ #elif defined(ROVER_FIRMWARE) || defined(ROTORCRAFT_FIRMWARE)
+ // We assume that the course and psi
+ // of the rover (steering wheel) are the same
+ gvf_c_state.course = stateGetNedToBodyEulers_f()->psi;
+ gvf_c_state.px_dot = stateGetSpeedEnu_f()->x;
+ gvf_c_state.py_dot = stateGetSpeedEnu_f()->y;
+ #endif
+}
+
+void gvf_low_level_control_2D(float omega)
+{
+
+ #if defined(FIXEDWING_FIRMWARE)
+ if (autopilot_get_mode() == AP_MODE_AUTO2) {
+
+ // Coordinated turn
+ struct FloatEulers *att = stateGetNedToBodyEulers_f();
+ float ground_speed = stateGetHorizontalSpeedNorm_f();
+
+ lateral_mode = LATERAL_MODE_ROLL;
+
+ h_ctl_roll_setpoint =
+ - gvf_c_params.k_roll * atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
+ BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
+ }
+ #endif
+
+ gvf_c_ctrl.omega = omega;
+}
+
+void gvf_low_level_control_3D(float heading_rate, float climbing_rate)
+{
+ #if defined(FIXEDWING_FIRMWARE)
+ if (autopilot_get_mode() == AP_MODE_AUTO2) {
+ // Vertical Z coordinate
+ v_ctl_mode = V_CTL_MODE_AUTO_CLIMB;
+ v_ctl_speed_mode = V_CTL_SPEED_THROTTLE;
+
+ v_ctl_climb_setpoint = gvf_c_params.k_climb * climbing_rate; // Setting point for vertical speed
+
+ // Lateral XY coordinates
+ lateral_mode = LATERAL_MODE_ROLL;
+
+ struct FloatEulers *att = stateGetNedToBodyEulers_f();
+ float ground_speed = stateGetHorizontalSpeedNorm_f();
+
+ h_ctl_roll_setpoint =
+ -gvf_c_params.k_roll * atanf(heading_rate * ground_speed / GVF_GRAVITY / cosf(att->theta));
+ BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
+ }
+ #endif
+
+ gvf_low_level_control_2D(heading_rate);
+
+ (void)(climbing_rate); // Avoid unused parameter warning
+}
+
+bool gvf_nav_approaching(float wp_x, float wp_y, float from_x, float from_y, float t)
+{
+
+ #if defined(FIXEDWING_FIRMWARE)
+ return nav_approaching_xy(wp_x, wp_y, from_x, from_y, t);
+
+ #else // FIXEDWING & ROVER FIRMWARE
+ struct EnuCoor_f wp, from;
+
+ wp.x = wp_x;
+ wp.y = wp_y;
+ from.x = from_x;
+ from.y = from_y;
+
+ return nav.nav_approaching(&wp, &from, t);
+ #endif
+
+ // Avoid unused parameter warning
+}
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf_common.h b/sw/airborne/modules/guidance/gvf_common.h
index d4a22d455b..565e0b0afe 100644
--- a/sw/airborne/modules/guidance/gvf_common.h
+++ b/sw/airborne/modules/guidance/gvf_common.h
@@ -21,33 +21,90 @@
#ifndef GVF_COMMON_H
#define GVF_COMMON_H
-// uint32_t
+#ifdef FIXEDWING_FIRMWARE
+#include "firmwares/fixedwing/nav.h"
+#include "modules/nav/common_nav.h"
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+#include "firmwares/fixedwing/guidance/guidance_v_n.h" // 3D low level control is only compatible with the new pprz controller!
+#define gvf_setNavMode(_navMode) (horizontal_mode = _navMode)
+#define GVF_MODE_ROUTE HORIZONTAL_MODE_ROUTE
+#define GVF_MODE_WAYPOINT HORIZONTAL_MODE_WAYPOINT
+#define GVF_MODE_CIRCLE HORIZONTAL_MODE_CIRCLE
+
+#elif defined(ROVER_FIRMWARE)
+#include "firmwares/rover/navigation.h"
+#define gvf_setNavMode(_navMode) (nav.mode = _navMode)
+#define GVF_MODE_ROUTE NAV_MODE_ROUTE
+#define GVF_MODE_WAYPOINT NAV_MODE_WAYPOINT
+#define GVF_MODE_CIRCLE NAV_MODE_CIRCLE
+
+#elif defined(ROTORCRAFT_FIRMWARE)
+#include "firmwares/rotorcraft/navigation.h"
+#define gvf_setNavMode(_navMode) (nav.horizontal_mode = _navMode)
+#define GVF_MODE_ROUTE NAV_HORIZONTAL_MODE_ROUTE
+#define GVF_MODE_WAYPOINT NAV_HORIZONTAL_MODE_WAYPOINT
+#define GVF_MODE_CIRCLE NAV_HORIZONTAL_MODE_CIRCLE
+
+#else
+#error "GVF does not support your firmware yet!"
+#endif
+
#include "std.h"
-/** @typedef gvf_common_omega
-* @brief Horizontal control signal for both gvf
-* @param omega is the horizontal control signal
-*/
+#define GVF_GRAVITY 9.806
+/** ------------------------------------------------------------------------ **/
+
+/** @typedef gvf_common_state
+ * @brief State variables for GVF
+ */
typedef struct {
- float omega;
-} gvf_common_omega;
+ float px;
+ float py;
+ float course;
+ float px_dot;
+ float py_dot;
+} gvf_common_state;
-extern gvf_common_omega gvf_c_omega;
+/** @typedef gvf_common_ctrl
+ * @brief Control-based variables for GVF
+ * @param omega Heading control signal
+ */
+typedef struct{
+ float omega;
+} gvf_common_ctrl;
-/** @typedef gvf_common_params
-* @brief Different parameters obtained from gvfs. dot means d/dt
-* @param kappa is the curve's curvature
-* @param ori_err is the orientation error
-*/
+/** @typedef gvf_common_info
+ * @brief Common GVF internal variables
+ * @param kappa GVF trajectory's curvature
+ * @param ori_err GVF orientation error
+ */
typedef struct {
float kappa;
- float kappa_dot;
float ori_err;
- float ori_err_dot;
+} gvf_common_info;
+
+/** @typedef gvf_common_params
+ * @brief Common GVF parameters
+ * @param k_roll GVF roll gain for the horizontal low level control
+ * @param k_climb GVF climb gain for the vertucal low level control
+ */
+typedef struct {
+ float k_roll;
+ float k_climb;
} gvf_common_params;
-extern gvf_common_params gvf_c_info;
+extern gvf_common_state gvf_c_state;
+extern gvf_common_ctrl gvf_c_ctrl;
+extern gvf_common_info gvf_c_info;
+extern gvf_common_params gvf_c_params;
-#endif // GVF_COMMON_H
+/** ------------------------------------------------------------------------ **/
+// Low level control functions
+extern void gvf_low_level_getState(void);
+extern void gvf_low_level_control_2D(float omega);
+extern void gvf_low_level_control_3D(float heading_rate, float climbing_rate);
+extern bool gvf_nav_approaching(float wp_x, float wp_y, float from_x, float from_y, float t);
+
+#endif // GVF_COMMON_H
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf_ik/gvf_ik.c b/sw/airborne/modules/guidance/gvf_ik/gvf_ik.c
new file mode 100644
index 0000000000..298b267e3e
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/gvf_ik.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include
+#include "std.h"
+
+#include "gvf_ik.h"
+#include "autopilot.h"
+
+/* TODO:
+ * - Reset gamma_t0 whenever any gamma setting is updated.
+ * - The GVF drawn in the GCS is not the IK-GVF, so it needs to be fixed.
+ */
+
+// Control
+gvf_ik_con gvf_ik_control = {0};
+gvf_ik_tel gvf_ik_telemetry = {0};
+
+// Time variables to check if GVF is active
+static uint32_t gamma_t0 = 0;
+static uint32_t last_gvf_t0 = 0;
+
+/** TELEMETRY -------------------------------------------------------------- **/
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+static void send_gvf(struct transport_tx *trans, struct link_device *dev)
+{
+
+ pprz_msg_send_GVF_IK(
+ trans, dev, AC_ID,
+ &gvf_ik_control.error,
+ &gvf_ik_control.gamma,
+ &gvf_ik_control.gamma_dot,
+ &gvf_ik_control.gamma_amplitude,
+ &gvf_ik_control.gamma_omega
+ );
+
+ uint32_t delta_T = get_sys_time_msec() - last_gvf_t0;
+ if (delta_T < 200) {
+ uint8_t traj_type = (uint8_t)gvf_trajectory.type;
+
+ pprz_msg_send_GVF(
+ trans, dev, AC_ID,
+ &gvf_ik_control.phi,
+ &traj_type,
+ &gvf_ik_control.s,
+ &gvf_ik_control.ke,
+ gvf_trajectory.p_len, gvf_trajectory.p,
+ &gvf_ik_control.error_n,
+ &gvf_ik_telemetry.n_norm,
+ &gvf_ik_telemetry.t_norm,
+ &gvf_ik_telemetry.omega_d,
+ &gvf_ik_telemetry.omega);
+
+#if GVF_OCAML_GCS
+ if (gvf_trajectory.type == ELLIPSE &&
+ ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
+ pprz_msg_send_CIRCLE(trans, dev, AC_ID,
+ &gvf_trajectory.p[0], &gvf_trajectory.p[1],
+ &gvf_trajectory.p[2]);
+ }
+
+ if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
+ pprz_msg_send_SEGMENT(trans, dev, AC_ID,
+ &gvf_segment.x1, &gvf_segment.y1,
+ &gvf_segment.x2, &gvf_segment.y2);
+ }
+#endif // GVF_OCAML_GCS
+
+ }
+}
+
+#endif // PERIODIC_TELEMETRY
+
+/** ------------------------------------------------------------------------ **/
+
+/** STATIC FUNCTIONS ------------------------------------------------------- **/
+
+static void reset_gamma_t0(void)
+{
+ gamma_t0 = get_sys_time_msec();
+}
+
+static bool check_alpha(float ke, float J1, float J2, float phi, float speed)
+{
+ float J_Jt = (J1*J1 + J2*J2);
+
+ float e = phi - gvf_ik_control.gamma;
+ float e_tdot = - gvf_ik_control.gamma_dot;
+
+ float u = - ke * e;
+
+ float nx = J1 / J_Jt * (u - e_tdot);
+ float ny = J2 / J_Jt * (u - e_tdot);
+
+ float un_norm2 = nx*nx + ny*ny;
+
+ return un_norm2 < speed*speed;
+}
+
+/** ------------------------------------------------------------------------ **/
+
+void gvf_ik_init(void)
+{
+ gvf_ik_control.ke = 1;
+ gvf_ik_control.kn = 1;
+ gvf_ik_control.s = 1;
+ gvf_trajectory.type = NONE;
+
+ gvf_ik_control.gamma_amplitude = GVF_IK_GAMMA_AMPLITUDE;
+ gvf_ik_control.gamma_omega = GVF_IK_GAMMA_OMEGA;
+
+ reset_gamma_t0();
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GVF, send_gvf);
+#endif // PERIODIC_TELEMETRY
+}
+
+// GENERIC TRAJECTORY CONTROLLER
+void gvf_ik_control_2D(float ke, float kn, float phi,
+ struct gvf_grad *grad, struct gvf_Hess *hess)
+{
+
+ // compute gamma period
+ float gamma_period = 2 * M_PI / gvf_ik_control.gamma_omega;
+
+ // compute gamma t
+ last_gvf_t0 = get_sys_time_msec();
+ float t = (last_gvf_t0 - gamma_t0) / 1000;
+
+ if (t > gamma_period) {
+ reset_gamma_t0();
+ }
+
+ // get state
+ gvf_low_level_getState();
+ float course = gvf_c_state.course;
+ float px_dot = gvf_c_state.px_dot;
+ float py_dot = gvf_c_state.py_dot;
+
+ float speed = sqrtf(px_dot*px_dot + py_dot*py_dot);
+ float speed2 = speed * speed;
+
+ int s = gvf_ik_control.s;
+
+ // gradient Phi
+ float J1 = grad->nx;
+ float J2 = grad->ny;
+ float J_Jt = (J1*J1 + J2*J2);
+
+ // Hessian
+ float H11 = hess->H11;
+ float H12 = hess->H12;
+ float H21 = hess->H21;
+ float H22 = hess->H22;
+
+ // -- Calculation of the desired angular velocity in the vector field --------
+ gvf_ik_control.gamma = gvf_ik_control.gamma_amplitude * sinf(gvf_ik_control.gamma_omega * t);
+ gvf_ik_control.gamma_dot = gvf_ik_control.gamma_omega * gvf_ik_control.gamma_amplitude * cosf(gvf_ik_control.gamma_omega * t);
+
+ bool cond_flag = check_alpha(ke, J1, J2, phi, speed);
+
+ float e = phi;
+ float e_tdot = 0;
+ float e_tddot = 0;
+ if (cond_flag) {
+ e = e - gvf_ik_control.gamma;
+ e_tdot = - gvf_ik_control.gamma_dot;
+ e_tddot = (gvf_ik_control.gamma_omega * gvf_ik_control.gamma_omega) * gvf_ik_control.gamma;
+ }
+
+ // control law
+ float u = - ke * e;
+
+ // normal to Phi in p_dot (IK)
+ float nx = J1 / J_Jt * (u - e_tdot);
+ float ny = J2 / J_Jt * (u - e_tdot);
+
+ float n_norm2 = nx*nx + ny*ny;
+ float n_norm = sqrtf(n_norm2);
+
+ // tangent to Phi in p_dot
+ float tx = s * J2;
+ float ty = -s * J1;
+
+ float t_norm = sqrtf(tx*tx + ty*ty);
+ float tx_hat = tx / t_norm;
+ float ty_hat = ty / t_norm;
+
+ // compute alpha and p_dot
+ float alpha = 0;
+ float pdx_dot = 0;
+ float pdy_dot = 0;
+
+ if (cond_flag){
+ alpha = sqrtf(speed2 - n_norm2);
+ pdx_dot = alpha * tx_hat + nx;
+ pdy_dot = alpha * ty_hat + ny;
+ } else {
+ pdx_dot = speed * nx / n_norm;
+ pdy_dot = speed * ny / n_norm;
+ }
+
+ // compute n_dot
+ float u_dot = - ke * (J1*pdx_dot + J2*pdy_dot + e_tdot);
+
+ float un_dot_A_x = (pdx_dot*H11 + pdy_dot*H21);
+ float un_dot_A_y = (pdx_dot*H12 + pdy_dot*H22);
+
+ float B_term = 2 * ((H11*pdx_dot + H21*pdy_dot)*J1 + (H21*pdx_dot + H22*pdy_dot)*J2) / J_Jt;
+ float un_dot_B_x = - J1 * B_term;
+ float un_dot_B_y = - J2 * B_term;
+
+ float C_term = (u_dot - e_tddot) / J_Jt;
+ float un_dot_C_x = J1 * C_term;
+ float un_dot_C_y = J2 * C_term;
+
+ float nx_dot = (un_dot_A_x + un_dot_B_x) * (u - e_tdot) / J_Jt + un_dot_C_x;
+ float ny_dot = (un_dot_A_y + un_dot_B_y) * (u - e_tdot) / J_Jt + un_dot_C_y;
+
+ // compute omega_d and omega
+ float pd_ddot_x = 0;
+ float pd_ddot_y = 0;
+
+ if (cond_flag){
+ float alpha_dot = - (nx*nx_dot + ny*ny_dot) / (alpha);
+
+ float tx_dot = s * (H12 * pdx_dot + H22 * pdy_dot);
+ float ty_dot = - s * (H11 * pdx_dot + H21 * pdy_dot);
+
+ float t_norm3 = t_norm * t_norm * t_norm;
+ float Bpd_ddot_x = alpha * (tx_dot / t_norm + (tx*tx*tx_dot - tx*ty*ty_dot) / t_norm3);
+ float Bpd_ddot_y = alpha * (ty_dot / t_norm + (tx*ty*tx_dot - ty*ty*ty_dot) / t_norm3);
+
+ pd_ddot_x = alpha_dot * tx_hat + Bpd_ddot_x + nx_dot;
+ pd_ddot_y = alpha_dot * ty_hat + Bpd_ddot_y + ny_dot;
+ } else {
+ float n_norm3 = n_norm2 * n_norm;
+ pd_ddot_x = speed * (nx_dot / n_norm + (nx*nx*nx_dot - nx*ny*ny_dot) / n_norm3);
+ pd_ddot_y = speed * (ny_dot / n_norm + (nx*ny*nx_dot - ny*ny*ny_dot) / n_norm3);
+ }
+
+
+ float omega_d = - (- pdx_dot*pd_ddot_y + pdy_dot*pd_ddot_x) / speed2;
+
+ float rx = speed * sinf(course);
+ float ry = speed * cosf(course);
+
+ float e_n = (pdx_dot*ry - pdy_dot*rx) / speed2;
+ float omega = omega_d - kn * e_n;
+
+ // Update external control-related and telemetry variables
+ gvf_ik_control.phi = phi;
+ gvf_ik_control.error = e;
+ gvf_ik_control.error_n = e_n;
+ gvf_ik_telemetry.n_norm = n_norm;
+ gvf_ik_telemetry.t_norm = alpha;
+ gvf_ik_telemetry.omega_d = omega_d;
+ gvf_ik_telemetry.omega = omega;
+
+ //printf("t: %f, pdx_dot: %f, pdy_dot: %f, rx: %f , ry: %f, omega: %f \n", t, pdx_dot, pdy_dot, rx, ry, omega);
+
+ // ---------------------------------------------------------------------------
+
+ // Set GVF common info
+ gvf_c_info.kappa = (J1*(H12*J2 - J1*H22) + J2*(H21*J1 - H11*J2))/powf(J_Jt, 1.5);
+ gvf_c_info.ori_err = e_n;
+
+ // Send the computed omega to the low level control
+ gvf_low_level_control_2D(omega);
+}
+
+void gvf_ik_set_direction(int8_t s)
+{
+ gvf_ik_control.s = s;
+}
+
+/** ------------------------------------------------------------------------ **/
diff --git a/sw/airborne/modules/guidance/gvf_ik/gvf_ik.h b/sw/airborne/modules/guidance/gvf_ik/gvf_ik.h
new file mode 100644
index 0000000000..22a594b8a0
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/gvf_ik.h
@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file gvf.h
+ *
+ * Guidance algorithm based on vector fields
+ */
+
+#ifndef GVF_IK_H
+#define GVF_IK_H
+
+#define GVF_GRAVITY 9.806
+
+/*! GVF-IK settings */
+#ifndef GVF_IK_GAMMA_AMPLITUDE
+#define GVF_IK_GAMMA_AMPLITUDE 0
+#endif
+
+#ifndef GVF_IK_GAMMA_OMEGA
+#define GVF_IK_GAMMA_OMEGA 0
+#endif
+
+/*! Default GCS trajectory painter */
+#ifndef GVF_OCAML_GCS
+#define GVF_OCAML_GCS true
+#endif
+
+#include "std.h"
+#include "modules/guidance/gvf_common.h"
+#include "modules/guidance/trajectories/gvf_traj.h"
+
+/** STRUCTS ---------------------------------------------------------------- **/
+
+/** @typedef gvf_con
+* @brief Control parameters for the GVF
+* @param ke Gain defining how agressive is the vector field
+* @param kn Gain for making converge the vehile to the vector field
+* @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory.
+* @param s Defines the direction to be tracked. Its meaning depends on the trajectory and its implementation. Check the wiki entry of the GVF. It takes the values -1 or 1.
+*/
+typedef struct {
+ float ke;
+ float kn;
+ float phi;
+ float error;
+ float error_n;
+ int8_t s;
+
+ float gamma_amplitude;
+ float gamma_omega;
+ float gamma;
+ float gamma_dot;
+} gvf_ik_con;
+
+typedef struct {
+ float n_norm;
+ float t_norm;
+ float omega_d;
+ float omega;
+} gvf_ik_tel;
+
+// Extern structs
+extern gvf_ik_con gvf_ik_control;
+
+/** EXTERN FUNCTIONS ------------------------------------------------------- **/
+
+extern void gvf_ik_init(void);
+extern void gvf_ik_control_2D(float ke, float kn, float e, struct gvf_grad *, struct gvf_Hess *);
+extern void gvf_ik_set_direction(int8_t s);
+
+#endif // GVF_IK_H
diff --git a/sw/airborne/modules/guidance/gvf_ik/nav/nav_ellipse.c b/sw/airborne/modules/guidance/gvf_ik/nav/nav_ellipse.c
new file mode 100644
index 0000000000..8855f64177
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/nav/nav_ellipse.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "nav_ellipse.h"
+
+#include "generated/airframe.h"
+#include "modules/guidance/gvf_ik/gvf_ik.h"
+
+/*! Default gain ke for the ellipse trajectory */
+#ifndef GVF_IK_ELLIPSE_KE
+#define GVF_IK_ELLIPSE_KE 1
+#endif
+
+/*! Default gain kn for the ellipse trajectory */
+#ifndef GVF_IK_ELLIPSE_KN
+#define GVF_IK_ELLIPSE_KN 1
+#endif
+
+/*! Default first axis for the ellipse trajectory */
+#ifndef GVF_IK_ELLIPSE_A
+#define GVF_IK_ELLIPSE_A 80
+#endif
+
+/*! Default second axis for the ellipse trajectory */
+#ifndef GVF_IK_ELLIPSE_B
+#define GVF_IK_ELLIPSE_B 80
+#endif
+
+/*! Default orientation in degrees for the ellipse trajectory */
+#ifndef GVF_IK_ELLIPSE_ALPHA
+#define GVF_IK_ELLIPSE_ALPHA 0
+#endif
+
+gvf_ik_ell_par gvf_ik_ellipse_par = {
+ GVF_IK_ELLIPSE_KE, GVF_IK_ELLIPSE_KN, GVF_IK_ELLIPSE_A, GVF_IK_ELLIPSE_B, GVF_IK_ELLIPSE_ALPHA
+};
+
+// Param array lenght
+static int gvf_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+// ELLIPSE
+
+bool nav_gvf_ik_ellipse_XY(float x, float y, float a, float b, float alpha)
+{
+ float e;
+ struct gvf_grad grad_ellipse;
+ struct gvf_Hess Hess_ellipse;
+
+ gvf_trajectory.type = 1;
+ gvf_trajectory.p[0] = x;
+ gvf_trajectory.p[1] = y;
+ gvf_trajectory.p[2] = a;
+ gvf_trajectory.p[3] = b;
+ gvf_trajectory.p[4] = alpha;
+ gvf_trajectory.p_len= 5 + gvf_p_len_wps;
+ gvf_p_len_wps = 0;
+
+ // SAFE MODE
+ if (a < 1 || b < 1) {
+ gvf_trajectory.p[2] = 60;
+ gvf_trajectory.p[3] = 60;
+ }
+
+ if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
+ gvf_setNavMode(GVF_MODE_CIRCLE);
+
+ } else {
+ gvf_setNavMode(GVF_MODE_WAYPOINT);
+ }
+
+ gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
+ gvf_ik_control.ke = gvf_ik_ellipse_par.ke;
+ gvf_ik_control_2D(gvf_ik_ellipse_par.ke, gvf_ik_ellipse_par.kn,
+ e, &grad_ellipse, &Hess_ellipse);
+
+ gvf_ik_control.error = e;
+
+ return true;
+}
+
+
+bool nav_gvf_ik_ellipse_wp(uint8_t wp, float a, float b, float alpha)
+{
+ gvf_trajectory.p[5] = wp;
+ gvf_p_len_wps = 1;
+
+ nav_gvf_ik_ellipse_XY(WaypointX(wp), WaypointY(wp), a, b, alpha);
+ return true;
+}
diff --git a/sw/airborne/modules/guidance/gvf_ik/nav/nav_ellipse.h b/sw/airborne/modules/guidance/gvf_ik/nav/nav_ellipse.h
new file mode 100644
index 0000000000..b908659a34
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/nav/nav_ellipse.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef GVF_IK_ELLIPSE_H
+#define GVF_IK_ELLIPSE_H
+
+#include "modules/guidance/trajectories/gvf_traj.h"
+
+/** @typedef gvf_ell_par
+* @brief Parameters for the GVF line trajectory
+* @param ke Gain defining how agressive is the vector field
+* @param kn Gain for making converge the vehile to the vector field
+* @param a First axis of the ellipse in meters
+* @param b Second axis of the ellipse in meters
+* @param alpha Orientation of the ellipse in rads
+*/
+typedef struct {
+ float ke;
+ float kn;
+ float a;
+ float b;
+ float alpha;
+} gvf_ik_ell_par;
+
+/** ------------------------------------------------------------------------ **/
+
+extern gvf_ik_ell_par gvf_ik_ellipse_par;
+
+// Ellipse
+extern bool nav_gvf_ik_ellipse_wp(uint8_t wp, float a, float b, float alpha);
+extern bool nav_gvf_ik_ellipse_XY(float x, float y, float a, float b, float alpha);
+
+#endif // GVF_IK_ELLIPSE_H
diff --git a/sw/airborne/modules/guidance/gvf_ik/nav/nav_line.c b/sw/airborne/modules/guidance/gvf_ik/nav/nav_line.c
new file mode 100644
index 0000000000..3f9ba59c04
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/nav/nav_line.c
@@ -0,0 +1,249 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "nav_line.h"
+
+#include "generated/airframe.h"
+#include "modules/guidance/gvf_ik/gvf_ik.h"
+
+/*! Gain ke for the line trajectory*/
+#ifndef GVF_IK_LINE_KE
+#define GVF_IK_LINE_KE 1
+#endif
+
+/*! Gain kn for the line trajectory*/
+#ifndef GVF_IK_LINE_KN
+#define GVF_IK_LINE_KN 1
+#endif
+
+/*! Default heading in degrees for a trajectory called from gvf_line_**_HEADING */
+#ifndef GVF_IK_LINE_HEADING
+#define GVF_IK_LINE_HEADING 0
+#endif
+
+/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x1,y1 before turning back */
+#ifndef GVF_IK_SEGMENT_D1
+#define GVF_IK_SEGMENT_D1 0
+#endif
+
+/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x2,y2 before turning back */
+#ifndef GVF_IK_SEGMENT_D2
+#define GVF_IK_SEGMENT_D2 0
+#endif
+
+gvf_ik_li_par gvf_ik_line_par = {GVF_IK_LINE_KE, GVF_IK_LINE_KN, GVF_IK_LINE_HEADING};
+gvf_ik_seg_par gvf_ik_segment_par = {GVF_IK_SEGMENT_D1, GVF_IK_SEGMENT_D2};
+
+// Param array lenght
+static int gvf_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+static void gvf_ik_line(float a, float b, float heading)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 0;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = heading;
+ gvf_trajectory.p_len= 3 + gvf_p_len_wps;
+ gvf_p_len_wps = 0;
+
+ gvf_line_info(&e, &grad_line, &Hess_line);
+ gvf_ik_control.ke = gvf_ik_line_par.ke;
+ gvf_ik_control_2D(1e-2 * gvf_ik_line_par.ke, gvf_ik_line_par.kn, e, &grad_line, &Hess_line);
+
+ gvf_ik_control.error = e;
+
+ gvf_setNavMode(GVF_MODE_WAYPOINT);
+
+ gvf_segment.seg = 0;
+}
+
+static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
+{
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x - x1;
+ float py = p->y - y1;
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+ float alpha = atan2f(zy, zx);
+
+ float cosa = cosf(-alpha);
+ float sina = sinf(-alpha);
+
+ float pxr = px * cosa - py * sina;
+ float zxr = zx * cosa - zy * sina;
+
+ int s = 0;
+
+ if (pxr < -d1) {
+ s = 1;
+ } else if (pxr > (zxr + d2)) {
+ s = -1;
+ }
+
+ if (zy < 0) {
+ s *= -1;
+ }
+
+ return s;
+}
+
+/** ------------------------------------------------------------------------ **/
+
+// STRAIGHT LINE
+
+bool nav_gvf_ik_line_XY_heading(float a, float b, float heading)
+{
+ gvf_ik_set_direction(1);
+ gvf_ik_line(a, b, heading);
+ return true;
+}
+
+bool nav_gvf_ik_line_wp_heading(uint8_t wp, float heading)
+{
+ heading = RadOfDeg(heading);
+
+ float a = WaypointX(wp);
+ float b = WaypointY(wp);
+
+ return nav_gvf_ik_line_XY_heading(a, b, heading);
+}
+
+bool nav_gvf_ik_line_XY1_XY2(float x1, float y1, float x2, float y2)
+{
+ if (gvf_p_len_wps != 2) {
+ gvf_trajectory.p[3] = x2;
+ gvf_trajectory.p[4] = y2;
+ gvf_trajectory.p[5] = 0;
+ gvf_p_len_wps = 3;
+ }
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+
+ nav_gvf_ik_line_XY_heading(x1, y1, atan2f(zx, zy));
+
+ gvf_setNavMode(GVF_MODE_ROUTE);
+ gvf_segment.seg = 1;
+ gvf_segment.x1 = x1;
+ gvf_segment.y1 = y1;
+ gvf_segment.x2 = x2;
+ gvf_segment.y2 = y2;
+
+ return true;
+}
+
+bool nav_gvf_ik_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
+{
+ gvf_trajectory.p[3] = wp1;
+ gvf_trajectory.p[4] = wp2;
+ gvf_p_len_wps = 2;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ return nav_gvf_ik_line_XY1_XY2(x1, y1, x2, y2);
+}
+
+bool nav_gvf_ik_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
+{
+ int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
+ if (s != 0) {
+ gvf_ik_set_direction(s);
+ }
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+ float alpha = atanf(zx / zy);
+
+ gvf_ik_line(x1, y1, alpha);
+
+ gvf_setNavMode(GVF_MODE_ROUTE);
+
+ gvf_segment.seg = 1;
+ gvf_segment.x1 = x1;
+ gvf_segment.y1 = y1;
+ gvf_segment.x2 = x2;
+ gvf_segment.y2 = y2;
+
+ return true;
+}
+
+bool nav_gvf_ik_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
+{
+ gvf_trajectory.p[3] = wp1;
+ gvf_trajectory.p[4] = wp2;
+ gvf_trajectory.p[5] = d1;
+ gvf_trajectory.p[6] = d2;
+ gvf_p_len_wps = 4;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ return nav_gvf_ik_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
+}
+
+bool nav_gvf_ik_segment_XY1_XY2(float x1, float y1, float x2, float y2)
+{
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x - x1;
+ float py = p->y - y1;
+
+ float zx = x2 - x1;
+ float zy = y2 - y1;
+
+ float beta = atan2f(zy, zx);
+ float cosb = cosf(-beta);
+ float sinb = sinf(-beta);
+ float zxr = zx * cosb - zy * sinb;
+ float pxr = px * cosb - py * sinb;
+
+ if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
+ return false;
+ }
+
+ return nav_gvf_ik_line_XY1_XY2(x1, y1, x2, y2);
+}
+
+bool nav_gvf_ik_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
+{
+ gvf_trajectory.p[3] = wp1;
+ gvf_trajectory.p[4] = wp2;
+ gvf_p_len_wps = 2;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ return nav_gvf_ik_segment_XY1_XY2(x1, y1, x2, y2);
+}
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf_ik/nav/nav_line.h b/sw/airborne/modules/guidance/gvf_ik/nav/nav_line.h
new file mode 100644
index 0000000000..6422f80c0c
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/nav/nav_line.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef GVF_IK_LINE_H
+#define GVF_IK_LINE_H
+
+#include "modules/guidance/trajectories/gvf_traj.h"
+
+/** @typedef gvf_li_par
+* @brief Parameters for the GVF line trajectory
+* @param ke Gain defining how agressive is the vector field
+* @param kn Gain for making converge the vehile to the vector field
+* @param heading Heading in rads defining the orientation of the line
+*/
+typedef struct {
+ float ke;
+ float kn;
+ float heading;
+} gvf_ik_li_par;
+
+/** @typedef gvf_seg_par
+* @brief Parameters for the segment case of the GVF line trajectory
+* @param d1 Distance beyond x1,y1 that the vehicle travels before turning back
+* @param d2 Distance beyond x2,y2 that the vehicle travels before turning back
+*/
+typedef struct {
+ float d1;
+ float d2;
+} gvf_ik_seg_par;
+
+/** ------------------------------------------------------------------------ **/
+
+extern gvf_ik_li_par gvf_ik_line_par;
+extern gvf_ik_seg_par gvf_ik_segment_par;
+
+// Straight line
+extern bool nav_gvf_ik_line_XY_heading(float x, float y, float heading);
+extern bool nav_gvf_ik_line_wp_heading(uint8_t wp, float heading);
+extern bool nav_gvf_ik_line_XY1_XY2(float x1, float y1, float x2, float y2);
+extern bool nav_gvf_ik_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
+extern bool nav_gvf_ik_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2);
+extern bool nav_gvf_ik_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2);
+extern bool nav_gvf_ik_segment_XY1_XY2(float x1, float y1, float x2, float y2);
+extern bool nav_gvf_ik_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
+
+#endif // GVF_IK_LINE_H
diff --git a/sw/airborne/modules/guidance/gvf_ik/nav/nav_sin.c b/sw/airborne/modules/guidance/gvf_ik/nav/nav_sin.c
new file mode 100644
index 0000000000..74e895fcc8
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/nav/nav_sin.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "nav_sin.h"
+
+#include "generated/airframe.h"
+#include "modules/guidance/gvf_ik/gvf_ik.h"
+
+/*! Default gain ke for the sin trajectory*/
+#ifndef GVF_IK_SIN_KE
+#define GVF_IK_SIN_KE 1
+#endif
+
+/*! Default gain kn for the sin trajectory*/
+#ifndef GVF_IK_SIN_KN
+#define GVF_IK_SIN_KN 1
+#endif
+
+/*! Default orientation in rads for the sin trajectory function gvf_sin_**_alpha*/
+#ifndef GVF_IK_SIN_ALPHA
+#define GVF_IK_SIN_ALPHA 0
+#endif
+
+/*! Default frequency for the sin trajectory in rads*/
+#ifndef GVF_IK_SIN_W
+#define GVF_IK_SIN_W 0
+#endif
+
+/*! Default off-set in rads for the sin trajectory in rads*/
+#ifndef GVF_IK_SIN_OFF
+#define GVF_IK_SIN_OFF 0
+#endif
+
+/*! Default amplitude for the sin trajectory in meters*/
+#ifndef GVF_IK_SIN_A
+#define GVF_IK_SIN_A 0
+#endif
+
+gvf_ik_s_par gvf_ik_sin_par = {
+ GVF_IK_SIN_KE, GVF_IK_SIN_KN, GVF_IK_SIN_ALPHA, GVF_IK_SIN_W, GVF_IK_SIN_OFF, GVF_IK_SIN_A
+};
+
+// Param array lenght
+static int gvf_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
+
+bool nav_gvf_ik_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 2;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = alpha;
+ gvf_trajectory.p[3] = w;
+ gvf_trajectory.p[4] = off;
+ gvf_trajectory.p[5] = A;
+ gvf_trajectory.p_len= 6 + gvf_p_len_wps;
+ gvf_p_len_wps = 0;
+
+ gvf_sin_info(&e, &grad_line, &Hess_line);
+ gvf_ik_control.ke = gvf_ik_sin_par.ke;
+ gvf_ik_control_2D(1e-2 * gvf_ik_sin_par.ke, gvf_ik_sin_par.kn, e, &grad_line, &Hess_line);
+
+ gvf_ik_control.error = e;
+
+ return true;
+}
+
+bool nav_gvf_ik_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+
+ gvf_trajectory.p[6] = wp1;
+ gvf_trajectory.p[7] = wp2;
+ gvf_p_len_wps = 2;
+
+ float x1 = WaypointX(wp1);
+ float y1 = WaypointY(wp1);
+ float x2 = WaypointX(wp2);
+ float y2 = WaypointY(wp2);
+
+ float zx = x1 - x2;
+ float zy = y1 - y2;
+
+ float alpha = atanf(zy / zx);
+
+ nav_gvf_ik_sin_XY_alpha(x1, y1, alpha, w, off, A);
+
+ return true;
+}
+
+bool nav_gvf_ik_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+ alpha = RadOfDeg(alpha);
+
+ gvf_trajectory.p[6] = wp;
+ gvf_p_len_wps = 1;
+
+ float x = WaypointX(wp);
+ float y = WaypointY(wp);
+
+ nav_gvf_ik_sin_XY_alpha(x, y, alpha, w, off, A);
+
+ return true;
+}
diff --git a/sw/airborne/modules/guidance/gvf_ik/nav/nav_sin.h b/sw/airborne/modules/guidance/gvf_ik/nav/nav_sin.h
new file mode 100644
index 0000000000..8e9737d199
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_ik/nav/nav_sin.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef GVF_IK_SIN_H
+#define GVF_IK_SIN_H
+
+#include "modules/guidance/trajectories/gvf_traj.h"
+
+/** @typedef gvf_s_par
+* @brief Parameters for the GVF line trajectory
+* @param ke Gain defining how agressive is the vector field
+* @param kn Gain for making converge the vehile to the vector field
+* @param alpha Orientation in rads of the sin trajectory
+* @param w Frequency in rads of the sin trajectory
+* @param off Off-set in rads of the sin trajectory
+* @param A Amplitude in meters of the sin trajectory
+*/
+typedef struct {
+ float ke;
+ float kn;
+ float alpha;
+ float w;
+ float off;
+ float A;
+} gvf_ik_s_par;
+
+/** ------------------------------------------------------------------------ **/
+
+extern gvf_ik_s_par gvf_ik_sin_par;
+
+// Sinusoidal
+extern bool nav_gvf_ik_sin_XY_alpha(float x, float y, float alpha, float w, float off, float A);
+extern bool nav_gvf_ik_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A);
+extern bool nav_gvf_ik_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A);
+
+#endif // GVF_IK_SIN_H
diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
index 1f843e6ae1..ce6bde4509 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
+++ b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
@@ -28,12 +28,31 @@
#include // https://eigen.tuxfamily.org/dox/GettingStarted.html
#include "gvf_parametric.h"
-#include "gvf_parametric_low_level_control.h"
-#include "./trajectories/gvf_parametric_3d_ellipse.h"
-#include "./trajectories/gvf_parametric_3d_lissajous.h"
-#include "./trajectories/gvf_parametric_2d_trefoil.h"
-#include "./trajectories/gvf_parametric_2d_bezier_splines.h"
-#include "../gvf_common.h"
+
+/*! Default gain kroll for tuning the "coordinated turn" */
+#ifndef GVF_PARAMETRIC_CONTROL_KROLL
+#define GVF_PARAMETRIC_CONTROL_KROLL 1
+#endif
+
+/*! Default gain kclimb for tuning the climbing setting point */
+#ifndef GVF_PARAMETRIC_CONTROL_KCLIMB
+#define GVF_PARAMETRIC_CONTROL_KCLIMB 1
+#endif
+
+/*! Default scale for the error signals */
+#ifndef GVF_PARAMETRIC_CONTROL_L
+#define GVF_PARAMETRIC_CONTROL_L 0.1
+#endif
+
+/*! Default scale for w */
+#ifndef GVF_PARAMETRIC_CONTROL_BETA
+#define GVF_PARAMETRIC_CONTROL_BETA 0.01
+#endif
+
+/*! Default gain kpsi for tuning the alignment of the vehicle with the vector field */
+#ifndef GVF_PARAMETRIC_CONTROL_KPSI
+#define GVF_PARAMETRIC_CONTROL_KPSI 1
+#endif
#ifdef __cplusplus
extern "C" {
@@ -42,22 +61,13 @@ extern "C" {
#include "autopilot.h"
// Control
-uint32_t gvf_parametric_t0 = 0; // We need it for calculting the time lapse delta_T
-uint32_t gvf_parametric_splines_ctr = 0; // We need it for Bézier curves splines Telemetry
gvf_parametric_con gvf_parametric_control;
+gvf_parametric_tel gvf_parametric_telemetry = {{0},1,0};
-// Trajectory
-gvf_parametric_tra gvf_parametric_trajectory;
+// Time variables to check if GVF is active
+uint32_t gvf_parametric_t0 = 0;
-// Parameters array lenght
-int gvf_parametric_plen = 1;
-int gvf_parametric_plen_wps = 0;
-
-// Error signals array lenght
-int gvf_parametric_elen = 3;
-
-// Bézier
-bezier_t gvf_bezier_2D[GVF_PARAMETRIC_2D_BEZIER_N_SEG];
+/** TELEMETRY -------------------------------------------------------------- **/
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -71,9 +81,12 @@ static void send_gvf_parametric(struct transport_tx *trans, struct link_device *
float wb = gvf_parametric_control.w * gvf_parametric_control.beta;
if (delta_T < 200) {
- gvf_parametric_splines_ctr = (gvf_parametric_splines_ctr + 1) % 3;
- pprz_msg_send_GVF_PARAMETRIC(trans, dev, AC_ID, &traj_type, &gvf_parametric_control.s, &wb, gvf_parametric_plen,
- gvf_parametric_trajectory.p_parametric, gvf_parametric_elen, gvf_parametric_trajectory.phi_errors);
+ gvf_parametric_telemetry.splines_ctr = (gvf_parametric_telemetry.splines_ctr + 1) % 3;
+ pprz_msg_send_GVF_PARAMETRIC(
+ trans, dev, AC_ID,
+ &traj_type, &gvf_parametric_control.s, &wb,
+ gvf_parametric_trajectory.p_len, gvf_parametric_trajectory.p_parametric,
+ gvf_parametric_telemetry.e_len, gvf_parametric_telemetry.phi_errors);
}
}
@@ -85,8 +98,10 @@ static void send_circle_parametric(struct transport_tx *trans, struct link_devic
if (delta_T < 200)
if (gvf_parametric_trajectory.type == ELLIPSE_3D) {
- pprz_msg_send_CIRCLE(trans, dev, AC_ID, &gvf_parametric_trajectory.p_parametric[0],
- &gvf_parametric_trajectory.p_parametric[1], &gvf_parametric_trajectory.p_parametric[2]);
+ pprz_msg_send_CIRCLE(
+ trans, dev, AC_ID,
+ &gvf_parametric_trajectory.p_parametric[0], &gvf_parametric_trajectory.p_parametric[1],
+ &gvf_parametric_trajectory.p_parametric[2]);
}
}
#endif // GVF_OCAML_GCS
@@ -97,13 +112,16 @@ static void send_circle_parametric(struct transport_tx *trans, struct link_devic
}
#endif
+/** ------------------------------------------------------------------------ **/
+
void gvf_parametric_init(void)
{
+ gvf_c_params.k_roll = GVF_PARAMETRIC_CONTROL_KROLL;
+ gvf_c_params.k_climb = GVF_PARAMETRIC_CONTROL_KCLIMB;
+
gvf_parametric_control.w = 0;
gvf_parametric_control.delta_T = 0;
gvf_parametric_control.s = 1;
- gvf_parametric_control.k_roll = GVF_PARAMETRIC_CONTROL_KROLL;
- gvf_parametric_control.k_climb = GVF_PARAMETRIC_CONTROL_KCLIMB;
gvf_parametric_control.k_psi = GVF_PARAMETRIC_CONTROL_KPSI;
gvf_parametric_control.L = GVF_PARAMETRIC_CONTROL_L;
gvf_parametric_control.beta = GVF_PARAMETRIC_CONTROL_BETA;
@@ -114,12 +132,6 @@ void gvf_parametric_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE, send_circle_parametric);
#endif // GVF_OCAML_GCS
#endif // PERIODIC_TELEMETRY
-
-}
-
-void gvf_parametric_set_direction(int8_t s)
-{
- gvf_parametric_control.s = s;
}
void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
@@ -154,9 +166,9 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
float phi1 = L * (x - f1);
float phi2 = L * (y - f2);
- gvf_parametric_trajectory.phi_errors[0] = phi1; // Error signals for the telemetry
- gvf_parametric_trajectory.phi_errors[1] = phi2;
- gvf_parametric_elen = 2;
+ gvf_parametric_telemetry.phi_errors[0] = phi1; // Error signals for the telemetry
+ gvf_parametric_telemetry.phi_errors[1] = phi2;
+ gvf_parametric_telemetry.e_len = 2;
// Chi
X(0) = L * beta * f1d - kx * phi1;
@@ -212,7 +224,7 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
sqrtf(Xt * G * X));
// From gvf_common.h TODO: implement d/dt of kppa and ori_err
- gvf_c_omega.omega = heading_rate;
+ gvf_c_ctrl.omega = heading_rate;
gvf_c_info.kappa = (f1d * f2dd - f1dd * f2d) / powf(f1d * f1d + f2d * f2d, 1.5);
gvf_c_info.ori_err = 1 - (Xh(0) * cosf(course) + Xh(1) * sinf(course));
@@ -220,7 +232,7 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
// the vehicle. So it is not only okei but advisable to update it.
gvf_parametric_control.w += w_dot * gvf_parametric_control.delta_T * 1e-3;
- gvf_parametric_low_level_control_2D(heading_rate);
+ gvf_low_level_control_2D(heading_rate);
}
#ifdef FIXEDWING_FIRMWARE
@@ -256,10 +268,10 @@ void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2,
float phi2 = L * (y - f2);
float phi3 = L * (z - f3);
- gvf_parametric_trajectory.phi_errors[0] = phi1 / L; // Error signals in meters for the telemetry
- gvf_parametric_trajectory.phi_errors[1] = phi2 / L;
- gvf_parametric_trajectory.phi_errors[2] = phi3 / L;
- gvf_parametric_elen = 3;
+ gvf_parametric_telemetry.phi_errors[0] = phi1 / L; // Error signals in meters for the telemetry
+ gvf_parametric_telemetry.phi_errors[1] = phi2 / L;
+ gvf_parametric_telemetry.phi_errors[2] = phi3 / L;
+ gvf_parametric_telemetry.e_len = 3;
// Chi
@@ -328,204 +340,13 @@ void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2,
// the vehicle. So it is not only okei but advisable to update it.
gvf_parametric_control.w += w_dot * gvf_parametric_control.delta_T * 1e-3;
- gvf_parametric_low_level_control_3D(heading_rate, climbing_rate);
+ gvf_low_level_control_3D(heading_rate, climbing_rate);
}
#endif // FIXED_WING FIRMWARE
-/** 2D TRAJECTORIES **/
-// 2D TREFOIL KNOT
-
-bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
+void gvf_parametric_set_direction(int8_t s)
{
- gvf_parametric_trajectory.type = TREFOIL_2D;
- gvf_parametric_trajectory.p_parametric[0] = xo;
- gvf_parametric_trajectory.p_parametric[1] = yo;
- gvf_parametric_trajectory.p_parametric[2] = w1;
- gvf_parametric_trajectory.p_parametric[3] = w2;
- gvf_parametric_trajectory.p_parametric[4] = ratio;
- gvf_parametric_trajectory.p_parametric[5] = r;
- gvf_parametric_trajectory.p_parametric[6] = alpha;
- gvf_parametric_plen = 7 + gvf_parametric_plen_wps;
- gvf_parametric_plen_wps = 0;
-
- float f1, f2, f1d, f2d, f1dd, f2dd;
-
- gvf_parametric_2d_trefoil_info(&f1, &f2, &f1d, &f2d, &f1dd, &f2dd);
- gvf_parametric_control_2D(gvf_parametric_2d_trefoil_par.kx, gvf_parametric_2d_trefoil_par.ky, f1, f2, f1d, f2d, f1dd,
- f2dd);
-
- return true;
+ gvf_parametric_control.s = s;
}
-bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
-{
- gvf_parametric_trajectory.p_parametric[7] = wp;
- gvf_parametric_plen_wps = 1;
- gvf_parametric_2D_trefoil_XY(WaypointX(wp), WaypointY(wp), w1, w2, ratio, r, alpha);
- return true;
-}
-
-// 2D CUBIC BEZIER CURVE
-bool gvf_parametric_2D_bezier_XY(void)
-{
- gvf_parametric_trajectory.type = BEZIER_2D;
- float fx, fy, fxd, fyd, fxdd, fydd;
- gvf_parametric_2d_bezier_splines_info(gvf_bezier_2D, &fx, &fy, &fxd, &fyd, &fxdd, &fydd);
- gvf_parametric_control_2D(gvf_parametric_2d_bezier_par.kx, gvf_parametric_2d_bezier_par.ky, fx, fy, fxd, fyd, fxdd,
- fydd);
- return true;
-}
-
-/* @param first_wp is the first waypoint of the Bézier Spline
- * there should be 3*GVF_PARAMETRIC_2D_BEZIER_N_SEG+1 points
- */
-bool gvf_parametric_2D_bezier_wp(uint8_t first_wp)
-{
- float x[3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1];
- float y[3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1];
- int k;
- for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
- x[k] = WaypointX(first_wp + k);
- y[k] = WaypointY(first_wp + k);
- }
- create_bezier_spline(gvf_bezier_2D, x, y);
-
- /* Send data piecewise. Some radio modules do not allow for a big data frame.*/
-
- // Send x points -> Indicate x with sign (+) in the first parameter
- if (gvf_parametric_splines_ctr == 0) {
- gvf_parametric_trajectory.p_parametric[0] = -GVF_PARAMETRIC_2D_BEZIER_N_SEG; // send x (negative value)
- for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
- gvf_parametric_trajectory.p_parametric[k + 1] = x[k];
- }
- }
- // Send y points -> Indicate y with sign (-) in the first parameter
- else if (gvf_parametric_splines_ctr == 1) {
- gvf_parametric_trajectory.p_parametric[0] = GVF_PARAMETRIC_2D_BEZIER_N_SEG; // send y (positive value)
- for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
- gvf_parametric_trajectory.p_parametric[k + 1] = y[k];
- }
- }
- // send kx, ky, beta and anything else needed..
- else {
- gvf_parametric_trajectory.p_parametric[0] = 0.0;
- gvf_parametric_trajectory.p_parametric[1] = gvf_parametric_2d_bezier_par.kx;
- gvf_parametric_trajectory.p_parametric[2] = gvf_parametric_2d_bezier_par.ky;
- gvf_parametric_trajectory.p_parametric[3] = gvf_parametric_control.beta;
- }
- gvf_parametric_plen = 16;
- gvf_parametric_plen_wps = 1;
-
- // restart the spline
- if (gvf_parametric_control.w >= (float)GVF_PARAMETRIC_2D_BEZIER_N_SEG) {
- gvf_parametric_control.w = 0;
- } else if (gvf_parametric_control.w < 0) {
- gvf_parametric_control.w = 0;
- }
- gvf_parametric_2D_bezier_XY();
- return true;
-}
-
-/** 3D TRAJECTORIES **/
-// 3D ELLIPSE
-#ifdef FIXEDWING_FIRMWARE
-bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
-{
- horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
-
- // Safety first! If the asked altitude is low
- if (zl > zh) {
- zl = zh;
- }
- if (zl < 1 || zh < 1) {
- zl = 10;
- zh = 10;
- }
- if (r < 1) {
- r = 60;
- }
-
- gvf_parametric_trajectory.type = ELLIPSE_3D;
- gvf_parametric_trajectory.p_parametric[0] = xo;
- gvf_parametric_trajectory.p_parametric[1] = yo;
- gvf_parametric_trajectory.p_parametric[2] = r;
- gvf_parametric_trajectory.p_parametric[3] = zl;
- gvf_parametric_trajectory.p_parametric[4] = zh;
- gvf_parametric_trajectory.p_parametric[5] = alpha;
- gvf_parametric_plen = 6 + gvf_parametric_plen_wps;
- gvf_parametric_plen_wps = 0;
-
- float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
-
- gvf_parametric_3d_ellipse_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
- gvf_parametric_control_3D(gvf_parametric_3d_ellipse_par.kx, gvf_parametric_3d_ellipse_par.ky,
- gvf_parametric_3d_ellipse_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
-
- return true;
-}
-
-bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
-{
- gvf_parametric_trajectory.p_parametric[6] = wp;
- gvf_parametric_plen_wps = 1;
-
- gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
- return true;
-}
-
-bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
-{
- float zl = alt_center - delta;
- float zh = alt_center + delta;
-
- gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
- return true;
-}
-
-// 3D Lissajous
-
-bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy,
- float wz, float dx, float dy, float dz, float alpha)
-{
- // Safety first! If the asked altitude is low
- if ((zo - cz) < 1) {
- zo = 10;
- cz = 0;
- }
-
- gvf_parametric_trajectory.type = LISSAJOUS_3D;
- gvf_parametric_trajectory.p_parametric[0] = xo;
- gvf_parametric_trajectory.p_parametric[1] = yo;
- gvf_parametric_trajectory.p_parametric[2] = zo;
- gvf_parametric_trajectory.p_parametric[3] = cx;
- gvf_parametric_trajectory.p_parametric[4] = cy;
- gvf_parametric_trajectory.p_parametric[5] = cz;
- gvf_parametric_trajectory.p_parametric[6] = wx;
- gvf_parametric_trajectory.p_parametric[7] = wy;
- gvf_parametric_trajectory.p_parametric[8] = wz;
- gvf_parametric_trajectory.p_parametric[9] = dx;
- gvf_parametric_trajectory.p_parametric[10] = dy;
- gvf_parametric_trajectory.p_parametric[11] = dz;
- gvf_parametric_trajectory.p_parametric[12] = alpha;
- gvf_parametric_plen = 13 + gvf_parametric_plen_wps;
- gvf_parametric_plen_wps = 0;
-
- float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
-
- gvf_parametric_3d_lissajous_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
- gvf_parametric_control_3D(gvf_parametric_3d_lissajous_par.kx, gvf_parametric_3d_lissajous_par.ky,
- gvf_parametric_3d_lissajous_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
-
- return true;
-}
-
-bool gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy,
- float wz, float dx, float dy, float dz, float alpha)
-{
- gvf_parametric_trajectory.p_parametric[13] = wp;
- gvf_parametric_plen_wps = 1;
-
- gvf_parametric_3D_lissajous_XYZ(waypoints[wp].x, waypoints[wp].y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha);
- return true;
-}
-#endif // FIXEDWING_FIRMWARE
+/** ------------------------------------------------------------------------ **/
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h
index 06fee0d2cb..480bd87fe7 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h
+++ b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h
@@ -27,33 +27,6 @@
#ifndef GVF_PARAMETRIC_H
#define GVF_PARAMETRIC_H
-#define GVF_PARAMETRIC_GRAVITY 9.806
-
-/*! Default gain kroll for tuning the "coordinated turn" */
-#ifndef GVF_PARAMETRIC_CONTROL_KROLL
-#define GVF_PARAMETRIC_CONTROL_KROLL 1
-#endif
-
-/*! Default gain kclimb for tuning the climbing setting point */
-#ifndef GVF_PARAMETRIC_CONTROL_KCLIMB
-#define GVF_PARAMETRIC_CONTROL_KCLIMB 1
-#endif
-
-/*! Default scale for the error signals */
-#ifndef GVF_PARAMETRIC_CONTROL_L
-#define GVF_PARAMETRIC_CONTROL_L 0.1
-#endif
-
-/*! Default scale for w */
-#ifndef GVF_PARAMETRIC_CONTROL_BETA
-#define GVF_PARAMETRIC_CONTROL_BETA 0.01
-#endif
-
-/*! Default gain kpsi for tuning the alignment of the vehicle with the vector field */
-#ifndef GVF_PARAMETRIC_CONTROL_KPSI
-#define GVF_PARAMETRIC_CONTROL_KPSI 1
-#endif
-
/*! Default GCS trajectory painter */
#ifndef GVF_OCAML_GCS
#define GVF_OCAML_GCS true
@@ -63,10 +36,8 @@
extern "C" {
#endif
-#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h"
-#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h"
-#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h"
-#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h"
+#include "modules/guidance/gvf_common.h"
+#include "modules/guidance/trajectories/gvf_param_traj.h"
/** @typedef gvf_parametric_con
* @brief Control parameters for the GVF_PARAMETRIC
@@ -80,34 +51,19 @@ typedef struct {
float w;
float delta_T;
int8_t s;
- float k_roll;
- float k_climb;
float k_psi;
float L;
float beta;
} gvf_parametric_con;
-extern gvf_parametric_con gvf_parametric_control;
-
-// Parameters for the trajectories
-enum trajectories_parametric {
- TREFOIL_2D = 0,
- ELLIPSE_3D = 1,
- LISSAJOUS_3D = 2,
- BEZIER_2D = 3,
- NONE_PARAMETRIC = 255,
-};
-
typedef struct {
- enum trajectories_parametric type;
- float p_parametric[16];
float phi_errors[3];
-} gvf_parametric_tra;
+ int e_len;
+ uint32_t splines_ctr;
+} gvf_parametric_tel;
-extern gvf_parametric_tra gvf_parametric_trajectory;
-
-// Bezier struct
-extern bezier_t gvf_bezier_2D[GVF_PARAMETRIC_2D_BEZIER_N_SEG];
+extern gvf_parametric_con gvf_parametric_control;
+extern gvf_parametric_tel gvf_parametric_telemetry;
// Init function
extern void gvf_parametric_init(void);
@@ -118,25 +74,6 @@ extern void gvf_parametric_control_2D(float, float, float, float, float, float,
extern void gvf_parametric_control_3D(float, float, float, float, float, float, float, float, float,
float, float, float);
-// 2D Trefoil
-extern bool gvf_parametric_2D_trefoil_XY(float, float, float, float, float, float, float);
-extern bool gvf_parametric_2D_trefoil_wp(uint8_t, float, float, float, float, float);
-
-// 2D BEZIER
-extern bool gvf_parametric_2D_bezier_wp(uint8_t);
-extern bool gvf_parametric_2D_bezier_XY(void);
-
-// 3D Ellipse
-extern bool gvf_parametric_3D_ellipse_XYZ(float, float, float, float, float, float);
-extern bool gvf_parametric_3D_ellipse_wp(uint8_t, float, float, float, float);
-extern bool gvf_parametric_3D_ellipse_wp_delta(uint8_t, float, float, float, float);
-
-// 3D Lissajous
-extern bool gvf_parametric_3D_lissajous_XYZ(float, float, float, float, float, float, float, float, float, float, float,
- float, float);
-extern bool gvf_parametric_3D_lissajous_wp_center(uint8_t, float, float, float, float, float, float, float, float,
- float, float, float);
-
#ifdef __cplusplus
}
#endif
diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c
deleted file mode 100644
index a94e23e94f..0000000000
--- a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- * Copyright (C) 2020 Hector Garcia de Marina
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c
- *
- * Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories.
- */
-
-#include "autopilot.h"
-#include "gvf_parametric_low_level_control.h"
-#include "gvf_parametric.h"
-
-#if defined(FIXEDWING_FIRMWARE)
-#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-#include "firmwares/fixedwing/guidance/guidance_v_n.h" // gvf_parametric is only compatible with the new pprz controller!
-#endif
-
-#if defined(FIXEDWING_FIRMWARE)
-#define USED_IN_FIXEDWING_ONLY
-#else
-#define USED_IN_FIXEDWING_ONLY UNUSED
-#endif
-
-void gvf_parametric_low_level_control_2D(float heading_rate USED_IN_FIXEDWING_ONLY )
-{
-#if defined(FIXEDWING_FIRMWARE)
- if (autopilot_get_mode() == AP_MODE_AUTO2) {
- // Lateral XY coordinates
- lateral_mode = LATERAL_MODE_ROLL;
-
- struct FloatEulers *att = stateGetNedToBodyEulers_f();
- float ground_speed = stateGetHorizontalSpeedNorm_f();
-
- h_ctl_roll_setpoint =
- -gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
- BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
- }
-// Allow for rover operation
-#elif defined(ROVER_FIRMWARE)
-#else
-#error gvf_parametric does not support your firmware yet
-#endif
-}
-
-void gvf_parametric_low_level_control_3D(float heading_rate USED_IN_FIXEDWING_ONLY, float climbing_rate USED_IN_FIXEDWING_ONLY)
-{
-#if defined(FIXEDWING_FIRMWARE)
- if (autopilot_get_mode() == AP_MODE_AUTO2) {
- // Vertical Z coordinate
- v_ctl_mode = V_CTL_MODE_AUTO_CLIMB;
- v_ctl_speed_mode = V_CTL_SPEED_THROTTLE;
-
- v_ctl_climb_setpoint = gvf_parametric_control.k_climb * climbing_rate; // Setting point for vertical speed
-
- // Lateral XY coordinates
- lateral_mode = LATERAL_MODE_ROLL;
-
- struct FloatEulers *att = stateGetNedToBodyEulers_f();
- float ground_speed = stateGetHorizontalSpeedNorm_f();
-
- h_ctl_roll_setpoint =
- -gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
- BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
- }
-// Allow for rover operation
-#elif defined(ROVER_FIRMWARE)
-#else
-#error gvf_parametric does not support your firmware yet
-#endif
-}
diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h
deleted file mode 100644
index dbed98ffe1..0000000000
--- a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * Copyright (C) 2020 Hector Garcia de Marina
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h
- *
- * Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories.
- */
-
-#ifndef GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H
-#define GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// Low level control functions
-extern void gvf_parametric_low_level_control_2D(float);
-extern void gvf_parametric_low_level_control_3D(float, float);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H
-
diff --git a/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_bezier_splines.c b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_bezier_splines.c
new file mode 100644
index 0000000000..dfd9a3cf28
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_bezier_splines.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2023 Alfredo Gonzalez Calvin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+#include "nav_parametric_2d_bezier_splines.h"
+#include "modules/guidance/gvf_parametric/gvf_parametric.h"
+
+#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX
+#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX 2.0
+#endif
+
+#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY
+#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY 2.0
+#endif
+
+gvf_par_2d_bezier_par gvf_parametric_2d_bezier_par = {
+ GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX, GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY
+};
+
+bezier_t gvf_bezier_2D[GVF_PARAMETRIC_2D_BEZIER_N_SEG];
+
+/** ------------------------------------------------------------------------ **/
+
+// Bezier is just an array
+static void create_bezier_spline(bezier_t *bezier, float *px, float *py)
+{
+ int k, j;
+ j = 0;
+ for (k = 0; k < GVF_PARAMETRIC_2D_BEZIER_N_SEG; k++) {
+ bezier[k].p0[0] = px[j];
+ bezier[k].p0[1] = py[j];
+ bezier[k].p1[0] = px[j + 1];
+ bezier[k].p1[1] = py[j + 1];
+ bezier[k].p2[0] = px[j + 2];
+ bezier[k].p2[1] = py[j + 2];
+ bezier[k].p3[0] = px[j + 3];
+ bezier[k].p3[1] = py[j + 3];
+
+ // This allows for C^0 continuity (last point is init point)
+ j += 3;
+ }
+}
+
+/** ------------------------------------------------------------------------ **/
+
+// 2D CUBIC BEZIER CURVE
+
+bool nav_gvf_parametric_2D_bezier_run(void)
+{
+ gvf_parametric_trajectory.type = BEZIER_2D;
+ float fx, fy, fxd, fyd, fxdd, fydd;
+ gvf_parametric_2d_bezier_splines_info(gvf_bezier_2D, &fx, &fy, &fxd, &fyd, &fxdd, &fydd, gvf_parametric_control.w);
+ gvf_parametric_control_2D(gvf_parametric_2d_bezier_par.kx, gvf_parametric_2d_bezier_par.ky, fx, fy, fxd, fyd, fxdd,
+ fydd);
+ return true;
+}
+
+/* @param first_wp is the first waypoint of the Bézier Spline
+ * there should be 3*GVF_PARAMETRIC_2D_BEZIER_N_SEG+1 points
+ */
+bool nav_gvf_parametric_2D_bezier_wp(uint8_t first_wp)
+{
+ float x[3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1];
+ float y[3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1];
+ int k;
+ for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
+ x[k] = WaypointX(first_wp + k);
+ y[k] = WaypointY(first_wp + k);
+ }
+ create_bezier_spline(gvf_bezier_2D, x, y);
+
+ /* Send data piecewise. Some radio modules do not allow for a big data frame.*/
+
+ // Send x points -> Indicate x with sign (+) in the first parameter
+ if (gvf_parametric_telemetry.splines_ctr == 0) {
+ gvf_parametric_trajectory.p_parametric[0] = -GVF_PARAMETRIC_2D_BEZIER_N_SEG; // send x (negative value)
+ for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
+ gvf_parametric_trajectory.p_parametric[k + 1] = x[k];
+ }
+ }
+ // Send y points -> Indicate y with sign (-) in the first parameter
+ else if (gvf_parametric_telemetry.splines_ctr == 1) {
+ gvf_parametric_trajectory.p_parametric[0] = GVF_PARAMETRIC_2D_BEZIER_N_SEG; // send y (positive value)
+ for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
+ gvf_parametric_trajectory.p_parametric[k + 1] = y[k];
+ }
+ }
+ // Send kx, ky, beta and anything else needed..
+ else {
+ gvf_parametric_trajectory.p_parametric[0] = 0.0;
+ gvf_parametric_trajectory.p_parametric[1] = gvf_parametric_2d_bezier_par.kx;
+ gvf_parametric_trajectory.p_parametric[2] = gvf_parametric_2d_bezier_par.ky;
+ gvf_parametric_trajectory.p_parametric[3] = gvf_parametric_control.beta;
+ }
+
+ gvf_parametric_trajectory.p_len = 16;
+
+ // Restart the spline
+ if (gvf_parametric_control.w >= (float)GVF_PARAMETRIC_2D_BEZIER_N_SEG) {
+ gvf_parametric_control.w = 0;
+ } else if (gvf_parametric_control.w < 0) {
+ gvf_parametric_control.w = 0;
+ }
+ nav_gvf_parametric_2D_bezier_run();
+ return true;
+}
+
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_bezier_splines.h
similarity index 69%
rename from sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h
rename to sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_bezier_splines.h
index 80d613fc11..aa7fdc2782 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_bezier_splines.h
@@ -21,45 +21,25 @@
#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_H
#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_H
-
-// Define only one segment by default
-#ifndef GVF_PARAMETRIC_2D_BEZIER_N_SEG
-#define GVF_PARAMETRIC_2D_BEZIER_N_SEG 1
-#endif
-
-
#ifdef __cplusplus
extern "C" {
#endif
+#include "modules/guidance/trajectories/gvf_param_traj.h"
typedef struct {
float kx;
float ky;
} gvf_par_2d_bezier_par;
-
-// Cubic bezier
-typedef struct {
- float p0[2];
- float p1[2];
- float p2[2];
- float p3[2];
-} bezier_t;
-
-
extern gvf_par_2d_bezier_par gvf_parametric_2d_bezier_par;
-extern void create_bezier_spline(bezier_t *bezier, float *px, float *py);
-extern void gvf_parametric_2d_bezier_splines_info(bezier_t *bezier, float *f1, float *f2, float *f1d, float *f2d,
- float *f1dd, float *f2dd);
+// 2D BEZIER
+extern bool nav_gvf_parametric_2D_bezier_run(void);
+extern bool nav_gvf_parametric_2D_bezier_wp(uint8_t first_wp);
#ifdef __cplusplus
}
#endif
-
-
-
-
#endif // bezier splines
diff --git a/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_trefoil.c b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_trefoil.c
new file mode 100644
index 0000000000..fa61fd79b8
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_trefoil.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2020 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * Guiding vector field algorithm for 2D and 3D complex trajectories.
+ *
+ * 2D trefoil know
+ */
+
+#include "nav_parametric_2d_trefoil.h"
+#include "modules/guidance/gvf_parametric/gvf_parametric.h"
+
+/*! Default gain kx for the 2d trefoil knot trajectory */
+#ifndef GVF_PARAMETRIC_2D_TREFOIL_KX
+#define GVF_PARAMETRIC_2D_TREFOIL_KX 0.001
+#endif
+
+/*! Default gain ky for the 2d trefoil knot trajectory */
+#ifndef GVF_PARAMETRIC_2D_TREFOIL_KY
+#define GVF_PARAMETRIC_2D_TREFOIL_KY 0.001
+#endif
+
+/*! Default 1st frequency for the 2d trefoil trajectory*/
+#ifndef GVF_PARAMETRIC_2D_TREFOIL_W1
+#define GVF_PARAMETRIC_2D_TREFOIL_W1 0.02
+#endif
+
+/*! Default 2nd frequency for the 2d trefoil trajectory*/
+#ifndef GVF_PARAMETRIC_2D_TREFOIL_W2
+#define GVF_PARAMETRIC_2D_TREFOIL_W2 0.03
+#endif
+
+/*! Default ratio for the 2d trefoil trajectory*/
+#ifndef GVF_PARAMETRIC_2D_TREFOIL_RATIO
+#define GVF_PARAMETRIC_2D_TREFOIL_RATIO 160
+#endif
+
+/*! Default radius of the circles for the 2d trefoil trajectory*/
+#ifndef GVF_PARAMETRIC_2D_TREFOIL_R
+#define GVF_PARAMETRIC_2D_TREFOIL_R 80
+#endif
+
+/*! Default orientation for the 2d trefoil trajectory*/
+#ifndef GVF_PARAMETRIC_2D_TREFOIL_ALPHA
+#define GVF_PARAMETRIC_2D_TREFOIL_ALPHA 0
+#endif
+
+gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par = {
+ GVF_PARAMETRIC_2D_TREFOIL_KX, GVF_PARAMETRIC_2D_TREFOIL_KY,
+ GVF_PARAMETRIC_2D_TREFOIL_W1, GVF_PARAMETRIC_2D_TREFOIL_W2,
+ GVF_PARAMETRIC_2D_TREFOIL_RATIO, GVF_PARAMETRIC_2D_TREFOIL_R,
+ GVF_PARAMETRIC_2D_TREFOIL_ALPHA
+};
+
+static int gvf_parametric_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+// 2D TREFOIL KNOT
+
+bool nav_gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
+{
+ gvf_parametric_trajectory.type = TREFOIL_2D;
+ gvf_parametric_trajectory.p_parametric[0] = xo;
+ gvf_parametric_trajectory.p_parametric[1] = yo;
+ gvf_parametric_trajectory.p_parametric[2] = w1;
+ gvf_parametric_trajectory.p_parametric[3] = w2;
+ gvf_parametric_trajectory.p_parametric[4] = ratio;
+ gvf_parametric_trajectory.p_parametric[5] = r;
+ gvf_parametric_trajectory.p_parametric[6] = alpha;
+ gvf_parametric_trajectory.p_len = 7 + gvf_parametric_p_len_wps;
+ gvf_parametric_p_len_wps = 0;
+
+ float f1, f2, f1d, f2d, f1dd, f2dd;
+ float wb = gvf_parametric_control.w * gvf_parametric_control.beta * gvf_parametric_control.s;
+
+ gvf_parametric_2d_trefoil_info(&f1, &f2, &f1d, &f2d, &f1dd, &f2dd, wb);
+ gvf_parametric_control_2D(gvf_parametric_2d_trefoil_par.kx, gvf_parametric_2d_trefoil_par.ky, f1, f2, f1d, f2d, f1dd,
+ f2dd);
+
+ return true;
+}
+
+bool nav_gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
+{
+ gvf_parametric_trajectory.p_parametric[7] = wp;
+ gvf_parametric_p_len_wps = 1;
+ nav_gvf_parametric_2D_trefoil_XY(WaypointX(wp), WaypointY(wp), w1, w2, ratio, r, alpha);
+ return true;
+}
+
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_trefoil.h
similarity index 84%
rename from sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h
rename to sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_trefoil.h
index 227576f307..fdf3e271ab 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_2d_trefoil.h
@@ -19,8 +19,6 @@
*/
/**
- * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h
- *
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 2D trefoil knot
@@ -33,6 +31,8 @@
extern "C" {
#endif
+#include "modules/guidance/trajectories/gvf_param_traj.h"
+
/** @typedef gvf_2d_tre_par
* @brief Parameters for the GVF parametric 2D trefoil knot
* @param kx Gain defining how agressive is the vector field in x coordinate
@@ -55,7 +55,9 @@ typedef struct {
extern gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par;
-extern void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd);
+// 2D Trefoil
+extern bool nav_gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha);
+extern bool nav_gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha);
#ifdef __cplusplus
}
diff --git a/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_ellipse.c b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_ellipse.c
new file mode 100644
index 0000000000..8150b4772e
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_ellipse.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2020 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * Guiding vector field algorithm for 2D and 3D complex trajectories.
+ *
+ * 3D ellipse (intersection between a cylinder and a tilted plane)
+ */
+
+#include "nav_parametric_3d_ellipse.h"
+#include "modules/guidance/gvf_parametric/gvf_parametric.h"
+
+/*! Default gain kx for the 3d ellipse trajectory */
+#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KX
+#define GVF_PARAMETRIC_3D_ELLIPSE_KX 0.001
+#endif
+
+/*! Default gain ky for the 3d ellipse trajectory */
+#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KY
+#define GVF_PARAMETRIC_3D_ELLIPSE_KY 0.001
+#endif
+
+/*! Default gain kz for the 3d ellipse trajectory */
+#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KZ
+#define GVF_PARAMETRIC_3D_ELLIPSE_KZ 0.001
+#endif
+
+/*! Default radius of the cylinder */
+#ifndef GVF_PARAMETRIC_3D_ELLIPSE_R
+#define GVF_PARAMETRIC_3D_ELLIPSE_R 80
+#endif
+
+/*! Default highest point for the ellipse trajectory */
+#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZL
+#define GVF_PARAMETRIC_3D_ELLIPSE_ZL 40
+#endif
+
+/*! Default highest point for the ellipse trajectory */
+#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZH
+#define GVF_PARAMETRIC_3D_ELLIPSE_ZH 40
+#endif
+
+/*! Default orientation in degrees for the lowest point of the ellipse */
+#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ALPHA
+#define GVF_PARAMETRIC_3D_ELLIPSE_ALPHA 0
+#endif
+
+gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par = {
+ GVF_PARAMETRIC_3D_ELLIPSE_KX, GVF_PARAMETRIC_3D_ELLIPSE_KY, GVF_PARAMETRIC_3D_ELLIPSE_KZ,
+ GVF_PARAMETRIC_3D_ELLIPSE_R, GVF_PARAMETRIC_3D_ELLIPSE_ZL, GVF_PARAMETRIC_3D_ELLIPSE_ZH,
+ GVF_PARAMETRIC_3D_ELLIPSE_ALPHA
+};
+
+#ifdef FIXEDWING_FIRMWARE
+
+static int gvf_parametric_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+// 3D ELLIPSE
+
+bool nav_gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
+{
+ horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
+
+ // Safety first! If the asked altitude is low
+ if (zl > zh) {
+ zl = zh;
+ }
+ if (zl < 1 || zh < 1) {
+ zl = 10;
+ zh = 10;
+ }
+ if (r < 1) {
+ r = 60;
+ }
+
+ gvf_parametric_trajectory.type = ELLIPSE_3D;
+ gvf_parametric_trajectory.p_parametric[0] = xo;
+ gvf_parametric_trajectory.p_parametric[1] = yo;
+ gvf_parametric_trajectory.p_parametric[2] = r;
+ gvf_parametric_trajectory.p_parametric[3] = zl;
+ gvf_parametric_trajectory.p_parametric[4] = zh;
+ gvf_parametric_trajectory.p_parametric[5] = alpha;
+ gvf_parametric_trajectory.p_len = 6 + gvf_parametric_p_len_wps;
+ gvf_parametric_p_len_wps = 0;
+
+ float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
+ float wb = gvf_parametric_control.w * gvf_parametric_control.beta * gvf_parametric_control.s;
+
+ gvf_parametric_3d_ellipse_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd, wb);
+ gvf_parametric_control_3D(gvf_parametric_3d_ellipse_par.kx, gvf_parametric_3d_ellipse_par.ky,
+ gvf_parametric_3d_ellipse_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
+
+ return true;
+}
+
+bool nav_gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
+{
+ gvf_parametric_trajectory.p_parametric[6] = wp;
+ gvf_parametric_p_len_wps = 1;
+
+ nav_gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
+ return true;
+}
+
+bool nav_gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
+{
+ float zl = alt_center - delta;
+ float zh = alt_center + delta;
+
+ nav_gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
+ return true;
+}
+
+#endif
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_ellipse.h
similarity index 81%
rename from sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h
rename to sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_ellipse.h
index c7edce3f85..0866f7e525 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_ellipse.h
@@ -19,8 +19,6 @@
*/
/**
- * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h
- *
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 3D ellipse (intersection between a cylinder and a tilted plane)
@@ -33,6 +31,8 @@
extern "C" {
#endif
+#include "modules/guidance/trajectories/gvf_param_traj.h"
+
/** @typedef gvf_3d_ell_par
* @brief Parameters for the GVF parametric 3D ellipse
* @param kx Gain defining how agressive is the vector field in x coordinate
@@ -55,8 +55,10 @@ typedef struct {
extern gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par;
-extern void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
- float *f1dd, float *f2dd, float *f3dd);
+// 3D Ellipse
+extern bool nav_gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha);
+extern bool nav_gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha);
+extern bool nav_gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha);
#ifdef __cplusplus
}
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_lissajous.c
similarity index 51%
rename from sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c
rename to sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_lissajous.c
index e8c463b2ea..5028ddb257 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_lissajous.c
@@ -19,16 +19,13 @@
*/
/**
- * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c
- *
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 3D lissajous
*/
-#include "modules/nav/common_nav.h"
+#include "nav_parametric_3d_lissajous.h"
#include "modules/guidance/gvf_parametric/gvf_parametric.h"
-#include "gvf_parametric_3d_lissajous.h"
/*! Default gain kx for the 3d lissajous trajectory */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KX
@@ -95,49 +92,66 @@
#define GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA 0
#endif
-gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par = {GVF_PARAMETRIC_3D_LISSAJOUS_KX, GVF_PARAMETRIC_3D_LISSAJOUS_KY, GVF_PARAMETRIC_3D_LISSAJOUS_KZ, GVF_PARAMETRIC_3D_LISSAJOUS_CX, GVF_PARAMETRIC_3D_LISSAJOUS_CY, GVF_PARAMETRIC_3D_LISSAJOUS_CZ, GVF_PARAMETRIC_3D_LISSAJOUS_WX, GVF_PARAMETRIC_3D_LISSAJOUS_WY, GVF_PARAMETRIC_3D_LISSAJOUS_WZ, GVF_PARAMETRIC_3D_LISSAJOUS_DX, GVF_PARAMETRIC_3D_LISSAJOUS_DY, GVF_PARAMETRIC_3D_LISSAJOUS_DZ, GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA};
+gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par = {
+ GVF_PARAMETRIC_3D_LISSAJOUS_KX, GVF_PARAMETRIC_3D_LISSAJOUS_KY, GVF_PARAMETRIC_3D_LISSAJOUS_KZ,
+ GVF_PARAMETRIC_3D_LISSAJOUS_CX, GVF_PARAMETRIC_3D_LISSAJOUS_CY, GVF_PARAMETRIC_3D_LISSAJOUS_CZ,
+ GVF_PARAMETRIC_3D_LISSAJOUS_WX, GVF_PARAMETRIC_3D_LISSAJOUS_WY, GVF_PARAMETRIC_3D_LISSAJOUS_WZ,
+ GVF_PARAMETRIC_3D_LISSAJOUS_DX, GVF_PARAMETRIC_3D_LISSAJOUS_DY, GVF_PARAMETRIC_3D_LISSAJOUS_DZ,
+ GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA
+};
-void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
- float *f1dd, float *f2dd, float *f3dd)
+#ifdef FIXEDWING_FIRMWARE
+
+static int gvf_parametric_p_len_wps = 0;
+
+/** ------------------------------------------------------------------------ **/
+
+// 3D Lissajous
+
+bool nav_gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy,
+ float wz, float dx, float dy, float dz, float alpha)
{
- float xo = gvf_parametric_trajectory.p_parametric[0];
- float yo = gvf_parametric_trajectory.p_parametric[1];
- float zo = gvf_parametric_trajectory.p_parametric[2];
- float cx = gvf_parametric_trajectory.p_parametric[3];
- float cy = gvf_parametric_trajectory.p_parametric[4];
- float cz = gvf_parametric_trajectory.p_parametric[5];
- float wx = gvf_parametric_trajectory.p_parametric[6];
- float wy = gvf_parametric_trajectory.p_parametric[7];
- float wz = gvf_parametric_trajectory.p_parametric[8];
- float deltax_rad = gvf_parametric_trajectory.p_parametric[9]*M_PI/180;
- float deltay_rad = gvf_parametric_trajectory.p_parametric[10]*M_PI/180;
- float deltaz_rad = gvf_parametric_trajectory.p_parametric[11]*M_PI/180;
- float alpha_rad = gvf_parametric_trajectory.p_parametric[12]*M_PI/180;
+ // Safety first! If the asked altitude is low
+ if ((zo - cz) < 1) {
+ zo = 10;
+ cz = 0;
+ }
- float w = gvf_parametric_control.w;
- float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s;
+ gvf_parametric_trajectory.type = LISSAJOUS_3D;
+ gvf_parametric_trajectory.p_parametric[0] = xo;
+ gvf_parametric_trajectory.p_parametric[1] = yo;
+ gvf_parametric_trajectory.p_parametric[2] = zo;
+ gvf_parametric_trajectory.p_parametric[3] = cx;
+ gvf_parametric_trajectory.p_parametric[4] = cy;
+ gvf_parametric_trajectory.p_parametric[5] = cz;
+ gvf_parametric_trajectory.p_parametric[6] = wx;
+ gvf_parametric_trajectory.p_parametric[7] = wy;
+ gvf_parametric_trajectory.p_parametric[8] = wz;
+ gvf_parametric_trajectory.p_parametric[9] = dx;
+ gvf_parametric_trajectory.p_parametric[10] = dy;
+ gvf_parametric_trajectory.p_parametric[11] = dz;
+ gvf_parametric_trajectory.p_parametric[12] = alpha;
+ gvf_parametric_trajectory.p_len = 13 + gvf_parametric_p_len_wps;
+ gvf_parametric_p_len_wps = 0;
- // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
+ float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
+ float wb = gvf_parametric_control.w * gvf_parametric_control.beta * gvf_parametric_control.s;
- float nrf1 = cx*cosf(wx*wb + deltax_rad);
- float nrf2 = cy*cosf(wy*wb + deltay_rad);
+ gvf_parametric_3d_lissajous_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd, wb);
+ gvf_parametric_control_3D(gvf_parametric_3d_lissajous_par.kx, gvf_parametric_3d_lissajous_par.ky,
+ gvf_parametric_3d_lissajous_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
- *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
- *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
- *f3 = cz*cosf(wz*wb + deltaz_rad) + zo;
-
- float nrf1d = -wx*cx*sinf(wx*wb + deltax_rad);
- float nrf2d = -wy*cy*sinf(wy*wb + deltay_rad);
-
- *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
- *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
- *f3d = -wz*cz*sinf(wz*wb + deltaz_rad);
-
- float nrf1dd = -wx*wx*cx*cosf(wx*wb + deltax_rad);
- float nrf2dd = -wy*wy*cy*cosf(wy*wb + deltay_rad);
-
- *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
- *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
- *f3dd = -wz*wz*cz*cosf(wz*wb + deltaz_rad);
+ return true;
}
+bool nav_gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy,
+ float wz, float dx, float dy, float dz, float alpha)
+{
+ gvf_parametric_trajectory.p_parametric[13] = wp;
+ gvf_parametric_p_len_wps = 1;
+
+ nav_gvf_parametric_3D_lissajous_XYZ(waypoints[wp].x, waypoints[wp].y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha);
+ return true;
+}
+
+#endif
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_lissajous.h
similarity index 83%
rename from sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h
rename to sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_lissajous.h
index eaaf337633..d607876ae9 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h
+++ b/sw/airborne/modules/guidance/gvf_parametric/nav/nav_parametric_3d_lissajous.h
@@ -19,8 +19,6 @@
*/
/**
- * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h
- *
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 3D lissajous figures
@@ -33,6 +31,8 @@
extern "C" {
#endif
+#include "modules/guidance/trajectories/gvf_param_traj.h"
+
/** @typedef gvf_3d_lis_par
* @brief Parameters for the GVF parametric 3D lissajous
* @param kx Gain defining how agressive is the vector field in x coordinate
@@ -67,8 +67,11 @@ typedef struct {
extern gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par;
-extern void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
- float *f1dd, float *f2dd, float *f3dd);
+// 3D Lissajous
+extern bool nav_gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy,
+ float wz, float dx, float dy, float dz, float alpha);
+extern bool nav_gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy,
+ float wz, float dx, float dy, float dz, float alpha);
#ifdef __cplusplus
}
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.c b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.c
deleted file mode 100644
index 4d2df4bd32..0000000000
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.c
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * Copyright (C) 2023 Alfredo Gonzalez Calvin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-#include "modules/nav/common_nav.h"
-#include "modules/guidance/gvf_parametric/gvf_parametric.h"
-#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h"
-
-#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX
-#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX 2.0
-#endif
-
-#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY
-#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY 2.0
-#endif
-
-gvf_par_2d_bezier_par gvf_parametric_2d_bezier_par = {GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX,
- GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY
- };
-
-// Bezier is just an array
-void create_bezier_spline(bezier_t *bezier, float *px, float *py)
-{
-
- int k, j;
- j = 0;
- for (k = 0; k < GVF_PARAMETRIC_2D_BEZIER_N_SEG; k++) {
- bezier[k].p0[0] = px[j];
- bezier[k].p0[1] = py[j];
- bezier[k].p1[0] = px[j + 1];
- bezier[k].p1[1] = py[j + 1];
- bezier[k].p2[0] = px[j + 2];
- bezier[k].p2[1] = py[j + 2];
- bezier[k].p3[0] = px[j + 3];
- bezier[k].p3[1] = py[j + 3];
-
- // This allows for C^0 continuity (last point is init point)
- j += 3;
- }
-}
-
-void gvf_parametric_2d_bezier_splines_info(bezier_t *bezier, float *f1, float *f2, float *f1d, float *f2d, float *f1dd,
- float *f2dd)
-{
- // How can we select in which bezier curve are we? Check w. spline zero: 0 <= t <= 1, spline ones: 1 <= t <= 2;
- float t = gvf_parametric_control.w;
- int n_seg = floorl(t);
- float tt = t - n_seg;
- if (n_seg < 0) {
- n_seg = 0; // w could be < 0 in that case go to first point of first segment
- tt = 0;
- }
- // Evalute the corresponding bezier curve
- float p0x = bezier[n_seg].p0[0]; float p0y = bezier[n_seg].p0[1];
- float p1x = bezier[n_seg].p1[0]; float p1y = bezier[n_seg].p1[1];
- float p2x = bezier[n_seg].p2[0]; float p2y = bezier[n_seg].p2[1];
- float p3x = bezier[n_seg].p3[0]; float p3y = bezier[n_seg].p3[1];
-
- // Bézier curves
-
- // Curve (x,y)
- *f1 = (1 - tt) * (1 - tt) * (1 - tt) * p0x + 3 * (1 - tt) * (1 - tt) * tt * p1x + 3 *
- (1 - tt) * tt * tt * p2x + tt * tt * tt * p3x;
- *f2 = (1 - tt) * (1 - tt) * (1 - tt) * p0y + 3 * (1 - tt) * (1 - tt) * tt * p1y + 3 *
- (1 - tt) * tt * tt * p2y + tt * tt * tt * p3y;
-
- // First derivative
- *f1d = 3 * (1 - tt) * (1 - tt) * (p1x - p0x) + 6 * (1 - tt) * tt * (p2x - p1x) + 3 * tt * tt * (p3x - p2x);
- *f2d = 3 * (1 - tt) * (1 - tt) * (p1y - p0y) + 6 * (1 - tt) * tt * (p2y - p1y) + 3 * tt * tt * (p3y - p2y);
-
- // Second derivative
- *f1dd = 6 * (1 - tt) * (p2x - 2 * p1x + p0x) + 6 * tt * (p3x - 2 * p2x + p1x);
- *f2dd = 6 * (1 - tt) * (p2y - 2 * p1y + p0y) + 6 * tt * (p3y - 2 * p2y + p1y);
-
-
-}
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c
deleted file mode 100644
index a76b3f95cf..0000000000
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c
+++ /dev/null
@@ -1,102 +0,0 @@
-/*
- * Copyright (C) 2020 Hector Garcia de Marina
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c
- *
- * Guiding vector field algorithm for 2D and 3D complex trajectories.
- *
- * 2D trefoil know
- */
-
-#include "modules/nav/common_nav.h"
-#include "modules/guidance/gvf_parametric/gvf_parametric.h"
-#include "gvf_parametric_2d_trefoil.h"
-
-/*! Default gain kx for the 2d trefoil knot trajectory */
-#ifndef GVF_PARAMETRIC_2D_TREFOIL_KX
-#define GVF_PARAMETRIC_2D_TREFOIL_KX 0.001
-#endif
-
-/*! Default gain ky for the 2d trefoil knot trajectory */
-#ifndef GVF_PARAMETRIC_2D_TREFOIL_KY
-#define GVF_PARAMETRIC_2D_TREFOIL_KY 0.001
-#endif
-
-/*! Default 1st frequency for the 2d trefoil trajectory*/
-#ifndef GVF_PARAMETRIC_2D_TREFOIL_W1
-#define GVF_PARAMETRIC_2D_TREFOIL_W1 0.02
-#endif
-
-/*! Default 2nd frequency for the 2d trefoil trajectory*/
-#ifndef GVF_PARAMETRIC_2D_TREFOIL_W2
-#define GVF_PARAMETRIC_2D_TREFOIL_W2 0.03
-#endif
-
-/*! Default ratio for the 2d trefoil trajectory*/
-#ifndef GVF_PARAMETRIC_2D_TREFOIL_RATIO
-#define GVF_PARAMETRIC_2D_TREFOIL_RATIO 160
-#endif
-
-/*! Default radius of the circles for the 2d trefoil trajectory*/
-#ifndef GVF_PARAMETRIC_2D_TREFOIL_R
-#define GVF_PARAMETRIC_2D_TREFOIL_R 80
-#endif
-
-/*! Default orientation for the 2d trefoil trajectory*/
-#ifndef GVF_PARAMETRIC_2D_TREFOIL_ALPHA
-#define GVF_PARAMETRIC_2D_TREFOIL_ALPHA 0
-#endif
-
-gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par = {GVF_PARAMETRIC_2D_TREFOIL_KX, GVF_PARAMETRIC_2D_TREFOIL_KY, GVF_PARAMETRIC_2D_TREFOIL_W1, GVF_PARAMETRIC_2D_TREFOIL_W2, GVF_PARAMETRIC_2D_TREFOIL_RATIO, GVF_PARAMETRIC_2D_TREFOIL_R, GVF_PARAMETRIC_2D_TREFOIL_ALPHA};
-
-void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd)
-{
- float xo = gvf_parametric_trajectory.p_parametric[0];
- float yo = gvf_parametric_trajectory.p_parametric[1];
- float w1 = gvf_parametric_trajectory.p_parametric[2];
- float w2 = gvf_parametric_trajectory.p_parametric[3];
- float ratio = gvf_parametric_trajectory.p_parametric[4];
- float r = gvf_parametric_trajectory.p_parametric[5];
- float alpha_rad = gvf_parametric_trajectory.p_parametric[6]*M_PI/180;
-
- float w = gvf_parametric_control.w;
- float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s;
-
- // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
- float nrf1 = cosf(wb*w1)*(r*cosf(wb*w2) + ratio);
- float nrf2 = sinf(wb*w1)*(r*cosf(wb*w2) + ratio);
-
- float nrf1d = -w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - cosf(wb*w1)*r*w2*sinf(wb*w2);
- float nrf2d = w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) - sinf(wb*w1)*r*w2*sinf(wb*w2);
-
- float nrf1dd = -w1*w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) - cosf(wb*w1)*r*w2*w2*cosf(wb*w2);
- float nrf2dd = -w1*w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - sinf(wb*w1)*r*w2*w2*cosf(wb*w2);
-
- *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
- *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
-
- *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
- *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
-
- *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
- *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
-}
-
diff --git a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c b/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c
deleted file mode 100644
index 90a5c62ebf..0000000000
--- a/sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * Copyright (C) 2020 Hector Garcia de Marina
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c
- *
- * Guiding vector field algorithm for 2D and 3D complex trajectories.
- *
- * 3D ellipse (intersection between a cylinder and a tilted plane)
- */
-
-#include "modules/nav/common_nav.h"
-#include "modules/guidance/gvf_parametric/gvf_parametric.h"
-#include "gvf_parametric_3d_ellipse.h"
-
-/*! Default gain kx for the 3d ellipse trajectory */
-#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KX
-#define GVF_PARAMETRIC_3D_ELLIPSE_KX 0.001
-#endif
-
-/*! Default gain ky for the 3d ellipse trajectory */
-#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KY
-#define GVF_PARAMETRIC_3D_ELLIPSE_KY 0.001
-#endif
-
-/*! Default gain kz for the 3d ellipse trajectory */
-#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KZ
-#define GVF_PARAMETRIC_3D_ELLIPSE_KZ 0.001
-#endif
-
-/*! Default radius of the cylinder */
-#ifndef GVF_PARAMETRIC_3D_ELLIPSE_R
-#define GVF_PARAMETRIC_3D_ELLIPSE_R 80
-#endif
-
-/*! Default highest point for the ellipse trajectory */
-#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZL
-#define GVF_PARAMETRIC_3D_ELLIPSE_ZL 40
-#endif
-
-/*! Default highest point for the ellipse trajectory */
-#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZH
-#define GVF_PARAMETRIC_3D_ELLIPSE_ZH 40
-#endif
-
-/*! Default orientation in degrees for the lowest point of the ellipse */
-#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ALPHA
-#define GVF_PARAMETRIC_3D_ELLIPSE_ALPHA 0
-#endif
-
-gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par = {GVF_PARAMETRIC_3D_ELLIPSE_KX,
- GVF_PARAMETRIC_3D_ELLIPSE_KY, GVF_PARAMETRIC_3D_ELLIPSE_KZ, GVF_PARAMETRIC_3D_ELLIPSE_R, GVF_PARAMETRIC_3D_ELLIPSE_ZL, GVF_PARAMETRIC_3D_ELLIPSE_ZH, GVF_PARAMETRIC_3D_ELLIPSE_ALPHA
- };
-
-void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
- float *f1dd, float *f2dd, float *f3dd)
-{
- float xo = gvf_parametric_trajectory.p_parametric[0];
- float yo = gvf_parametric_trajectory.p_parametric[1];
- float r = gvf_parametric_trajectory.p_parametric[2];
- float zl = gvf_parametric_trajectory.p_parametric[3];
- float zh = gvf_parametric_trajectory.p_parametric[4];
- float alpha_rad = gvf_parametric_trajectory.p_parametric[5]*M_PI/180;
-
- float w = gvf_parametric_control.w;
- float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s;
-
- // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
- *f1 = r * cosf(wb) + xo;
- *f2 = r * sinf(wb) + yo;
- *f3 = 0.5 * (zh + zl + (zl - zh) * sinf(alpha_rad - wb));
-
- *f1d = -r * sinf(wb);
- *f2d = r * cosf(wb);
- *f3d = -0.5 * (zl - zh) * cosf(alpha_rad - wb);
-
- *f1dd = -r * cosf(wb);
- *f2dd = -r * sinf(wb);
- *f3dd = -0.5 * (zl - zh) * sinf(alpha_rad - wb);
-}
-
diff --git a/sw/airborne/modules/guidance/trajectories/gvf_param_traj.c b/sw/airborne/modules/guidance/trajectories/gvf_param_traj.c
new file mode 100644
index 0000000000..c5a664ebaa
--- /dev/null
+++ b/sw/airborne/modules/guidance/trajectories/gvf_param_traj.c
@@ -0,0 +1,162 @@
+/*
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "gvf_param_traj.h"
+
+// Trajectory
+gvf_parametric_tra gvf_parametric_trajectory = {NONE_PARAMETRIC, {0}, 1};
+
+/** ------------------------------------------------------------------------ **/
+
+void gvf_parametric_2d_trefoil_info(
+ float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd,
+ float wb)
+{
+ float xo = gvf_parametric_trajectory.p_parametric[0];
+ float yo = gvf_parametric_trajectory.p_parametric[1];
+ float w1 = gvf_parametric_trajectory.p_parametric[2];
+ float w2 = gvf_parametric_trajectory.p_parametric[3];
+ float ratio = gvf_parametric_trajectory.p_parametric[4];
+ float r = gvf_parametric_trajectory.p_parametric[5];
+ float alpha_rad = gvf_parametric_trajectory.p_parametric[6]*M_PI/180;
+
+ // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
+ float nrf1 = cosf(wb*w1)*(r*cosf(wb*w2) + ratio);
+ float nrf2 = sinf(wb*w1)*(r*cosf(wb*w2) + ratio);
+
+ float nrf1d = -w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - cosf(wb*w1)*r*w2*sinf(wb*w2);
+ float nrf2d = w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) - sinf(wb*w1)*r*w2*sinf(wb*w2);
+
+ float nrf1dd = -w1*w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) - cosf(wb*w1)*r*w2*w2*cosf(wb*w2);
+ float nrf2dd = -w1*w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - sinf(wb*w1)*r*w2*w2*cosf(wb*w2);
+
+ *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
+ *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
+
+ *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
+ *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
+
+ *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
+ *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
+}
+
+void gvf_parametric_3d_ellipse_info(
+ float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd,
+ float wb)
+{
+ float xo = gvf_parametric_trajectory.p_parametric[0];
+ float yo = gvf_parametric_trajectory.p_parametric[1];
+ float r = gvf_parametric_trajectory.p_parametric[2];
+ float zl = gvf_parametric_trajectory.p_parametric[3];
+ float zh = gvf_parametric_trajectory.p_parametric[4];
+ float alpha_rad = gvf_parametric_trajectory.p_parametric[5]*M_PI/180;
+
+ // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
+ *f1 = r * cosf(wb) + xo;
+ *f2 = r * sinf(wb) + yo;
+ *f3 = 0.5 * (zh + zl + (zl - zh) * sinf(alpha_rad - wb));
+
+ *f1d = -r * sinf(wb);
+ *f2d = r * cosf(wb);
+ *f3d = -0.5 * (zl - zh) * cosf(alpha_rad - wb);
+
+ *f1dd = -r * cosf(wb);
+ *f2dd = -r * sinf(wb);
+ *f3dd = -0.5 * (zl - zh) * sinf(alpha_rad - wb);
+}
+
+void gvf_parametric_3d_lissajous_info(
+ float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd,
+ float wb)
+{
+ float xo = gvf_parametric_trajectory.p_parametric[0];
+ float yo = gvf_parametric_trajectory.p_parametric[1];
+ float zo = gvf_parametric_trajectory.p_parametric[2];
+ float cx = gvf_parametric_trajectory.p_parametric[3];
+ float cy = gvf_parametric_trajectory.p_parametric[4];
+ float cz = gvf_parametric_trajectory.p_parametric[5];
+ float wx = gvf_parametric_trajectory.p_parametric[6];
+ float wy = gvf_parametric_trajectory.p_parametric[7];
+ float wz = gvf_parametric_trajectory.p_parametric[8];
+ float deltax_rad = gvf_parametric_trajectory.p_parametric[9]*M_PI/180;
+ float deltay_rad = gvf_parametric_trajectory.p_parametric[10]*M_PI/180;
+ float deltaz_rad = gvf_parametric_trajectory.p_parametric[11]*M_PI/180;
+ float alpha_rad = gvf_parametric_trajectory.p_parametric[12]*M_PI/180;
+
+ // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
+
+ float nrf1 = cx*cosf(wx*wb + deltax_rad);
+ float nrf2 = cy*cosf(wy*wb + deltay_rad);
+
+ *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
+ *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
+ *f3 = cz*cosf(wz*wb + deltaz_rad) + zo;
+
+ float nrf1d = -wx*cx*sinf(wx*wb + deltax_rad);
+ float nrf2d = -wy*cy*sinf(wy*wb + deltay_rad);
+
+ *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
+ *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
+ *f3d = -wz*cz*sinf(wz*wb + deltaz_rad);
+
+ float nrf1dd = -wx*wx*cx*cosf(wx*wb + deltax_rad);
+ float nrf2dd = -wy*wy*cy*cosf(wy*wb + deltay_rad);
+
+ *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
+ *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
+ *f3dd = -wz*wz*cz*cosf(wz*wb + deltaz_rad);
+}
+
+// BEZIER
+
+void gvf_parametric_2d_bezier_splines_info(
+ bezier_t *bezier, float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd,
+ float w)
+{
+ // How can we select in which bezier curve are we? Check w. spline zero: 0 <= t <= 1, spline ones: 1 <= t <= 2;
+ float t = w;
+ int n_seg = floorl(t);
+ float tt = t - n_seg;
+ if (n_seg < 0) {
+ n_seg = 0; // w could be < 0 in that case go to first point of first segment
+ tt = 0;
+ }
+ // Evalute the corresponding bezier curve
+ float p0x = bezier[n_seg].p0[0]; float p0y = bezier[n_seg].p0[1];
+ float p1x = bezier[n_seg].p1[0]; float p1y = bezier[n_seg].p1[1];
+ float p2x = bezier[n_seg].p2[0]; float p2y = bezier[n_seg].p2[1];
+ float p3x = bezier[n_seg].p3[0]; float p3y = bezier[n_seg].p3[1];
+
+ // Bézier curves
+
+ // Curve (x,y)
+ *f1 = (1 - tt) * (1 - tt) * (1 - tt) * p0x + 3 * (1 - tt) * (1 - tt) * tt * p1x + 3 *
+ (1 - tt) * tt * tt * p2x + tt * tt * tt * p3x;
+ *f2 = (1 - tt) * (1 - tt) * (1 - tt) * p0y + 3 * (1 - tt) * (1 - tt) * tt * p1y + 3 *
+ (1 - tt) * tt * tt * p2y + tt * tt * tt * p3y;
+
+ // First derivative
+ *f1d = 3 * (1 - tt) * (1 - tt) * (p1x - p0x) + 6 * (1 - tt) * tt * (p2x - p1x) + 3 * tt * tt * (p3x - p2x);
+ *f2d = 3 * (1 - tt) * (1 - tt) * (p1y - p0y) + 6 * (1 - tt) * tt * (p2y - p1y) + 3 * tt * tt * (p3y - p2y);
+
+ // Second derivative
+ *f1dd = 6 * (1 - tt) * (p2x - 2 * p1x + p0x) + 6 * tt * (p3x - 2 * p2x + p1x);
+ *f2dd = 6 * (1 - tt) * (p2y - 2 * p1y + p0y) + 6 * tt * (p3y - 2 * p2y + p1y);
+}
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/trajectories/gvf_param_traj.h b/sw/airborne/modules/guidance/trajectories/gvf_param_traj.h
new file mode 100644
index 0000000000..e538a5c739
--- /dev/null
+++ b/sw/airborne/modules/guidance/trajectories/gvf_param_traj.h
@@ -0,0 +1,80 @@
+/*
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef GVF_PARAMETRIC_TRAJ_H
+#define GVF_PARAMETRIC_TRAJ_H
+
+// Define only one segment by default
+#ifndef GVF_PARAMETRIC_2D_BEZIER_N_SEG
+#define GVF_PARAMETRIC_2D_BEZIER_N_SEG 1
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "modules/guidance/gvf_common.h"
+#include "std.h"
+
+// Parameters for the trajectories
+enum trajectories_parametric {
+ TREFOIL_2D = 0,
+ ELLIPSE_3D = 1,
+ LISSAJOUS_3D = 2,
+ BEZIER_2D = 3,
+ NONE_PARAMETRIC = 255,
+};
+
+typedef struct {
+ enum trajectories_parametric type;
+ float p_parametric[16];
+ int p_len;
+} gvf_parametric_tra;
+
+// Cubic bezier
+typedef struct {
+ float p0[2];
+ float p1[2];
+ float p2[2];
+ float p3[2];
+} bezier_t;
+
+extern gvf_parametric_tra gvf_parametric_trajectory;
+
+/** ------------------------------------------------------------------------ **/
+
+extern void gvf_parametric_2d_trefoil_info(
+ float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd,
+ float wb);
+extern void gvf_parametric_3d_ellipse_info(
+ float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd,
+ float wb);
+extern void gvf_parametric_3d_lissajous_info(
+ float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd,
+ float wb);
+extern void gvf_parametric_2d_bezier_splines_info(
+ bezier_t *bezier, float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd,
+ float w);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // GVF_PARAMETRIC_TRAJ_H
\ No newline at end of file
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.c b/sw/airborne/modules/guidance/trajectories/gvf_traj.c
similarity index 51%
rename from sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.c
rename to sw/airborne/modules/guidance/trajectories/gvf_traj.c
index a468c95b4f..6d4715b3ae 100644
--- a/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.c
+++ b/sw/airborne/modules/guidance/trajectories/gvf_traj.c
@@ -1,6 +1,4 @@
/*
- * Copyright (C) 2016 Hector Garcia de Marina
- *
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
@@ -20,56 +18,74 @@
*
*/
-/** \file gvf_sin.c
- *
- * Guidance algorithm based on vector fields
- * 2D sinusoidal trajectory
- */
+#include "gvf_traj.h"
+// Trajectory
+gvf_tra gvf_trajectory = {NONE, {0}, 1};
+gvf_seg gvf_segment;
-#include "modules/nav/common_nav.h"
-#include "gvf_sin.h"
-#include "generated/airframe.h"
+/** ------------------------------------------------------------------------ **/
-/*! Default gain ke for the sin trajectory*/
-#ifndef GVF_SIN_KE
-#define GVF_SIN_KE 1
-#endif
+void gvf_line_info(float *phi, struct gvf_grad *grad,
+ struct gvf_Hess *hess)
+{
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x;
+ float py = p->y;
+ float a = gvf_trajectory.p[0];
+ float b = gvf_trajectory.p[1];
+ float alpha = gvf_trajectory.p[2];
-/*! Default gain kn for the sin trajectory*/
-#ifndef GVF_SIN_KN
-#define GVF_SIN_KN 1
-#endif
+ // Phi(x,y)
+ *phi = -(px - a) * cosf(alpha) + (py - b) * sinf(alpha);
-/*! Default orientation in rads for the sin trajectory function gvf_sin_**_alpha*/
-#ifndef GVF_SIN_ALPHA
-#define GVF_SIN_ALPHA 0
-#endif
+ // grad Phi
+ grad->nx = -cosf(alpha);
+ grad->ny = sinf(alpha);
-/*! Default frequency for the sin trajectory in rads*/
-#ifndef GVF_SIN_W
-#define GVF_SIN_W 0
-#endif
+ // Hessian Phi
+ hess->H11 = 0;
+ hess->H12 = 0;
+ hess->H21 = 0;
+ hess->H22 = 0;
+}
-/*! Default off-set in rads for the sin trajectory in rads*/
-#ifndef GVF_SIN_OFF
-#define GVF_SIN_OFF 0
-#endif
+void gvf_ellipse_info(float *phi, struct gvf_grad *grad,
+ struct gvf_Hess *hess)
+{
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x;
+ float py = p->y;
+ float wx = gvf_trajectory.p[0];
+ float wy = gvf_trajectory.p[1];
+ float a = gvf_trajectory.p[2];
+ float b = gvf_trajectory.p[3];
+ float alpha = gvf_trajectory.p[4];
-/*! Default amplitude for the sin trajectory in meters*/
-#ifndef GVF_SIN_A
-#define GVF_SIN_A 0
-#endif
+ float cosa = cosf(alpha);
+ float sina = sinf(alpha);
-gvf_s_par gvf_sin_par = {GVF_SIN_KE, GVF_SIN_KN,
- GVF_SIN_ALPHA, GVF_SIN_W, GVF_SIN_OFF, GVF_SIN_A
- };
+ // Phi(x,y)
+ float xel = (px - wx) * cosa - (py - wy) * sina;
+ float yel = (px - wx) * sina + (py - wy) * cosa;
+ *phi = (xel / a) * (xel / a) + (yel / b) * (yel / b) - 1;
+ // grad Phi
+ grad->nx = (2 * xel / (a * a)) * cosa + (2 * yel / (b * b)) * sina;
+ grad->ny = (2 * yel / (b * b)) * cosa - (2 * xel / (a * a)) * sina;
+
+ // Hessian Phi
+ hess->H11 = 2 * (cosa * cosa / (a * a)
+ + sina * sina / (b * b));
+ hess->H12 = 2 * sina * cosa * (1 / (b * b) - 1 / (a * a));
+ hess->H21 = hess->H12;
+ hess->H22 = 2 * (sina * sina / (a * a)
+ + cosa * cosa / (b * b));
+}
void gvf_sin_info(float *phi, struct gvf_grad *grad,
struct gvf_Hess *hess)
{
-
struct EnuCoor_f *p = stateGetPositionEnu_f();
float px = p->x;
float py = p->y;
diff --git a/sw/airborne/modules/guidance/trajectories/gvf_traj.h b/sw/airborne/modules/guidance/trajectories/gvf_traj.h
new file mode 100644
index 0000000000..e45bd2bac7
--- /dev/null
+++ b/sw/airborne/modules/guidance/trajectories/gvf_traj.h
@@ -0,0 +1,83 @@
+/*
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef GVF_TRAJ_H
+#define GVF_TRAJ_H
+
+#include "modules/guidance/gvf_common.h"
+
+enum trajectories {
+ LINE = 0,
+ ELLIPSE,
+ SIN,
+ NONE = 255,
+};
+
+typedef struct {
+ enum trajectories type;
+ float p[16];
+ int p_len;
+} gvf_tra;
+
+struct gvf_grad {
+ float nx;
+ float ny;
+ float nz;
+};
+
+struct gvf_Hess {
+ float H11;
+ float H12;
+ float H13;
+ float H21;
+ float H22;
+ float H23;
+ float H31;
+ float H32;
+ float H33;
+};
+
+/** @typedef gvf_seg
+* @brief Struct employed by the LINE trajectory for the special case of tracking
+a segment, which is described by the coordinates x1, y1, x2, y2
+* @param seg Tracking a segment or not
+* @param x1 coordinate w.r.t. HOME
+* @param y1 coordinate w.r.t. HOME
+* @param x2 coordinate w.r.t. HOME
+* @param y2 coordinate w.r.t. HOME
+*/
+typedef struct {
+ int seg;
+ float x1;
+ float y1;
+ float x2;
+ float y2;
+} gvf_seg;
+
+extern gvf_tra gvf_trajectory;
+extern gvf_seg gvf_segment;
+
+/** ------------------------------------------------------------------------ **/
+
+extern void gvf_line_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+extern void gvf_ellipse_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+extern void gvf_sin_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+
+#endif // GVF_TRAJ_H
\ No newline at end of file
diff --git a/sw/airborne/modules/multi/dcf/dcf.c b/sw/airborne/modules/multi/dcf/dcf.c
index 69bc742253..414ffe0479 100644
--- a/sw/airborne/modules/multi/dcf/dcf.c
+++ b/sw/airborne/modules/multi/dcf/dcf.c
@@ -112,7 +112,7 @@ bool distributed_circular(uint8_t wp)
u *= -gvf_control.s * dcf_control.k;
- gvf_ellipse_XY(xc, yc, dcf_control.radius + u, dcf_control.radius + u, 0);
+ nav_gvf_ellipse_XY(xc, yc, dcf_control.radius + u, dcf_control.radius + u, 0);
if ((now - last_transmision > dcf_control.broadtime) && (autopilot_get_mode() == AP_MODE_AUTO2)) {
send_theta_to_nei();
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_base.c b/sw/airborne/modules/nav/nav_rotorcraft_base.c
index 949c14cebb..089537b40a 100644
--- a/sw/airborne/modules/nav/nav_rotorcraft_base.c
+++ b/sw/airborne/modules/nav/nav_rotorcraft_base.c
@@ -259,6 +259,7 @@ static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
+#if ROTORCRAFT_BASE_SEND_TRAJECTORY
static void send_segment(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_SEGMENT(trans, dev, AC_ID,
@@ -275,6 +276,7 @@ static void send_circle(struct transport_tx *trans, struct link_device *dev)
&nav_rotorcraft_base.circle.center.y,
&nav_rotorcraft_base.circle.radius);
}
+# endif // ROTORCRAFT_BASE_SEND_TRAJECTORY
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
{
@@ -285,11 +287,13 @@ static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
&dist_home, &dist_wp,
&nav_block, &nav_stage,
&nav.horizontal_mode);
+#if ROTORCRAFT_BASE_SEND_TRAJECTORY
if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ROUTE) {
send_segment(trans, dev);
} else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_CIRCLE) {
send_circle(trans, dev);
}
+#endif // ROTORCRAFT_BASE_SEND_TRAJECTORY
}
#endif
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_base.h b/sw/airborne/modules/nav/nav_rotorcraft_base.h
index 6d44ae8cee..331ce7460d 100644
--- a/sw/airborne/modules/nav/nav_rotorcraft_base.h
+++ b/sw/airborne/modules/nav/nav_rotorcraft_base.h
@@ -29,6 +29,10 @@
#include "modules/nav/nav_base.h"
+#ifndef ROTORCRAFT_BASE_SEND_TRAJECTORY
+#define ROTORCRAFT_BASE_SEND_TRAJECTORY TRUE
+#endif
+
/** Basic Nav struct
*/
extern struct NavBase_t nav_rotorcraft_base;
diff --git a/sw/simulator/nps/nps_autopilot_rover.c b/sw/simulator/nps/nps_autopilot_rover.c
index 062a363c21..390bd109fa 100644
--- a/sw/simulator/nps/nps_autopilot_rover.c
+++ b/sw/simulator/nps/nps_autopilot_rover.c
@@ -179,7 +179,8 @@ void nps_autopilot_run_step(double time)
void sim_overwrite_ahrs(void)
-{
+{
+ stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_NPS_ID);
struct FloatQuat quat_f;
QUAT_COPY(quat_f, fdm.ltp_to_body_quat);