Gvf for rotorcrafts (#3197)

* Core GVF changes
* Update GVF module conf file for rotorcrafts
* Integration of acceleration in guidance rotorcraft firmware
* Update command chain in guidance for ACCEL command integration
* [guidance] select guidance sp mode with enum instead of bitmask
* [guidance] reset ref if input type has changed

---------

Co-authored-by: Mael FEURGARD <mael.feurgard@laas.fr>
Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>

conf/modules/gvf_module.xml

@@ -83,6 +83,14 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
      <file name="trajectories/gvf_ellipse.c"/>
      <file name="nav/nav_survey_polygon_gvf.c"/>
    </makefile>
+   
+   <makefile firmware="rotorcraft">
+     <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"/>
+   </makefile>
 
    <makefile firmware="rover">
      <file name="gvf.c"/>
@@ -94,4 +102,3 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
    </makefile>
 
 </module>
-

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c

@@ -114,6 +114,8 @@ void guidance_h_init(void)
   FLOAT_EULERS_ZERO(guidance_h.rc_sp);
   guidance_h.sp.heading = 0.0;
   guidance_h.sp.heading_rate = 0.0;
+  guidance_h.sp.h_mask = GUIDANCE_H_SP_POS;
+  guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW;
   transition_percentage = 0;
   transition_theta_offset = 0;
 
@@ -252,7 +254,7 @@ void guidance_h_read_rc(bool  in_flight)
 #if GUIDANCE_H_USE_SPEED_REF
       read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
       /* enable x,y velocity setpoints */
-      SetBit(guidance_h.sp.mask, 5);
+      guidance_h.sp.h_mask = GUIDANCE_H_SP_SPEED;
 #endif
       break;
 
@@ -345,7 +347,13 @@ static void guidance_h_update_reference(void)
 {
   /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
 #if GUIDANCE_H_USE_REF
-  if (bit_is_set(guidance_h.sp.mask, 5)) {
+  if (guidance_h.sp.h_mask == GUIDANCE_H_SP_ACCEL) {
+    struct FloatVect2 sp_accel;
+    sp_accel.x = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.x);
+    sp_accel.y = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.y);
+    gh_update_ref_from_accel_sp(sp_accel);
+  }
+  else if (guidance_h.sp.h_mask == GUIDANCE_H_SP_SPEED) {
     struct FloatVect2 sp_speed;
     sp_speed.x = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.x);
     sp_speed.y = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.y);
@@ -364,18 +372,32 @@ static void guidance_h_update_reference(void)
     guidance_h.ref.accel.x = ACCEL_BFP_OF_REAL(gh_ref.accel.x);
     guidance_h.ref.accel.y = ACCEL_BFP_OF_REAL(gh_ref.accel.y);
   } else {
-    if (nav.setpoint_mode == NAV_SETPOINT_MODE_POS) {
-      VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
-      INT_VECT2_ZERO(guidance_h.ref.speed);
-      INT_VECT2_ZERO(guidance_h.ref.accel);
-    } else { //(nav.setpoint_mode == NAV_SETPOINT_MODE_SPEED)
-      guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
-      guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
-      guidance_h.ref.speed.x = guidance_h.sp.speed.x;
-      guidance_h.ref.speed.y = guidance_h.sp.speed.y;
-      guidance_h.ref.accel.x = 0;
-      guidance_h.ref.accel.y = 0;
-    } // TODO: make accel ref set
+    switch (nav.setpoint_mode) {
+      case NAV_SETPOINT_MODE_SPEED:
+        guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
+        guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
+        guidance_h.ref.speed.x = guidance_h.sp.speed.x;
+        guidance_h.ref.speed.y = guidance_h.sp.speed.y;
+        guidance_h.ref.accel.x = 0;
+        guidance_h.ref.accel.y = 0;
+        break;
+
+      case NAV_SETPOINT_MODE_ACCEL:
+        guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
+        guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
+        guidance_h.ref.speed.x = stateGetSpeedNed_i()->x;
+        guidance_h.ref.speed.y = stateGetSpeedNed_i()->y;
+        guidance_h.ref.accel.x = guidance_h.sp.accel.x;
+        guidance_h.ref.accel.y = guidance_h.sp.accel.y;
+        break;
+
+      case NAV_SETPOINT_MODE_POS:
+      default: // Fallback is guidance by pos
+        VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
+        INT_VECT2_ZERO(guidance_h.ref.speed);
+        INT_VECT2_ZERO(guidance_h.ref.accel);
+        break;
+    }
   }
 
 #if GUIDANCE_H_USE_SPEED_REF
