Support for rovers and third order Bézier splines in GVF parametric (#3145)

conf/airframes/UCM/rover_steering.xml

@@ -50,6 +50,10 @@
     <module name="gvf"  type="module">
       <define name="GVF_OCAML_GCS" value="FALSE"/>
     </module>
+    
+    <module name="gvf_parametric">
+      <define name="GVF_PARAMETRIC_2D_BEZIER_N_SEG" value="4"/>
+    </module>
 
     <module name="guidance" type="rover_steering">
       <define name="MAX_DELTA"            value="15.0"/>

conf/autopilot/rover_steering.xml

@@ -11,7 +11,7 @@
       <include name="modules/gps/gps.h"/>
       <include name="modules/actuators/actuators.h"/>
       <include name="modules/radio_control/radio_control.h"/>
-      <include name="modules/guidance/gvf/gvf.h"/>
+      <include name="modules/guidance/gvf_common.h"/>
       <include name="navigation.h"/>
       <include name="state.h"/>
       <define  name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
@@ -78,7 +78,7 @@
       </control>
       <control>
         <call fun="rover_guidance_steering_speed_ctrl()"/>
-        <call fun="rover_guidance_steering_heading_ctrl(gvf_control.omega)"/>
+        <call fun="rover_guidance_steering_heading_ctrl(gvf_c_omega.omega)"/>
 
         <call fun="commands[COMMAND_STEERING] = GetCmdFromDelta(guidance_control.cmd.delta);"/>
         <call fun="commands[COMMAND_THROTTLE] = GetCmdFromThrottle(guidance_control.throttle);"/>

conf/flight_plans/UCM/fixedwing_gvfMission.xml

@@ -11,7 +11,7 @@
     <waypoint alt="700.0" name="CLIMB"   x="-30" y="-40"/>
     <waypoint alt="700.0" name="P1"      x="-20" y="-40"/>
     <waypoint alt="700.0" name="P2"      x="20" y="-40"/>
-
+    
     <!-- Sectors waypoints -->
     <waypoint name="_S1" x="3" y="4"/>
     <waypoint name="_S2" x="3" y="-4"/>
@@ -106,9 +106,9 @@
     <block name="2D_trefoil_wp" strip_icon="eight.png"> 
       <call fun="gvf_parametric_2D_trefoil_wp(WP_P1, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
     </block>
-
+    
     <block name="3D_ellipse_wp" strip_icon="oval.png">
-	<call fun="gvf_parametric_3D_ellipse_wp(WP_ELLIPSE, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
+	  <call fun="gvf_parametric_3D_ellipse_wp(WP_ELLIPSE, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
     </block>
 
     <block name="3D_lissajous_wp" strip_icon="eight.png"> 

conf/flight_plans/UCM/steering_rover_gvfMission.xml

@@ -1,5 +1,5 @@
+<?xml version="1.0"?>
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
-
 <flight_plan alt="660" ground_alt="650" lat0="40.450631" lon0="-3.726058" max_dist_from_home="1500" name="Rover Steering" security_height="0.3">
   <header>
 
@@ -7,9 +7,9 @@
   <waypoints>
     <waypoint name="HOME" x="0.0" y="0.0"/>
     <waypoint name="STDBY" x="0" y="-4"/>
-    <waypoint name="ELLIPSE" x="-3" y="-3"/>
+    <waypoint name="ELLIPSE" x="-6.116" y="-8.525"/>
-    <waypoint name="P1" x="-2" y="-4"/>
+    <waypoint name="P1" x="-4.39" y="-0.738"/>
-    <waypoint name="P2" x="2" y="-4"/>
+    <waypoint name="P2" x="-7.046" y="-4.57"/>
     <!--waypoint name="OBJ" x="2" y="-4"/-->
     
     <!-- Sectors waypoints -->
@@ -21,8 +21,25 @@
     <waypoint name="_N2" x="100" y="-100"/>
     <waypoint name="_N3" x="-100" y="-100"/>
     <waypoint name="_N4" x="-100" y="100"/>
+    
+    <!-- Bézier Waypoints. Define them in order -->
+    <waypoint name="BZ0" x="1.7" y="-6.0"/>
+    <waypoint name="BZ1" x="-1.4" y="-14.7"/>
+    <waypoint name="BZ2" x="-0.688" y="-24.723"/>
+    <waypoint name="BZ3" x="6.3" y="-26.2"/>
+    <waypoint name="BZ4" x="16.9" y="-30.7"/>
+    <waypoint name="BZ5" x="25.6" y="-30.0"/>
+    <waypoint name="BZ6" x="31.0" y="-22.6"/>
+    <waypoint name="BZ7" x="33.4" y="-14.7"/>
+    <waypoint name="BZ8" x="32.3" y="-7.0"/>
+    <waypoint name="BZ9" x="27.3" y="-2.0"/>
+    <waypoint name="BZ10" x="19.7" y="0.6"/>
+    <waypoint name="BZ11" x="12.6" y="1.6"/>
+    <waypoint name="BZ12" x="8.373" y="-2.597"/>
   </waypoints>
   
+  
+  
   <sectors>
     <sector name="Net" color="red">
       <corner name="_N1"/>
@@ -90,5 +107,11 @@
       <exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
     </block>
     
+    <block name="2D_bezier" strip_icon="eight.png">
+      <call fun="gvf_parametric_2D_bezier_wp(WP_BZ0)"/>
+      <exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
+    </block>
+    
+    
   </blocks>
 </flight_plan>

conf/modules/gvf_common.xml

@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="gvf_common" dir="guidance/">
+  <doc>
+    <description> 
+    	Common file to allow both gvf to work together.
+    	Still requires at least one module providing the actual gvf implementation
+    </description>
+  </doc>
+  
+  <header>
+     <file name="gvf_common.h"/>
+  </header>
+   
+  <makefile firmware="fixedwing">
+     <file name="gvf_common.c"/>
+  </makefile>
+   
+  <makefile firmware="rover">
+     <file name="gvf_common.c"/>
+  </makefile>
+  
+</module>
+

conf/modules/gvf_module.xml

@@ -59,7 +59,11 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
       </dl_settings>
     </dl_settings>
   </settings>
-
+  
+  <dep>
+    <depends>gvf_common</depends>
+  </dep>
+  
    <header>
      <file name="gvf.h"/>
      <file name="gvf_low_level_control.h"/>

conf/modules/gvf_parametric.xml

@@ -29,6 +29,10 @@
           <dl_setting MAX="200" MIN="10" STEP="5" VAR="gvf_parametric_2d_trefoil_par.r" shortname="2d_tre_r" param="GVF_PARAMETRIC_2D_TREFOIL_R"/>
           <dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_2d_trefoil_par.alpha" shortname="2d_tre_alpha" param="GVF_PARAMETRIC_2D_TREFOIL_ALPHA"/>
         </dl_settings>
+        <dl_settings NAME="2D_bezier">
+          <dl_setting MAX="9.750" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_2d_bezier_par.kx" shortname="2d_bezier_kx" param="GVF_PARAMETRIC_2D_BEZIER_KX"/>
+          <dl_setting MAX="9.750" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_2d_bezier_par.ky" shortname="2d_bezier_ky" param="GVF_PARAMETRIC_2D_BEZIER_KY"/>
+        </dl_settings>   
         <dl_settings NAME="3D_ellipse">
           <dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_ellipse_par.kx" shortname="3d_ell_kx" param="GVF_PARAMETRIC_3D_ELLIPSE_KX"/>
           <dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_ellipse_par.ky" shortname="3d_ell_ky" param="GVF_PARAMETRIC_3D_ELLIPSE_KY"/>
@@ -53,16 +57,22 @@
           <dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_lissajous_par.dz" shortname="3d_lis_dz" param="GVF_PARAMETRIC_3D_LISSAJOUS_DZ"/>
           <dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_lissajous_par.alpha" shortname="3d_lis_alpha" param="GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA"/>
         </dl_settings>
+        
       </dl_settings>
     </dl_settings>
   </settings>
-
+  
+  <dep>
+    <depends>gvf_common</depends>
+  </dep>
+  
   <header>
     <file name="gvf_parametric.h"/>
     <file name="gvf_parametric_low_level_control.h"/>
     <file name="trajectories/gvf_parametric_3d_ellipse.h"/>
     <file name="trajectories/gvf_parametric_3d_lissajous.h"/>
     <file name="trajectories/gvf_parametric_2d_trefoil.h"/>
+    <file name="trajectories/gvf_parametric_2d_bezier_splines.h"/> 
   </header>
 
   <init fun = "gvf_parametric_init()"/>
@@ -78,10 +88,30 @@
     <file name="trajectories/gvf_parametric_3d_ellipse.c"/>
     <file name="trajectories/gvf_parametric_3d_lissajous.c"/>
     <file name="trajectories/gvf_parametric_2d_trefoil.c"/>
+    <file name="trajectories/gvf_parametric_2d_bezier_splines.c"/>
+    <flag name="LDFLAGS" value="lstdc++" />
+  </makefile>
+
+  <makefile target="ap|nps" firmware="rover">
+    <configure name="CXXSTANDARD" value="-std=c++14"/>
+    <flag name="CXXFLAGS" value="Wno-int-in-bool-context -Wno-deprecated-copy"/>
+    <include name="$(PAPARAZZI_SRC)/sw/ext/eigen"/>
+    <define name="EIGEN_NO_MALLOC"/>
+    <define name="EIGEN_NO_AUTOMATIC_RESIZING"/>
+    <file name="gvf_parametric.cpp"/>
+    <file name="gvf_parametric_low_level_control.c"/>
+    <file name="trajectories/gvf_parametric_3d_ellipse.c"/>
+    <file name="trajectories/gvf_parametric_3d_lissajous.c"/>
+    <file name="trajectories/gvf_parametric_2d_trefoil.c"/>
+    <file name="trajectories/gvf_parametric_2d_bezier_splines.c"/>
     <flag name="LDFLAGS" value="lstdc++" />
   </makefile>
 
   <makefile target="ap" firmware="fixedwing">
     <define name="EIGEN_NO_DEBUG"/>
   </makefile>
+
+  <makefile target="ap" firmware="rover">
+    <define name="EIGEN_NO_DEBUG"/>
+  </makefile>
 </module>

conf/telemetry/GVF/gvf_default_fixedwing.xml

@@ -29,7 +29,7 @@
       <message name="AIR_DATA"            period="1.3"/>
       <message name="VECTORNAV_INFO"      period="0.5"/>
       <message name="GVF"                 period="3."/>
-      <message name="GVF_PARAMETRIC"      period="3."/>
+      <message name="GVF_PARAMETRIC"      period="0.5"/>
     </mode>
     <mode name="minimal">
       <message name="ALIVE"               period="5"/>

conf/telemetry/GVF/gvf_rover.xml

@@ -25,6 +25,7 @@
       <message name="I2C_ERRORS"             period="4.1"/>
       <message name="UART_ERRORS"            period="3.1"/>
       <message name="GVF"                    period="1.0"/>
+      <message name="GVF_PARAMETRIC"         period="0.5"/>
     </mode>
 
     <mode name="low" key_press="l">

conf/userconf/GVF/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/gps.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_module.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"
   />
   <aircraft
@@ -18,7 +18,7 @@
    telemetry="telemetry/GVF/gvf_rover.xml"
    flight_plan="flight_plans/UCM/steering_rover_gvfMission.xml"
    settings=""
-   settings_modules="modules/ahrs_float_dcm.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rover_steering.xml modules/gvf_module.xml~GVF~ modules/imu_common.xml modules/nav_rover_base.xml"
+   settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_module.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
    gui_color="blue"
   />
 </conf>

sw/airborne/modules/guidance/gvf/gvf.c

@@ -29,6 +29,7 @@
 #include "modules/guidance/gvf/trajectories/gvf_line.h"
 #include "modules/guidance/gvf/trajectories/gvf_sin.h"
 #include "autopilot.h"
+#include "../gvf_common.h"
 
 // Control
 gvf_con gvf_control;
@@ -183,6 +184,11 @@ void gvf_control_2D(float ke, float kn, float e,
   float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
   
   gvf_control.omega = omega;
+  
+  // From gvf_common.h TODO: derivative of curvature and ori_err
+  gvf_c_omega.omega  = omega; 
+  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_low_level_control_2D(omega);
 }
 

sw/airborne/modules/guidance/gvf_common.c

@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) 2023 Alfredo Gonzalez Calvin <alfredgo@ucm.es>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+#include "./gvf_common.h"
+
+gvf_common_omega  gvf_c_omega;
+gvf_common_params gvf_c_info;

