Non parametric GVF rebuild (#2885)

conf/modules/gvf_module.xml

@@ -62,6 +62,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
 
    <header>
      <file name="gvf.h"/>
+     <file name="gvf_low_level_control.h"/>
      <file name="trajectories/gvf_line.h"/>
      <file name="trajectories/gvf_sin.h"/>
      <file name="trajectories/gvf_ellipse.h"/>
@@ -72,6 +73,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
 
    <makefile firmware="fixedwing">
      <file name="gvf.c"/>
+     <file name="gvf_low_level_control.c"/>
      <file name="trajectories/gvf_line.c"/>
      <file name="trajectories/gvf_sin.c"/>
      <file name="trajectories/gvf_ellipse.c"/>
@@ -80,6 +82,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
 
    <makefile firmware="rover">
      <file name="gvf.c"/>
+     <file name="gvf_low_level_control.c"/>
      <file name="trajectories/gvf_line.c"/>
      <file name="trajectories/gvf_sin.c"/>
      <file name="trajectories/gvf_ellipse.c"/>

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

@@ -24,28 +24,18 @@
 #include "std.h"
 
 #include "modules/guidance/gvf/gvf.h"
+#include "modules/guidance/gvf/gvf_low_level_control.h"
 #include "modules/guidance/gvf/trajectories/gvf_ellipse.h"
 #include "modules/guidance/gvf/trajectories/gvf_line.h"
 #include "modules/guidance/gvf/trajectories/gvf_sin.h"
-
-#ifdef FIXEDWING_FIRMWARE
-#include "firmwares/fixedwing/nav.h"
-#include "modules/nav/common_nav.h"
-#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-#elif defined(ROVER_FIRMWARE)
-#include "state.h"
-#include "modules/nav/nav_rover_base.h"
-#include "firmwares/rover/navigation.h"
-struct EnuCoor_f gvf_draw_wp;
-#else
-#error "Firmware not supported by GVF!"
-#endif
-
 #include "autopilot.h"
 
 // Control
 gvf_con gvf_control;
 
+// State
+gvf_st gvf_state;
+
 // Trajectory
 gvf_tra gvf_trajectory;
 gvf_seg gvf_segment;
@@ -165,19 +155,11 @@ void gvf_control_2D(float ke, float kn, float e,
                     struct gvf_grad *grad, struct gvf_Hess *hess)
 {
   gvf_t0 = get_sys_time_msec();
-
-  #if defined(FIXEDWING_FIRMWARE)
-    float ground_speed = stateGetHorizontalSpeedNorm_f();
-    float course = stateGetHorizontalSpeedDir_f();
-    float px_dot = ground_speed * sinf(course);
-    float py_dot = ground_speed * cosf(course);
-  #elif defined(ROVER_FIRMWARE)
-    // We assume that the course and psi
-    // of the rover (steering wheel) are the same
-    float course = stateGetNedToBodyEulers_f()->psi;
-    float px_dot = stateGetSpeedEnu_f()->x;
-    float py_dot = stateGetSpeedEnu_f()->y;
-  #endif
+  
+  gvf_low_level_getState();
+  float course = gvf_state.course;
+  float px_dot = gvf_state.px_dot;
+  float py_dot = gvf_state.py_dot;
 
   int s = gvf_control.s;
 
@@ -226,29 +208,9 @@ void gvf_control_2D(float ke, float kn, float e,
   float mr_y = cosf(course);
 
   float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
-
-  #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);
-  }
-
-  #elif defined(ROVER_FIRMWARE) 
-  if (autopilot_get_mode() != AP_MODE_DIRECT) {
-    guidance_control.gvf_omega = omega; //TODO: set omega into external GVF variable
-  }
-
-  #else
-  #error GVF does not support your firmware yet
-  #endif
+  
+  gvf_control.omega = omega;
+  gvf_low_level_control_2D(omega);
 }
 
 void gvf_set_direction(int8_t s)
@@ -256,10 +218,6 @@ void gvf_set_direction(int8_t s)
   gvf_control.s = s;
 }
 
-
-
-#ifdef FIXEDWING_FIRMWARE // FIXEDWING TRAJECTORIES
-
 // STRAIGHT LINE
 
 static void gvf_line(float a, float b, float heading)