@@ -385,7 +407,7 @@ static void guidance_h_update_reference(void)
 #endif
 
   /* update heading setpoint from rate */
-  if (bit_is_set(guidance_h.sp.mask, 7)) {
+  if (guidance_h.sp.yaw_mask == GUIDANCE_H_SP_YAW_RATE) {
     guidance_h.sp.heading += guidance_h.sp.heading_rate / PERIODIC_FREQUENCY;
     FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
   }
@@ -478,7 +500,8 @@ void guidance_h_from_nav(bool in_flight)
         break;
 
       case NAV_SETPOINT_MODE_ACCEL:
-        // TODO set_accel ref
+        guidance_h_set_acc(nav.accel.y, nav.accel.x); // nav acc is in ENU frame, convert to NED
+        guidance_h_update_reference();
         guidance_h_set_heading(nav.heading);
         guidance_h_cmd = guidance_h_run_accel(in_flight, &guidance_h);
         break;
@@ -559,14 +582,17 @@ void guidance_h_guided_run(bool in_flight)
 
 void guidance_h_set_pos(float x, float y)
 {
-  ClearBit(guidance_h.sp.mask, 5);
+  if (guidance_h.sp.h_mask != GUIDANCE_H_SP_POS) {
+    reset_guidance_reference_from_current_position();
+  }
+  guidance_h.sp.h_mask = GUIDANCE_H_SP_POS;
   guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
   guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
 }
 
 void guidance_h_set_heading(float heading)
 {
-  ClearBit(guidance_h.sp.mask, 7);
+  guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW;
   guidance_h.sp.heading = heading;
   FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
 }
@@ -581,14 +607,35 @@ void guidance_h_set_body_vel(float vx, float vy)
 
 void guidance_h_set_vel(float vx, float vy)
 {
-  SetBit(guidance_h.sp.mask, 5);
+  if (guidance_h.sp.h_mask != GUIDANCE_H_SP_SPEED) {
+    reset_guidance_reference_from_current_position();
+  }
+  guidance_h.sp.h_mask = GUIDANCE_H_SP_SPEED;
   guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx);
   guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy);
 }
 
+void guidance_h_set_body_acc(float ax, float ay)
+{
+  float psi = stateGetNedToBodyEulers_f()->psi;
+  float newax =  cosf(-psi) * ax + sinf(-psi) * ay;
+  float neway = -sinf(-psi) * ax + cosf(-psi) * ay;
+  guidance_h_set_acc(newax, neway);
+}
+
+void guidance_h_set_acc(float ax, float ay)
+{
+  if (guidance_h.sp.h_mask != GUIDANCE_H_SP_ACCEL) {
+    reset_guidance_reference_from_current_position();
+  }
+  guidance_h.sp.h_mask = GUIDANCE_H_SP_ACCEL;
+  guidance_h.sp.accel.x = ACCEL_BFP_OF_REAL(ax);
+  guidance_h.sp.accel.y = ACCEL_BFP_OF_REAL(ay);
+}
+
 void guidance_h_set_heading_rate(float rate)
 {
-  SetBit(guidance_h.sp.mask, 7);
+  guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW_RATE;
   guidance_h.sp.heading_rate = rate;
 }
 

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h