sw/airborne/modules/guidance/gvf_common.h

@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2023 Alfredo Gonzalez Calvin <alfredgo@ucm.es>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef GVF_COMMON_H
+#define GVF_COMMON_H
+
+// uint32_t
+#include "std.h"
+
+/** @typedef gvf_common_omega
+* @brief Horizontal control signal for both gvf
+* @param omega is the horizontal control signal
+*/
+
+typedef struct {
+  float omega;
+} gvf_common_omega;
+
+extern gvf_common_omega gvf_c_omega;
+
+/** @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 struct {
+  float kappa;
+  float kappa_dot;
+  float ori_err;
+  float ori_err_dot;
+} gvf_common_params;
+
+extern gvf_common_params gvf_c_info;
+
+#endif // GVF_COMMON_H
+

sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp

@@ -32,6 +32,8 @@
 #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"
 
 #ifdef __cplusplus
 extern "C" {
@@ -41,6 +43,7 @@ extern "C" {
 
 // 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;
 
 // Trajectory
@@ -53,6 +56,9 @@ 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];
+
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
 static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
@@ -65,6 +71,7 @@ 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);
   }
@@ -128,8 +135,10 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
   }
 
   // Carrot position
+#ifdef FIXEDWING_FIRMWARE
   desired_x = f1;
   desired_y = f2;
+#endif
 
   float L = gvf_parametric_control.L;
   float beta = gvf_parametric_control.beta * gvf_parametric_control.s;
@@ -202,6 +211,11 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
   float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
                        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_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));
+
   // Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
   // 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;
@@ -209,6 +223,7 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
   gvf_parametric_low_level_control_2D(heading_rate);
 }
 
+#ifdef FIXEDWING_FIRMWARE
 void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2, float f3, float f1d, float f2d,
                                float f3d, float f1dd, float f2dd, float f3dd)
 {
@@ -315,6 +330,7 @@ void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2,
 
   gvf_parametric_low_level_control_3D(heading_rate, climbing_rate);
 }
+#endif // FIXED_WING FIRMWARE
 
 /** 2D TRAJECTORIES **/
 // 2D TREFOIL KNOT
