diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index d388bb1014b..cc8f78e1f77 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -4,6 +4,16 @@
     <load name="servo_switch.xml"/>
     <load name="rotorcraft_cam.xml"/>
     <load name="sonar_maxbotix_booz.xml"/>
+    <!--load name="baro_mpl3115.xml">
+      <define name="MPL3115_I2C_DEV" value="i2c1"/>
+      <define name="USE_I2C1"/>
+      <define name="SENSOR_SYNC_SEND"/>
+    </load-->
+    <load name="mavlink_decoder.xml">
+      <configure name="MAVLINK_PORT" value="UART0"/>
+      <configure name="MAVLINK_BAUD" value="B115200"/>
+    </load>
+    <load name="px4flow.xml"/>
     <!--load name="adc_generic.xml">
       <configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
     </load-->
@@ -31,9 +41,9 @@
       <define name="USE_PWM1"/>
     </subsystem>
     <subsystem name="imu"           type="b2_v1.1"/>
-    <subsystem name="gps"           type="ublox">
+    <!--subsystem name="gps"           type="ublox">
       <configure name="GPS_BAUD"    value="B57600"/>
-    </subsystem>
+    </subsystem-->
     <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_euler"/>
     <subsystem name="ins"           type="extended"/>
@@ -165,7 +175,8 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="15." integer="16"/>
-    <define name="SONAR_SENS" value="0.00650498" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply -->
+    <!--define name="SONAR_SENS" value="0.00650498" integer="16"/--> <!-- XL-MaxSonar-EZ4 5V supply -->
+    <define name="SONAR_SENS" value="0.009856" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply scaled to 3.3V -->
     <define name="SONAR_MAX_RANGE" value="5.0"/>
     <define name="SONAR_MIN_RANGE" value="0.25"/>
     <!--define name="SONAR_VARIANCE_THRESHOLD" value="0.01"/-->
diff --git a/conf/flight_plans/rotorcraft_vision.xml b/conf/flight_plans/rotorcraft_vision.xml
new file mode 100644
index 00000000000..69fe7a703c0
--- /dev/null
+++ b/conf/flight_plans/rotorcraft_vision.xml
@@ -0,0 +1,33 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan alt="1.5" ground_alt="0" lat0="43 33 50.83" lon0="1 28 52.61" max_dist_from_home="400" name="Booz Vision" security_height="0.5">
+  <waypoints>
+    <waypoint name="HOME" x="0.0" y="0.0"/>
+  </waypoints>
+  <blocks>
+    <block name="Geo init">
+      <while cond="LessThan(NavBlockTime(), 3)"/>
+      <call fun="NavSetGroundReferenceHere()"/>
+    </block>
+    <block name="Holding point">
+      <!--exception cond="booz_ins_enu_pos.z > POS_BFP_OF_REAL(1.)" deroute="Standby"/-->
+      <call fun="NavKillThrottle()"/>
+      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
+    </block>
+    <block name="Start Engine">
+      <call fun="NavResurrect()"/>
+      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
+    </block>
+    <block name="Standby" strip_button="Standby" strip_icon="home.png">
+      <stay wp="HOME"/>
+    </block>
+    <block name="land">
+      <go wp="HOME"/>
+    </block>
+    <block name="flare">
+      <exception cond="NavDetectGround()" deroute="Holding point"/>
+      <call fun="NavStartDetectGround()"/>
+      <stay climb="-0.8" vmode="climb" wp="HOME"/>
+    </block>
+  </blocks>
+</flight_plan>
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index 3d45e47fc9e..0f029292023 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -256,7 +256,11 @@ void autopilot_on_rc_frame(void) {
     uint8_t new_autopilot_mode = 0;
     AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
     /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
-    if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost()))
+    if (!(new_autopilot_mode == AP_MODE_NAV
+#if USE_GPS
+          && GpsIsLost()
+#endif
+       ))
       autopilot_set_mode(new_autopilot_mode);
   }