@@ -279,7 +237,7 @@ static void gvf_line(float a, float b, float heading)
 
   gvf_control.error = e;
   
-  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
+  gvf_setNavMode(GVF_MODE_WAYPOINT);
   
   gvf_segment.seg = 0;
 }
@@ -298,7 +256,7 @@ bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
 
   gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
   
-  horizontal_mode = HORIZONTAL_MODE_ROUTE;
+  gvf_setNavMode(GVF_MODE_ROUTE);
   gvf_segment.seg = 1;
   gvf_segment.x1 = x1;
   gvf_segment.y1 = y1;
@@ -310,10 +268,10 @@ bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
 
 bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
 {
-  float x1 = waypoints[wp1].x;
-  float y1 = waypoints[wp1].y;
-  float x2 = waypoints[wp2].x;
-  float y2 = waypoints[wp2].y;
+  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);
 }
@@ -331,7 +289,7 @@ bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1,
 
   gvf_line(x1, y1, alpha);
 
-  horizontal_mode = HORIZONTAL_MODE_ROUTE;
+  gvf_setNavMode(GVF_MODE_ROUTE);
   
   gvf_segment.seg = 1;
   gvf_segment.x1 = x1;
@@ -344,10 +302,10 @@ bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1,
 
 bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
 {
-  float x1 = waypoints[wp1].x;
-  float y1 = waypoints[wp1].y;
-  float x2 = waypoints[wp2].x;
-  float y2 = waypoints[wp2].y;
+  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);
 }
@@ -376,10 +334,10 @@ bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
 
 bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
 {
-  float x1 = waypoints[wp1].x;
-  float y1 = waypoints[wp1].y;
-  float x2 = waypoints[wp2].x;
-  float y2 = waypoints[wp2].y;
+  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);
 }
@@ -388,8 +346,8 @@ bool gvf_line_wp_heading(uint8_t wp, float heading)
 {
   heading = RadOfDeg(heading);
 
-  float a = waypoints[wp].x;
-  float b = waypoints[wp].y;
+  float a = WaypointX(wp);
+  float b = WaypointY(wp);
 
   return gvf_line_XY_heading(a, b, heading);
 }
@@ -416,10 +374,10 @@ bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
   }
 
   if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
-    horizontal_mode = HORIZONTAL_MODE_CIRCLE;
+    gvf_setNavMode(GVF_MODE_CIRCLE);
 
   } else {
-    horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
+    gvf_setNavMode(GVF_MODE_WAYPOINT);
   }
 
   gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
@@ -434,8 +392,8 @@ bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
 
 
 bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
