diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml
index 13f02a8146..dd70fdc532 100644
--- a/conf/airframes/tudelft/cyfoam.xml
+++ b/conf/airframes/tudelft/cyfoam.xml
@@ -313,7 +313,9 @@
       <define name="FE_LIFT_A_AS" value="0.0008"/>
       <define name="FE_LIFT_B_AS" value="0.00009"/>
     </module>
-    <module name="nav" type="hybrid"/>
+    <module name="nav" type="hybrid">
+      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
+    </module>
 
     <!--<module name="ahrs"          type="int_cmpl_quat">-->
       <!--<configure name="USE_MAGNETOMETER" value="TRUE"/>-->
diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml
index 7dbfb5580d..a50cc78880 100644
--- a/conf/airframes/tudelft/nederdrone4.xml
+++ b/conf/airframes/tudelft/nederdrone4.xml
@@ -108,6 +108,10 @@
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
     </module-->
 
+    <module name="nav" type="hybrid">
+      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
+    </module>
+
     <module name="guidance" type="indi_hybrid">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
diff --git a/conf/airframes/tudelft/nederdrone4_tem.xml b/conf/airframes/tudelft/nederdrone4_tem.xml
index 1b643f7912..410f2f1ece 100644
--- a/conf/airframes/tudelft/nederdrone4_tem.xml
+++ b/conf/airframes/tudelft/nederdrone4_tem.xml
@@ -77,7 +77,7 @@
     <module name="stabilization" type="indi_simple"/>
     <module name="stabilization" type="rate_indi"/>
     <module name="ins"           type="ekf2" />
-    
+
     <module name="air_data"/>
 
     <!-- Internal MAG -->
@@ -97,6 +97,10 @@
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
     </module-->
 
+    <module name="nav" type="hybrid">
+      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
+    </module>
+
     <module name="guidance" type="indi_hybrid">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
diff --git a/conf/airframes/tudelft/nederdrone5.xml b/conf/airframes/tudelft/nederdrone5.xml
index f5e26b2dd2..8e576d9875 100644
--- a/conf/airframes/tudelft/nederdrone5.xml
+++ b/conf/airframes/tudelft/nederdrone5.xml
@@ -104,6 +104,10 @@
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
     </module-->
 
+    <module name="nav" type="hybrid">
+      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
+    </module>
+
     <module name="guidance" type="indi_hybrid">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
diff --git a/conf/airframes/tudelft/nederdrone6.xml b/conf/airframes/tudelft/nederdrone6.xml
index af50bfd205..7aec7060a6 100644
--- a/conf/airframes/tudelft/nederdrone6.xml
+++ b/conf/airframes/tudelft/nederdrone6.xml
@@ -128,6 +128,10 @@
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
     </module-->
 
+    <module name="nav" type="hybrid">
+      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
+    </module>
+
     <module name="guidance" type="indi_hybrid">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
diff --git a/conf/airframes/tudelft/nederdrone7.xml b/conf/airframes/tudelft/nederdrone7.xml
index 141413aee7..be20bc2cdd 100644
--- a/conf/airframes/tudelft/nederdrone7.xml
+++ b/conf/airframes/tudelft/nederdrone7.xml
@@ -127,6 +127,10 @@
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
     </module-->
 
+    <module name="nav" type="hybrid">
+      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
+    </module>
+
     <module name="guidance" type="indi_hybrid">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml
index c8b5204e30..96b8a3768d 100644
--- a/conf/airframes/tudelft/nederdrone8.xml
+++ b/conf/airframes/tudelft/nederdrone8.xml
@@ -127,6 +127,10 @@
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
     </module-->
 
+    <module name="nav" type="hybrid">
+      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
+    </module>
+
     <module name="guidance" type="indi_hybrid">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 204dc06578..bcecac685b 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -360,9 +360,18 @@ 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 {
-    VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
-    INT_VECT2_ZERO(guidance_h.ref.speed);
-    INT_VECT2_ZERO(guidance_h.ref.accel);
+    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
   }
 
 #if GUIDANCE_H_USE_SPEED_REF