diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml
index 331faddbb2..fcf79517e9 100644
--- a/conf/airframes/tudelft/nederdrone4.xml
+++ b/conf/airframes/tudelft/nederdrone4.xml
@@ -73,6 +73,10 @@
+
+
+
+
@@ -84,7 +88,7 @@
-
+
diff --git a/conf/airframes/tudelft/nederdrone5.xml b/conf/airframes/tudelft/nederdrone5.xml
index 81fb4e2fa7..e4bc0444a0 100644
--- a/conf/airframes/tudelft/nederdrone5.xml
+++ b/conf/airframes/tudelft/nederdrone5.xml
@@ -70,6 +70,10 @@
+
+
+
+
@@ -80,7 +84,7 @@
-
+
@@ -193,7 +197,7 @@
-
+
diff --git a/conf/airframes/tudelft/nederdrone6.xml b/conf/airframes/tudelft/nederdrone6.xml
new file mode 100644
index 0000000000..d16849bfa2
--- /dev/null
+++ b/conf/airframes/tudelft/nederdrone6.xml
@@ -0,0 +1,394 @@
+
+
+
+
+
+ Neddrone6
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/tudelft/nederdrone7.xml b/conf/airframes/tudelft/nederdrone7.xml
new file mode 100644
index 0000000000..bac5d1f2e0
--- /dev/null
+++ b/conf/airframes/tudelft/nederdrone7.xml
@@ -0,0 +1,393 @@
+
+
+
+
+
+ Neddrone7
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml
new file mode 100644
index 0000000000..49156290e6
--- /dev/null
+++ b/conf/airframes/tudelft/nederdrone8.xml
@@ -0,0 +1,396 @@
+
+
+
+
+
+ Neddrone8
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
index 60b352888e..9abfdc95e5 100644
--- a/conf/conf_tests.xml
+++ b/conf/conf_tests.xml
@@ -7,7 +7,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
settings="settings/rotorcraft_basic.xml"
- settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
+ settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
+
+
+
+ #include "autopilot.h"
+ #include "modules/datalink/datalink.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/tudelft/nederdrone_troia.xml b/conf/flight_plans/tudelft/nederdrone_troia.xml
new file mode 100644
index 0000000000..ee4a0f7e46
--- /dev/null
+++ b/conf/flight_plans/tudelft/nederdrone_troia.xml
@@ -0,0 +1,220 @@
+
+
+
+
+ #include "autopilot.h"
+ #include "modules/datalink/datalink.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/approach_moving_target.xml b/conf/modules/approach_moving_target.xml
new file mode 100644
index 0000000000..f801b933f8
--- /dev/null
+++ b/conf/modules/approach_moving_target.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+ Approach a moving target (e.g. ship) along a diagonal.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ target_pos
+
+
+
+
+
+
+
+
diff --git a/conf/modules/eff_scheduling_nederdrone.xml b/conf/modules/eff_scheduling_nederdrone.xml
new file mode 100644
index 0000000000..64d1aff7b7
--- /dev/null
+++ b/conf/modules/eff_scheduling_nederdrone.xml
@@ -0,0 +1,30 @@
+
+
+
+ Interpolation of control effectivenss matrix
+of the Nederdrone.
+
+If instead using online adaptation is an option, be sure to
+not use this module at the same time!
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/target_pos.xml b/conf/modules/target_pos.xml
new file mode 100644
index 0000000000..0502c31908
--- /dev/null
+++ b/conf/modules/target_pos.xml
@@ -0,0 +1,39 @@
+
+
+
+
+
+ Target position calculation
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/pprz_center_settings.ini b/conf/pprz_center_settings.ini
new file mode 100644
index 0000000000..b9507f4a54
--- /dev/null
+++ b/conf/pprz_center_settings.ini
@@ -0,0 +1,4 @@
+[ui]
+last_AC=CubeOrange
+last_session=Flight ACM1 Transparent @115200
+window_size=@Size(1624 986)
diff --git a/conf/simulator/jsbsim/aircraft/nederdrone4_tem.xml b/conf/simulator/jsbsim/aircraft/nederdrone4_tem.xml
new file mode 100644
index 0000000000..5e4c31b703
--- /dev/null
+++ b/conf/simulator/jsbsim/aircraft/nederdrone4_tem.xml
@@ -0,0 +1,578 @@
+
+
+
+
+
+ Ewoud Smeur
+ 23-04-2020
+ Version 0.92 - beta
+ Nederdrone
+
+
+
+ 1
+ 2
+ 0.25
+ 0
+ 0
+ 0
+ 0
+ 90
+
+ 0
+ 0
+ 0
+
+
+ 0
+ 0
+ 0
+
+
+ 0
+ 0
+ 0
+
+
+
+
+ 2.25
+ 0.3322
+ 4.0
+ 0.
+ 0.
+ 0.
+ 13.0
+
+ 0
+ 0
+ 0
+
+
+
+
+
+
+ 0.41
+ 0.2
+ -0.11
+
+ 0.8
+ 0.5
+ 500
+ 100
+ 1000
+ 0.0
+ NONE
+ 0
+
+
+
+
+ 0.41
+ -0.2
+ -0.11
+
+ 0.8
+ 0.5
+ 500
+ 100
+ 1000
+ 0.0
+ NONE
+ 0
+
+
+
+
+ 0.11
+ 0.2
+ 0.41
+
+ 0.8
+ 0.5
+ 500
+ 100
+ 1000
+ 0.0
+ NONE
+ 0
+
+
+
+
+ 0.11
+ -0.2
+ 0.41
+
+ 0.8
+ 0.5
+ 500
+ 100
+ 1000
+ 0.0
+ NONE
+ 0
+
+
+
+
+
+
+
+
+ fcs/a8
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a0
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a0
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a1
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a1
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a8
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a2
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a2
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a2
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a3
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a3
+
+ 0
+ 1
+
+ 18.0
+
+
+
+ fcs/a3
+
+ 0
+ 1
+
+ 18.0
+
+
+
+
+
+
+
+ fcs/a0
+ fcs/a1
+ fcs/a2
+ fcs/a3
+ fcs/a4
+ fcs/a5
+ fcs/a6
+ fcs/a7
+ fcs/a8
+
+ fcs/m1_lag
+ fcs/m2_lag
+ fcs/m3_lag
+ fcs/m4_lag
+ fcs/m5_lag
+ fcs/m6_lag
+ fcs/m7_lag
+ fcs/m8_lag
+ fcs/m9_lag
+ fcs/m10_lag
+ fcs/m11_lag
+ fcs/m12_lag
+
+ fcs/ail1
+ fcs/ail2
+ fcs/ail3
+ fcs/ail4
+
+
+
+
+
+
+
+ fcs/m1_lag
+ 3.822
+
+
+ fcs/a4
+ 0.785
+
+
+
+
+
+ -0.141
+ -1.105
+ 0.1
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m1_lag
+ 3.822
+
+
+ fcs/a4
+ 0.785
+
+
+
+
+
+ -0.141
+ -1.105
+ 0.1
+
+
+ 1
+ 0
+ 0
+
+
+
+
+
+
+ fcs/m2_lag
+ 3.822
+
+
+
+ -0.141
+ -0.735
+ 0.2
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m3_lag
+ 3.822
+
+
+
+ -0.141
+ -0.320
+ 0.2
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m4_lag
+ 3.822
+
+
+
+ -0.141
+ 0.320
+ 0.2
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m5_lag
+ 3.822
+
+
+
+ -0.141
+ 0.735
+ 0.2
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m6_lag
+ 3.822
+
+
+ fcs/a5
+ 0.785
+
+
+
+
+
+ -0.141
+ 1.105
+ 0.1
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m6_lag
+ 3.822
+
+
+ fcs/a5
+ 0.785
+
+
+
+
+
+ -0.141
+ 1.105
+ 0.1
+
+
+ 1
+ 0
+ 0
+
+
+
+
+
+
+ fcs/m7_lag
+ 3.822
+
+
+
+ 0.141
+ -1.105
+ -0.4
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m8_lag
+ 3.822
+
+
+
+ 0.141
+ -0.735
+ -0.1
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m9_lag
+ 3.822
+
+
+
+ 0.141
+ -0.320
+ -0.1
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m10_lag
+ 3.822
+
+
+
+ 0.141
+ 0.320
+ -0.1
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m11_lag
+ 3.822
+
+
+
+ 0.141
+ 0.735
+ -0.1
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+ fcs/m12_lag
+ 3.822
+
+
+
+ 0.141
+ 1.105
+ -0.4
+
+
+ 0
+ 0
+ -1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index d2f5247d63..d10731b388 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -9,6 +9,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
gui_color="red"
+ release="ac5eaa8da36b999a135f979e598930a9d40f6401"
/>
+
+
+
-
-
qi = 1 + z_rot.z;
tilt->qx = axis.x;
@@ -1021,6 +1021,17 @@ void vect_bound_in_2d(struct FloatVect3 *vect3, float bound) {
}
}
+/* Scale a 3D vector to within a 3D bound */
+void vect_bound_in_3d(struct FloatVect3 *vect3, float bound) {
+ float norm = FLOAT_VECT3_NORM(*vect3);
+ if(norm>bound) {
+ float scale = bound/norm;
+ vect3->x *= scale;
+ vect3->y *= scale;
+ vect3->z *= scale;
+ }
+}
+
/* Scale a 3D vector to a certain length in 2D */
void vect_scale(struct FloatVect3 *vect3, float norm_des) {
float norm = FLOAT_VECT2_NORM(*vect3);
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index 2024f1f2b5..fe63e50000 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -886,6 +886,7 @@ extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct Fl
extern bool float_mat_inv_4d(float invOut[16], float mat_in[16]);
extern void vect_bound_in_2d(struct FloatVect3 *vect3, float bound);
+extern void vect_bound_in_3d(struct FloatVect3 *vect3, float bound);
extern void vect_scale(struct FloatVect3 *vect3, float norm_des);
#ifdef __cplusplus
diff --git a/sw/airborne/modules/ctrl/approach_moving_target.c b/sw/airborne/modules/ctrl/approach_moving_target.c
new file mode 100644
index 0000000000..ac5786de3b
--- /dev/null
+++ b/sw/airborne/modules/ctrl/approach_moving_target.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright (C) 2021 Ewoud Smeur
+ *
+ * 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/ctrl/approach_moving_target.c"
+ * @author Ewoud Smeur
+ * Approach a moving target (e.g. ship)
+ */
+
+#include "approach_moving_target.h"
+
+#include "generated/modules.h"
+#include "modules/core/abi.h"
+
+float amt_err_slowdown_gain = AMT_ERR_SLOWDOWN_GAIN;
+
+float approach_moving_target_angle_deg;
+
+#define DEBUG_AMT TRUE
+#include
+
+struct Amt amt = {
+ .distance = 60,
+ .speed = -1.0,
+ .pos_gain = 0.3,
+ .psi_ref = 180.0,
+ .slope_ref = 35.0,
+ .speed_gain = 1.0,
+ .relvel_gain = 1.0,
+ .enabled_time = 0,
+ .wp_id = 0,
+};
+
+struct AmtTelem {
+ struct FloatVect3 des_pos;
+ struct FloatVect3 des_vel;
+};
+
+struct AmtTelem amt_telem;
+bool approach_moving_target_enabled = false;
+
+struct FloatVect3 nav_get_speed_sp_from_diagonal(struct EnuCoor_i target, float pos_gain, float rope_heading);
+void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned);
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+static void send_approach_moving_target(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_APPROACH_MOVING_TARGET(trans, dev, AC_ID,
+ &amt_telem.des_pos.x,
+ &amt_telem.des_pos.y,
+ &amt_telem.des_pos.z,
+ &amt_telem.des_vel.x,
+ &amt_telem.des_vel.y,
+ &amt_telem.des_vel.z,
+ &amt.distance);
+}
+#endif
+
+void approach_moving_target_init(void)
+{
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_APPROACH_MOVING_TARGET, send_approach_moving_target);
+#endif
+}
+
+// Update a waypoint such that you can see on the GCS where the drone wants to go
+void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned) {
+
+ // Update the waypoint
+ struct EnuCoor_f target_enu;
+ ENU_OF_TO_NED(target_enu, *target_ned);
+ waypoint_set_enu(wp_id, &target_enu);
+
+ // Send waypoint update every half second
+ RunOnceEvery(100/2, {
+ // Send to the GCS that the waypoint has been moved
+ DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
+ &waypoints[wp_id].enu_i.x,
+ &waypoints[wp_id].enu_i.y,
+ &waypoints[wp_id].enu_i.z);
+ } );
+}
+
+// Function to enable from flight plan (call repeatedly!)
+void approach_moving_target_enable(uint8_t wp_id) {
+ amt.enabled_time = get_sys_time_msec();
+ amt.wp_id = wp_id;
+}
+
+/**
+ * @brief Generates a velocity reference from a diagonal approach path
+ *
+ */
+void follow_diagonal_approach(void) {
+
+ // Check if the flight plan recently called the enable function
+ if ( (get_sys_time_msec() - amt.enabled_time) > (2000 / NAVIGATION_FREQUENCY)) {
+ return;
+ }
+
+ // Get the target position, velocity and heading
+ struct NedCoor_f target_pos, target_vel = {0};
+ float target_heading;
+ if(!target_get_pos(&target_pos, &target_heading)) {
+ // TODO: What to do? Same can be checked for the velocity
+ return;
+ }
+ target_get_vel(&target_vel);
+ VECT3_SMUL(target_vel, target_vel, amt.speed_gain);
+
+ // Reference model
+ float gamma_ref = RadOfDeg(amt.slope_ref);
+ float psi_ref = RadOfDeg(target_heading + amt.psi_ref);
+
+ amt.rel_unit_vec.x = cosf(gamma_ref) * cosf(psi_ref);
+ amt.rel_unit_vec.y = cosf(gamma_ref) * sinf(psi_ref);
+ amt.rel_unit_vec.z = -sinf(gamma_ref);
+
+ // desired position = rel_pos + target_pos
+ struct FloatVect3 ref_relpos;
+ VECT3_SMUL(ref_relpos, amt.rel_unit_vec, amt.distance);
+
+ // ATTENTION, target_pos is already relative now!
+ struct FloatVect3 des_pos;
+ VECT3_SUM(des_pos, ref_relpos, target_pos);
+
+ struct FloatVect3 ref_relvel;
+ VECT3_SMUL(ref_relvel, amt.rel_unit_vec, amt.speed * amt.relvel_gain);
+
+ // error controller
+ struct FloatVect3 pos_err;
+ struct FloatVect3 ec_vel;
+ struct NedCoor_f *drone_pos = stateGetPositionNed_f();
+ // ATTENTION, target_pos is already relative now!
+ // VECT3_DIFF(pos_err, des_pos, *drone_pos);
+ VECT3_COPY(pos_err, des_pos);
+ VECT3_SMUL(ec_vel, pos_err, amt.pos_gain);
+
+ // desired velocity = rel_vel + target_vel + error_controller(using NED position)
+ struct FloatVect3 des_vel = {
+ ref_relvel.x + target_vel.x + ec_vel.x,
+ ref_relvel.y + target_vel.y + ec_vel.y,
+ ref_relvel.z + target_vel.z + ec_vel.z,
+ };
+
+ vect_bound_in_3d(&des_vel, 10.0);
+
+ // Bound vertical speed setpoint
+ if(stateGetAirspeed_f() > 13.0) {
+ Bound(des_vel.z, -4.0, 5.0);
+ } else {
+ Bound(des_vel.z, -nav_climb_vspeed, -nav_descend_vspeed);
+ }
+
+ AbiSendMsgVEL_SP(VEL_SP_FCR_ID, &des_vel);
+
+ /* limit the speed such that the vertical component is small enough
+ * and doesn't outrun the vehicle
+ */
+ float min_speed;
+ float sin_gamma_ref = sinf(gamma_ref);
+ if (sin_gamma_ref > 0.05) {
+ min_speed = (nav_descend_vspeed+0.1) / sin_gamma_ref;
+ } else {
+ min_speed = -5.0; // prevent dividing by zero
+ }
+
+ // The upper bound is not very important
+ Bound(amt.speed, min_speed, 4.0);
+
+ // Reduce approach speed if the error is large
+ float norm_pos_err_sq = VECT3_NORM2(pos_err);
+ float int_speed = amt.speed / (norm_pos_err_sq * amt_err_slowdown_gain + 1.0);
+
+ // integrate speed to get the distance
+ float dt = FOLLOW_DIAGONAL_APPROACH_PERIOD;
+ amt.distance += int_speed*dt;
+
+ // For display purposes
+ struct FloatVect3 disp_pos_target;
+ VECT3_SUM(disp_pos_target, des_pos, *drone_pos);
+ update_waypoint(amt.wp_id, &disp_pos_target);
+
+ // Debug target pos:
+ VECT3_DIFF(amt_telem.des_pos, *drone_pos, target_pos);
+
+ // Update values for telemetry
+ VECT3_COPY(amt_telem.des_pos, des_pos);
+ VECT3_COPY(amt_telem.des_vel, des_vel);
+}
diff --git a/sw/airborne/modules/ctrl/approach_moving_target.h b/sw/airborne/modules/ctrl/approach_moving_target.h
new file mode 100644
index 0000000000..0c7097a064
--- /dev/null
+++ b/sw/airborne/modules/ctrl/approach_moving_target.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2021 Ewoud Smeur
+ *
+ * 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/ctrl/approach_moving_target.h"
+ * @author Ewoud Smeur
+ * Approach a moving target (e.g. ship)
+ */
+
+#ifndef APPROACH_MOVING_TARGET_H
+#define APPROACH_MOVING_TARGET_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+// Angle of the approach in degrees
+extern float approach_moving_target_angle_deg;
+
+struct Amt {
+ struct FloatVect3 rel_unit_vec;
+ float distance;
+ float speed;
+ float pos_gain;
+ float psi_ref;
+ float slope_ref;
+ float speed_gain;
+ float relvel_gain;
+ int32_t enabled_time;
+ uint8_t wp_id;
+};
+
+extern struct Amt amt;
+extern float amt_err_slowdown_gain;
+
+extern void approach_moving_target_init(void);
+extern void follow_diagonal_approach(void);
+extern void approach_moving_target_enable(uint8_t wp_id);
+
+#endif // APPROACH_MOVING_TARGET_H
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
new file mode 100644
index 0000000000..91d22c7d0d
--- /dev/null
+++ b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) 2022 Dennis van Wijngaarden
+ *
+ * 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/ctrl/eff_scheduling_nederdrone.c"
+ * @author Dennis van Wijngaarden
+ * Interpolation of control effectivenss matrix
+of the Nederdrone.
+
+If instead using online adaptation is an option, be sure to
+not use this module at the same time!
+ */
+
+#include "modules/ctrl/eff_scheduling_nederdrone.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
+#include "state.h"
+#include "autopilot.h"
+#include "modules/radio_control/radio_control.h"
+#include "generated/radio.h"
+#include "modules/actuators/actuators.h"
+#define INDI_SCHEDULING_LOWER_BOUND_G1 0.0001
+
+// Airspeed at which tip props should be turned on
+#ifndef INDI_SCHEDULING_LOW_AIRSPEED
+#define INDI_SCHEDULING_LOW_AIRSPEED 12.0
+#endif
+
+bool all_act_fwd_sched = false;
+
+int32_t use_scheduling = 1;
+
+float thrust_eff_scaling = 1.0;
+
+static float g_forward[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL_FWD, STABILIZATION_INDI_G1_PITCH_FWD, STABILIZATION_INDI_G1_YAW_FWD, STABILIZATION_INDI_G1_THRUST_FWD};
+
+static float g_hover[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST};
+
+// Functions to schedule switching on and of of tip props on front wing
+float sched_ratio_tip_props = 1.0;
+// If pitch lower, pitch props gradually switch off till sched_tip_prop_lower_pitch_limit_deg (1 > sched_ratio_tip_props > 0)
+float sched_tip_prop_upper_pitch_limit_deg = -45;
+// If pitch lower, pitch props switch fully off (sched_ratio_tip_props goes to 0)
+float sched_tip_prop_lower_pitch_limit_deg = -65;
+// Setting to not switch off tip props during forward flight
+bool sched_tip_props_always_on = false;
+
+void schdule_control_effectiveness(void);
+
+void ctrl_eff_scheduling_init(void)
+{
+ // your init code here
+ for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
+ g_hover[0][i] = g_hover[0][i] / INDI_G_SCALING;
+ g_hover[1][i] = g_hover[1][i] / INDI_G_SCALING;
+ g_hover[2][i] = g_hover[2][i] / INDI_G_SCALING;
+ g_hover[3][i] = g_hover[3][i] / INDI_G_SCALING;
+
+ g_forward[0][i] = g_forward[0][i] / INDI_G_SCALING;
+ g_forward[1][i] = g_forward[1][i] / INDI_G_SCALING;
+ g_forward[2][i] = g_forward[2][i] / INDI_G_SCALING;
+ g_forward[3][i] = g_forward[3][i] / INDI_G_SCALING;
+ }
+}
+
+void ctrl_eff_scheduling_periodic(void)
+{
+#ifdef SITL
+ sched_ratio_tip_props = 1.0;
+#else
+ schdule_control_effectiveness();
+#endif
+}
+
+/**
+ * Function that calculates control effectiveness values for the Nederdrone inner loop.
+ * Default requency: 20 Hz.
+ */
+void schdule_control_effectiveness(void) {
+ float airspeed = stateGetAirspeed_f();
+
+ float ratio = 0.0;
+ struct FloatEulers eulers_zxy;
+ float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+
+ // Ratio is only based on pitch now, as the pitot tube is often not mounted.
+ if (use_scheduling == 1) {
+ ratio = fabs(eulers_zxy.theta) / M_PI_2;
+ } else {
+ ratio = 0.0;
+ }
+ Bound(ratio,0.0,1.0);
+
+ float stall_speed = 14.0; // m/s
+ float pitch_ratio = 0.0;
+ // Assume hover or stalled conditions below 14 m/s
+ if (use_scheduling == 1) {
+ if ( (eulers_zxy.theta > -M_PI_4) || (airspeed < stall_speed) ) {
+ if (eulers_zxy.theta > -M_PI_4) {
+ pitch_ratio = 0.0;
+ } else {
+ pitch_ratio = fabs(1.0 - eulers_zxy.theta/(-M_PI_4));
+ }
+
+ } else {
+ pitch_ratio = 1.0;
+ }
+ } else {
+ pitch_ratio = 0.0;
+ }
+ Bound(pitch_ratio,0.0,1.0);
+
+ float airspeed_pitch_eff = airspeed;
+ Bound(airspeed_pitch_eff, 8.0, 30.0);
+
+ for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
+
+ // Roll
+ g1g2[0][i] = g_hover[0][i] * (1.0 - ratio) + g_forward[0][i] * ratio;
+ //Pitch, scaled with v^2
+ if (i>3 || all_act_fwd_sched) {
+ g1g2[1][i] = g_hover[1][i] * (1.0 - pitch_ratio) + g_forward[1][i] * pitch_ratio;
+ } else {
+ g1g2[1][i] = g_hover[1][i] * (1.0 - pitch_ratio) + g_forward[1][i] * pitch_ratio * airspeed_pitch_eff * airspeed_pitch_eff / (16.0*16.0);
+ }
+ //Yaw
+ g1g2[2][i] = g_hover[2][i] * (1.0 - ratio) + g_forward[2][i] * ratio;
+
+ // Determine thrust of the wing to adjust the effectiveness of servos
+ float wing_thrust_scaling = 1.0;
+#if INDI_NUM_ACT != 8
+#error "ctfl_eff_scheduling_nederdrone is very specific and only works for one Nederdrone configuration!"
+#endif
+ if (i>3) {
+ float wing_thrust = actuators_pprz[i-4];
+ Bound(wing_thrust,3000.0,9600.0);
+ wing_thrust_scaling = wing_thrust/9600.0/0.8;
+ }
+
+ g1g2[0][i] *= wing_thrust_scaling;
+ g1g2[1][i] *= wing_thrust_scaling;
+ g1g2[2][i] *= wing_thrust_scaling;
+ }
+
+ // Thrust effectiveness
+ float ratio_spec_force = 0.0;
+ float airspeed_spec_force = airspeed;
+ Bound(airspeed_spec_force, 8.0, 20.0);
+ ratio_spec_force = (airspeed_spec_force-8.0) / 12.0;
+
+ for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
+ // Thrust
+ g1g2[3][i] = g_hover[3][i] * (1.0 - ratio_spec_force) + g_forward[3][i] * ratio_spec_force;
+ g1g2[3][i] *= thrust_eff_scaling;
+ }
+
+ bool low_airspeed = stateGetAirspeed_f() < INDI_SCHEDULING_LOW_AIRSPEED;
+
+ // Tip prop ratio
+ float pitch_deg = eulers_zxy.theta / M_PI * 180.f;
+ float pitch_range_deg = sched_tip_prop_upper_pitch_limit_deg - sched_tip_prop_lower_pitch_limit_deg;
+ if (sched_tip_props_always_on || low_airspeed || radio_control.values[RADIO_AUX2] > 0) {
+ sched_ratio_tip_props = 1.0;
+ } else {
+ float pitch_offset = pitch_deg - sched_tip_prop_lower_pitch_limit_deg;
+ sched_ratio_tip_props = pitch_offset / pitch_range_deg;
+ }
+ Bound(sched_ratio_tip_props, 0.0, 1.0);
+}
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.h b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.h
new file mode 100644
index 0000000000..d50e826527
--- /dev/null
+++ b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2022 Dennis van Wijngaarden
+ *
+ * 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/ctrl/eff_scheduling_nederdrone.h"
+ * @author Dennis van Wijngaarden
+ * Interpolation of control effectivenss matrix
+of the Nederdrone.
+
+If instead using online adaptation is an option, be sure to
+not use this module at the same time!
+ */
+
+#ifndef EFF_SCHEDULING_NEDERDRONE_H
+#define EFF_SCHEDULING_NEDERDRONE_H
+
+#include
+#include
+
+extern void ctrl_eff_scheduling_init(void);
+extern void ctrl_eff_scheduling_periodic(void);
+
+// Functions to schedule switching on and of of tip props on front wing
+extern float sched_ratio_tip_props;
+// If pitch lower, pitch props gradually switch off till sched_tip_prop_lower_pitch_limit_deg (1 > sched_ratio_tip_props > 0)
+extern float sched_tip_prop_upper_pitch_limit_deg;
+// If pitch lower, pitch props switch fully off (sched_ratio_tip_props goes to 0)
+extern float sched_tip_prop_lower_pitch_limit_deg;
+// Setting to not switch off tip props during forward flight
+extern bool sched_tip_props_always_on;
+// Setting to scale the thrust effectiveness
+extern float thrust_eff_scaling;
+
+// Schedule all forward actuators with airspeed instead of only the flaps
+extern bool all_act_fwd_sched;
+
+#endif // EFF_SCHEDULING_NEDERDRONE_H
diff --git a/sw/airborne/modules/ctrl/target_pos.c b/sw/airborne/modules/ctrl/target_pos.c
new file mode 100644
index 0000000000..f07991199d
--- /dev/null
+++ b/sw/airborne/modules/ctrl/target_pos.c
@@ -0,0 +1,229 @@
+/*
+ * Copyright (C) 2021 Freek van Tienen
+ *
+ * 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/ctrl/target_pos.c"
+ * @author Freek van Tienen
+ * Control a rotorcraft to follow at a defined distance from the target
+ */
+
+#include "target_pos.h"
+#include
+
+#include "modules/datalink/telemetry.h"
+#include "modules/core/abi.h"
+
+// The timeout when receiving GPS messages from the ground in ms
+#ifndef TARGET_POS_TIMEOUT
+#define TARGET_POS_TIMEOUT 5000
+#endif
+
+// The timeout when recceiving an RTK gps message from the GPS
+#ifndef TARGET_RTK_TIMEOUT
+#define TARGET_RTK_TIMEOUT 1000
+#endif
+
+#ifndef TARGET_OFFSET_HEADING
+#define TARGET_OFFSET_HEADING 180.0
+#endif
+
+#ifndef TARGET_OFFSET_DISTANCE
+#define TARGET_OFFSET_DISTANCE 12.0
+#endif
+
+#ifndef TARGET_OFFSET_HEIGHT
+#define TARGET_OFFSET_HEIGHT -1.2
+#endif
+
+#ifndef TARGET_INTEGRATE_XY
+#define TARGET_INTEGRATE_XY true
+#endif
+
+#ifndef TARGET_INTEGRATE_Z
+#define TARGET_INTEGRATE_Z false
+#endif
+
+/* Initialize the main structure */
+struct target_t target = {
+ .pos = {0},
+ .offset = {
+ .heading = TARGET_OFFSET_HEADING,
+ .distance = TARGET_OFFSET_DISTANCE,
+ .height = TARGET_OFFSET_HEIGHT,
+ },
+ .target_pos_timeout = TARGET_POS_TIMEOUT,
+ .rtk_timeout = TARGET_RTK_TIMEOUT,
+ .integrate_xy = TARGET_INTEGRATE_XY,
+ .integrate_z = TARGET_INTEGRATE_Z
+};
+
+/* GPS abi callback */
+static abi_event gps_ev;
+static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+static void send_target_pos_info(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_TARGET_POS_INFO(trans, dev, AC_ID,
+ &target.pos.lla.lat,
+ &target.pos.lla.lon,
+ &target.pos.lla.alt,
+ &target.pos.ground_speed,
+ &target.pos.climb,
+ &target.pos.course,
+ &target.pos.heading,
+ &target.offset.heading,
+ &target.offset.distance,
+ &target.offset.height);
+}
+#endif
+
+void target_pos_init(void)
+{
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_TARGET_POS_INFO, send_target_pos_info);
+#endif
+
+ AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
+}
+
+/* Get the GPS lla position */
+static void gps_cb(uint8_t sender_id __attribute__((unused)),
+ uint32_t stamp __attribute__((unused)),
+ struct GpsState *gps_s)
+{
+ target.gps_lla.lat = gps_s->lla_pos.lat;
+ target.gps_lla.lon = gps_s->lla_pos.lon;
+ target.gps_lla.alt = gps_s->lla_pos.alt;
+}
+
+/**
+ * Receive a TARGET_POS message from the ground
+ */
+void target_parse_target_pos(uint8_t *buf)
+{
+ if(DL_TARGET_POS_ac_id(buf) != AC_ID)
+ return;
+
+ // Save the received values
+ target.pos.recv_time = get_sys_time_msec();
+ target.pos.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // FIXME: need to get from the real GPS
+ target.pos.lla.lat = DL_TARGET_POS_lat(buf);
+ target.pos.lla.lon = DL_TARGET_POS_lon(buf);
+ target.pos.lla.alt = DL_TARGET_POS_alt(buf);
+ target.pos.ground_speed = DL_TARGET_POS_speed(buf);
+ target.pos.climb = DL_TARGET_POS_climb(buf);
+ target.pos.course = DL_TARGET_POS_course(buf);
+ target.pos.heading = DL_TARGET_POS_heading(buf);
+ target.pos.valid = true;
+}
+
+/**
+ * Get the current target position (NED) and heading
+ */
+bool target_get_pos(struct NedCoor_f *pos, float *heading) {
+ float time_diff = 0;
+
+ /* When we have a valid target_pos message, state ned is initialized and no timeout */
+ if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec()) {
+ struct NedCoor_i target_pos_cm, drone_pos_cm;
+
+ // Convert from LLA to NED using origin from the UAV
+ ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &target.pos.lla);
+ // Convert from LLA to NED using origin from the UAV
+ ned_of_lla_point_i(&drone_pos_cm, &state.ned_origin_i, &target.gps_lla);
+
+ // Convert to floating point (cm to meters)
+ pos->x = (target_pos_cm.x - drone_pos_cm.x) * 0.01;
+ pos->y = (target_pos_cm.y - drone_pos_cm.y) * 0.01;
+ pos->z = (target_pos_cm.z - drone_pos_cm.z) * 0.01;
+
+ // In seconds, overflow uint32_t in 49,7 days
+ time_diff = (get_sys_time_msec() - target.pos.recv_time) * 0.001; // FIXME: should be based on TOW of ground gps
+
+ // Return the heading
+ *heading = target.pos.heading;
+
+ // If we have a velocity measurement try to integrate the x-y position when enabled
+ struct NedCoor_f vel = {0};
+ bool got_vel = target_get_vel(&vel);
+ if(target.integrate_xy && got_vel) {
+ pos->x = pos->x + vel.x * time_diff;
+ pos->y = pos->y + vel.y * time_diff;
+ }
+
+ if(target.integrate_z && got_vel) {
+ pos->z = pos->z + vel.z * time_diff;
+ }
+
+ // Offset the target
+ pos->x += target.offset.distance * cosf((*heading + target.offset.heading)/180.*M_PI);
+ pos->y += target.offset.distance * sinf((*heading + target.offset.heading)/180.*M_PI);
+ pos->z -= target.offset.height;
+
+ return true;
+ }
+
+ return false;
+}
+
+/**
+ * Get the current target velocity (NED)
+ */
+bool target_get_vel(struct NedCoor_f *vel) {
+
+ /* When we have a valid target_pos message, state ned is initialized and no timeout */
+ if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec()) {
+ // Calculate baed on ground speed and course
+ vel->x = target.pos.ground_speed * cosf(target.pos.course/180.*M_PI);
+ vel->y = target.pos.ground_speed * sinf(target.pos.course/180.*M_PI);
+ vel->z = -target.pos.climb;
+
+ return true;
+ }
+
+ return false;
+}
+
+/**
+ * Set the current measured distance and heading as offset
+ */
+bool target_pos_set_current_offset(float unk __attribute__((unused))) {
+ if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec()) {
+ struct NedCoor_i target_pos_cm;
+ struct NedCoor_f uav_pos = *stateGetPositionNed_f();
+
+ // Convert from LLA to NED using origin from the UAV
+ ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &target.pos.lla);
+
+ // Convert to floating point (cm to meters)
+ struct NedCoor_f pos;
+ pos.x = target_pos_cm.x * 0.01;
+ pos.y = target_pos_cm.y * 0.01;
+ pos.z = target_pos_cm.z * 0.01;
+
+ target.offset.distance = sqrtf(powf(uav_pos.x - pos.x, 2) + powf(uav_pos.y - pos.y, 2));
+ target.offset.height = -(uav_pos.z - pos.z);
+ target.offset.heading = atan2f((uav_pos.y - pos.y), (uav_pos.x - pos.x))*180.0/M_PI - target.pos.heading;
+ }
+
+ return false;
+}
diff --git a/sw/airborne/modules/ctrl/target_pos.h b/sw/airborne/modules/ctrl/target_pos.h
new file mode 100644
index 0000000000..412474f8e6
--- /dev/null
+++ b/sw/airborne/modules/ctrl/target_pos.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2021 Freek van Tienen
+ *
+ * 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/ctrl/target_pos.h"
+ * @author Freek van Tienen
+ * Create a target position derived from and RTK gps or TARGET_POS message
+ */
+
+#ifndef TARGET_POS_H
+#define TARGET_POS_H
+
+#include "std.h"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_geodetic_float.h"
+
+struct target_pos_t {
+ bool valid; ///< If the data of the target position is valid
+ uint32_t recv_time; ///< Time of when the target position message was received [msec]
+ uint32_t tow; ///< Time of week of the target position measurement
+ struct LlaCoor_i lla; ///< Lat, lon and altitude position of the target
+ float ground_speed; ///< Ground speed of the target [m/s]
+ float course; ///< Ground course of the target [deg]
+ float heading; ///< Heading of the target [deg]
+ float climb; ///< Climb speed, z-up [m/s]
+};
+
+struct target_offset_t {
+ float heading; ///< Target offset heading
+ float distance; ///< Target offset distance
+ float height; ///< Target offset height
+};
+
+struct target_t {
+ struct target_pos_t pos; ///< The target position message
+ struct target_offset_t offset; ///< The target offset relative to ground heading
+ uint32_t target_pos_timeout; ///< Ground target position message timeout [msec]
+ uint32_t rtk_timeout; ///< RTK message timeout [msec]
+ bool integrate_xy; ///< Enable integration of the position in X-Y (North/East) frame
+ bool integrate_z; ///< Enable integration of the position in Z (Up) frame
+ struct LlaCoor_i gps_lla; ///< GPS LLA position
+};
+
+extern struct target_t target;
+extern void target_pos_init(void);
+extern void target_parse_target_pos(uint8_t *buf);
+extern bool target_get_pos(struct NedCoor_f *pos, float *heading);
+extern bool target_get_vel(struct NedCoor_f *vel);
+extern bool target_pos_set_current_offset(float unk);
+
+#endif
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index e4e61bb7aa..50441e04cb 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit e4e61bb7aade61ac309d6b30d85416bd86af4236
+Subproject commit 50441e04cbd08ed27a38e03f141ed59fb4a9becf