-{
-  gvf_ellipse_XY(waypoints[wp].x,  waypoints[wp].y, a, b, alpha);
+{  
+  gvf_ellipse_XY(WaypointX(wp),  WaypointY(wp), a, b, alpha);
   return true;
 }
 
@@ -468,10 +426,10 @@ bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
 {
   w = 2 * M_PI * w;
 
-  float x1 = waypoints[wp1].x;
-  float y1 = waypoints[wp1].y;
-  float x2 = waypoints[wp2].x;
-  float y2 = waypoints[wp2].y;
+  float x1 = WaypointX(wp1);
+  float y1 = WaypointY(wp1);
+  float x2 = WaypointX(wp2);
+  float y2 = WaypointY(wp2);
 
   float zx = x1 - x2;
   float zy = y1 - y2;
@@ -488,267 +446,11 @@ bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
   w = 2 * M_PI * w;
   alpha = RadOfDeg(alpha);
 
-  float x = waypoints[wp].x;
-  float y = waypoints[wp].y;
+  float x = WaypointX(wp);
+  float y = WaypointY(wp);
 
   gvf_sin_XY_alpha(x, y, alpha, w, off, A);
 
   return true;
 }
 
-
-#elif defined(ROVER_FIRMWARE) // ROVER TRAJECTORIES
-
-// 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_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;
-  
-  nav.mode = NAV_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)
-{
-  float zx = x2 - x1;
-  float zy = y2 - y1;
-
-  gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
-  
-  nav.mode = NAV_MODE_ROUTE;
-  
-  gvf_segment.seg = 1;
-  gvf_segment.x1 = x1;
-  gvf_segment.y1 = y1;
-  gvf_segment.x2 = x2;
-  gvf_segment.y2 = y2;
-
-  // Send rover_base navigation data to draw segment (ROUTE)
-  nav_rover_base.goto_wp.from.x = x1;
-  nav_rover_base.goto_wp.from.y = y1;
-  nav_rover_base.goto_wp.to.x = x2;
-  nav_rover_base.goto_wp.to.y = y2;
-
-  return true;
-}
-
-bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
-{
-  float x1 = waypoint_get_x(wp1);
-  float y1 = waypoint_get_y(wp1);
-  float x2 = waypoint_get_x(wp2);
-  float y2 = waypoint_get_y(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_control.s = s;
-  }
-
-  float zx = x2 - x1;
-  float zy = y2 - y1;
-  float alpha = atanf(zx / zy);
-
-  gvf_line(x1, y1, alpha);
-
-  nav.mode = NAV_MODE_ROUTE;
-  
-  gvf_segment.seg = 1;
-  gvf_segment.x1 = x1;
-  gvf_segment.y1 = y1;
-  gvf_segment.x2 = x2;
-  gvf_segment.y2 = y2;
-
-  // Send rover_base navigation data to draw segment (ROUTE)
-  nav_rover_base.goto_wp.from.x = x1;
-  nav_rover_base.goto_wp.from.y = y1;
-  nav_rover_base.goto_wp.to.x = x2;
-  nav_rover_base.goto_wp.to.y = y2;
-
-  return true;
-}
-
-bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
-{
-  float x1 = waypoint_get_x(wp1);
-  float y1 = waypoint_get_y(wp1);
-  float x2 = waypoint_get_x(wp2);
-  float y2 = waypoint_get_y(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)
-{
-  float x1 = waypoint_get_x(wp1);
-  float y1 = waypoint_get_y(wp1);
-  float x2 = waypoint_get_x(wp2);
-  float y2 = waypoint_get_y(wp2);
-
-  return gvf_segment_XY1_XY2(x1, y1, x2, y2);
-}
-
-bool gvf_line_wp_heading(uint8_t wp, float heading)
-{
-  heading = RadOfDeg(heading);
-
-  float a = waypoint_get_x(wp);
-  float b = waypoint_get_y(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;
-
-  // SAFE MODE
-  if (a < 1 || b < 1) {
-    gvf_trajectory.p[2] = 60;
-    gvf_trajectory.p[3] = 60;
-  }
-
-  // Send rover_base navigation data to draw circle (CIRCLE)
-  if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
-    nav.mode = NAV_MODE_CIRCLE;
-    nav_rover_base.circle.center.x = x;
-    nav_rover_base.circle.center.y = y;
-    nav_rover_base.circle.radius = a;
-  } else {
-    nav.mode = NAV_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_ellipse_XY(waypoint_get_x(wp),  waypoint_get_y(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_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;
-
-  float x1 = waypoint_get_x(wp1);
-  float y1 = waypoint_get_y(wp1);
-  float x2 = waypoint_get_x(wp2);
-  float y2 = waypoint_get_y(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);
-
-  float x = waypoint_get_x(wp);
-  float y = waypoint_get_y(wp);
-
-  gvf_sin_XY_alpha(x, y, alpha, w, off, A);
-
-  return true;
-}
-#endif
-

sw/airborne/modules/guidance/gvf/gvf.h

@@ -43,11 +43,20 @@ typedef struct {
   float ke;
   float kn;
   float error;
+  float omega;
   int8_t s;
 } gvf_con;
 
 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,

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

@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2020 Hector Garcia de Marina <hgarciad@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/>.
+ */
+
+/**
+ * @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(course);
+    gvf_state.py_dot = ground_speed * cosf(course);
+    
+  #elif defined(ROVER_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
+
+}
+

sw/airborne/modules/guidance/gvf/gvf_low_level_control.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2020 Hector Garcia de Marina <hgarciad@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/>.
+ */
+
+/**
+ * @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
+
+#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
+