@@ -342,17 +358,77 @@ bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float
 }
 
 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;
+}
 
-  gvf_parametric_2D_trefoil_XY(waypoints[wp].x, waypoints[wp].y, w1, w2, ratio, r, alpha);
+// 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
@@ -389,7 +465,7 @@ bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float
 }
 
 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;
 
@@ -445,10 +521,11 @@ bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, flo
 
 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

sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h

@@ -66,6 +66,7 @@ extern "C" {
 #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"
 
 /** @typedef gvf_parametric_con
 * @brief Control parameters for the GVF_PARAMETRIC
@@ -93,6 +94,7 @@ enum trajectories_parametric {
   TREFOIL_2D = 0,
   ELLIPSE_3D = 1,
   LISSAJOUS_3D = 2,
+  BEZIER_2D = 3,
   NONE_PARAMETRIC = 255,
 };
 
@@ -104,6 +106,9 @@ typedef struct {
 
 extern gvf_parametric_tra gvf_parametric_trajectory;
 
+// Bezier struct
+extern bezier_t gvf_bezier_2D[GVF_PARAMETRIC_2D_BEZIER_N_SEG];
+
 // Init function
 extern void gvf_parametric_init(void);
 
@@ -117,6 +122,10 @@ extern void gvf_parametric_control_3D(float, float, float, float, float, float,
 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);

sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c

@@ -47,6 +47,8 @@ void gvf_parametric_low_level_control_2D(float heading_rate)
       -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
@@ -72,6 +74,8 @@ void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate
       -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

sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.c

@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) 2023 Alfredo Gonzalez Calvin <alfredgo@ucm.es>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+#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);
+
+
+}

sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2023 Alfredo Gonzalez Calvin <alfredgo@ucm.es>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+#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
+
+
+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);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+
+
+#endif // bezier splines