@@ -79,9 +79,20 @@ struct HorizontalGuidanceSetpoint {
    */
   struct Int32Vect2 pos;
   struct Int32Vect2 speed;  ///< only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
+  struct Int32Vect2 accel;  ///< For direct control of acceleration, if the guidance scheme is able to provide it
+
   float heading;
   float heading_rate;
-  uint8_t mask;             ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw
+
+  enum {
+    GUIDANCE_H_SP_POS   = 0,
+    GUIDANCE_H_SP_SPEED = 1,
+    GUIDANCE_H_SP_ACCEL = 2
+  } h_mask;
+  enum {
+    GUIDANCE_H_SP_YAW       = 0,
+    GUIDANCE_H_SP_YAW_RATE  = 1
+  } yaw_mask;
 };
 
 struct HorizontalGuidanceReference {
@@ -146,6 +157,18 @@ extern void guidance_h_set_body_vel(float vx, float vy);
  * @param vx North velocity (local NED frame) in meters/sec.
  * @param vy East velocity (local NED frame) in meters/sec.
  */
+extern void guidance_h_set_acc(float ax, float ay);
+
+/** Set body relative horizontal acceleration setpoint.
+ * @param vx forward acceleration (body frame) in meters/sec².
+ * @param vy right acceleration (body frame) in meters/sec².
+ */
+extern void guidance_h_set_body_acc(float ax, float ay);
+
+/** Set horizontal acceleration setpoint.
+ * @param vx North acceleration (local NED frame) in meters/sec².
+ * @param vy East acceleration (local NED frame) in meters/sec².
+ */
 extern void guidance_h_set_vel(float vx, float vy);
 
 /** Set heading rate setpoint.

sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c

@@ -167,6 +167,26 @@ void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
   VECT2_COPY(gh_ref.accel, accel_sp);
 }
 
+void gh_update_ref_from_accel_sp(struct FloatVect2 accel_sp)
+{
+  struct FloatVect2 pos_step, speed_step;
+
+  VECT2_SMUL(pos_step, gh_ref.speed, gh_ref.dt);
+  VECT2_SMUL(speed_step, gh_ref.accel, gh_ref.dt);
+
+  struct Int64Vect2 pos_update;
+  pos_update.x = LBFP_OF_REAL(pos_step.x, GH_POS_REF_FRAC);
+  pos_update.y = LBFP_OF_REAL(pos_step.y, GH_POS_REF_FRAC);
+
+  VECT2_ADD(gh_ref.pos, pos_update);
+  VECT2_ADD(gh_ref.speed, speed_step);
+
+  gh_saturate_accel(&accel_sp);
+
+  // copy accel
+  VECT2_COPY(gh_ref.accel, accel_sp);
+}
+
 static void gh_saturate_speed(struct FloatVect2 *speed_sp)
 {
   // Speed squared

sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h

@@ -102,6 +102,7 @@ extern void gh_ref_init(void);
 extern void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel);
 extern void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
 extern void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp);
+extern void gh_update_ref_from_accel_sp(struct FloatVect2 accel_sp);
 
 /**
  * Set a new maximum speed for waypoint navigation.

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c

@@ -316,33 +316,49 @@ struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struc
   struct FloatVect3 pos_err = { 0 };
   struct FloatVect3 accel_sp = { 0 };
 
-  if (h_mode == GUIDANCE_INDI_H_POS) {
-    pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
-    pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
-    speed_sp.x = pos_err.x * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
-    speed_sp.y = pos_err.y * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
+  struct FloatVect3 speed_fb;
+
+
+  if (h_mode == GUIDANCE_INDI_H_ACCEL) {
+    // Speed feedback is included in the guidance when running in ACCEL mode
+    speed_fb.x = 0.;
+    speed_fb.y = 0.;
   }
-  else if (h_mode == GUIDANCE_INDI_H_SPEED) {
-    speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
-    speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
-  }
-  else { // H_ACCEL
-    speed_sp.x = 0.f;
-    speed_sp.y = 0.f;
+  else {
+    // Generate speed feedback for acceleration, as it is estimated
+    if (h_mode == GUIDANCE_INDI_H_SPEED) {
+      speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
+      speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
+    }
+    else { // H_POS
+      pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
+      pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
+      speed_sp.x = pos_err.x * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
+      speed_sp.y = pos_err.y * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
+    }
+    speed_fb.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain;
+    speed_fb.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain;
   }
 
-  if (v_mode == GUIDANCE_INDI_V_POS) {
-    pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
-    speed_sp.z = pos_err.z * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gv->zd_ref);
-  } else if (v_mode == GUIDANCE_INDI_V_SPEED) {
-    speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
-  } else { // V_ACCEL
-    speed_sp.z = 0.f;
+  if (v_mode == GUIDANCE_INDI_V_ACCEL)  {
+    // Speed feedback is included in the guidance when running in ACCEL mode
+    speed_fb.z = 0;
+  }
+  else {
+    // Generate speed feedback for acceleration, as it is estimated
+    if (v_mode == GUIDANCE_INDI_V_SPEED) {
+      speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
+    }
+    else { // V_POS
+      pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
+      speed_sp.z = pos_err.z * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gv->zd_ref);
+    }
+    speed_fb.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
   }
 
-  accel_sp.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
-  accel_sp.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
-  accel_sp.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
+  accel_sp.x = speed_fb.x + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
+  accel_sp.y = speed_fb.y + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
+  accel_sp.z = speed_fb.z + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
 
   return guidance_indi_run(&accel_sp, gh->sp.heading);
 }

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

@@ -23,14 +23,15 @@
 #include <math.h>
 #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"
+#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;
 
@@ -129,7 +130,7 @@ void gvf_control_2D(float ke, float kn, float e,
                     struct gvf_grad *grad, struct gvf_Hess *hess)
 {
   gvf_t0 = get_sys_time_msec();
-  
+
   gvf_low_level_getState();
   float course = gvf_state.course;
   float px_dot = gvf_state.px_dot;
@@ -155,10 +156,6 @@ void gvf_control_2D(float ke, float kn, float e,
   float pdx_dot = tx - ke * e * nx;
   float pdy_dot = ty - ke * e * ny;
 
-  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 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;
 
@@ -170,26 +167,67 @@ void gvf_control_2D(float ke, float kn, float e,
   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;
 
   float md_dot_x =  md_y * md_dot_const;
   float md_dot_y = -md_x * md_dot_const;
 
+
+  #ifdef ROTORCRAFT_FIRMWARE
+
+  // Set nav for command
+
+  // Use parameter kn as the speed command
+  nav.speed.x = md_x * kn;
+  nav.speed.y = md_y * kn;
+
+
+  // Acceleration induced by the field with speed set to kn (!WIP!)
+#warning "Using GVF for rotorcraft is still experimental, proceed with caution"
+  float n_norm = sqrtf(nx*nx+ny*ny);
+  float hess_px_dot = px_dot * H11 + py_dot * H12;
+  float hess_py_dot = px_dot * H21 + py_dot * H22;
+
+  float hess_pdx_dot = pdx_dot * H11 + pdy_dot * H12;
+  float hess_pdy_dot = pdx_dot * H21 + pdy_dot * H22;
+
+  float curvature_correction = tx * hess_px_dot + ty * hess_py_dot / (n_norm * n_norm);
+  float accel_correction_x = kn * hess_py_dot / n_norm;
+  float accel_correction_y = - kn * hess_px_dot / n_norm;
+  float accel_cmd_x = accel_correction_x + px_dot * curvature_correction;
+  float accel_cmd_y = accel_correction_y + py_dot * curvature_correction;
+
+  float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm);
+  float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm);
+
+  // TODO don't change nav struct directly
+  nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot);
+  nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
+  nav.heading = atan2f(md_x,md_y);
+
+  #else
+
   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);
-  
+
   gvf_control.omega = omega;
-  
+
   // From gvf_common.h TODO: derivative of curvature and ori_err
-  gvf_c_omega.omega  = omega; 
+  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);
+
+  #endif
 }
 
 void gvf_set_direction(int8_t s)
@@ -217,9 +255,9 @@ static void gvf_line(float a, float b, float heading)
   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;
 }
 
@@ -231,7 +269,7 @@ bool gvf_line_XY_heading(float a, float b, float heading)
 }
 
 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;
@@ -243,7 +281,7 @@ bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
   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;
@@ -259,7 +297,7 @@ 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);
@@ -282,7 +320,7 @@ bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1,
   gvf_line(x1, y1, alpha);
 
   gvf_setNavMode(GVF_MODE_ROUTE);
-  
+
   gvf_segment.seg = 1;
   gvf_segment.x1 = x1;
   gvf_segment.y1 = y1;
@@ -293,7 +331,7 @@ 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)
-{ 
+{
   gvf_trajectory.p[3] = wp1;
   gvf_trajectory.p[4] = wp2;
   gvf_trajectory.p[5] = d1;
@@ -309,7 +347,7 @@ bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float 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;
@@ -399,7 +437,7 @@ 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_trajectory.p[5] = wp;
   gvf_plen_wps = 1;
 

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

@@ -34,13 +34,13 @@
 
 void gvf_low_level_getState(void)
 {
-  #if defined(FIXEDWING_FIRMWARE)
+  #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)
+
+  #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;

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

@@ -44,6 +44,17 @@
 #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