diff --git a/conf/airframes/Poine/test_libeknav.xml b/conf/airframes/Poine/test_libeknav.xml
index bf39cf6151..356e3cd348 100644
--- a/conf/airframes/Poine/test_libeknav.xml
+++ b/conf/airframes/Poine/test_libeknav.xml
@@ -5,6 +5,11 @@
 
     HOST=auto3
 
+    PAPARAZZI_INC = -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT) \
+                    -I$(PAPARAZZI_SRC)/sw/airborne      \
+                    -I$(PAPARAZZI_SRC)/sw/include
+
+
     LIB_EIGEN_DIR = /opt/paparazzi/omap/overo-oe/tmp/sysroots/armv7a-angstrom-linux-gnueabi/usr/include/eigen2
     LIB_EIGEN_CFLAGS =  -I$(LIB_EIGEN_DIR) -DEIGEN_DONT_VECTORIZE  -DEIGEN_DONT_ALIGN -DNDEBUG 
 
@@ -19,7 +24,7 @@
     # test 1: how do I build cpp for using libeigen ?
     test1.ARCHDIR = omap
     test1.CXXFLAGS += $(LIB_EIGEN_CFLAGS)
-    test1.cpp_srcs  = fms/libeknav/hello_world.cpp
+    test1.cpp_srcs  = fms/libeknav/test_libeknav_1.cpp
 
     # test 2: now build with libeknav
     test2.ARCHDIR = omap
@@ -31,9 +36,7 @@
     # test 3: now try to add Paparazzi's C
     test3.ARCHDIR = omap
     test3.CXXFLAGS += $(LIB_EIGEN_CFLAGS)
-    test3.CXXFLAGS += -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT)
-    test3.CXXFLAGS += -I$(PAPARAZZI_SRC)/sw/airborne
-    test3.CXXFLAGS += -I$(PAPARAZZI_SRC)/sw/include
+    test3.CXXFLAGS += $(PAPARAZZI_INC)
     test3.cpp_srcs  = fms/libeknav/test_libeknav_3.cpp
     test3.CXXFLAGS += $(LIBEKNAV_CFLAGS)
     test3.cpp_srcs += $(LIBEKNAV_SRCS)
@@ -45,7 +48,7 @@
 
     # test network based telemetry on overo (using udp_transport2/messages2)
     overo_test_telemetry2.ARCHDIR  = omap
-    overo_test_telemetry2.CFLAGS  += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
+    overo_test_telemetry2.CFLAGS  += $(PAPARAZZI_INC)
     overo_test_telemetry2.srcs     = fms/overo_test_telemetry2.c
     overo_test_telemetry2.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
     overo_test_telemetry2.srcs    += fms/udp_transport2.c downlink.c
diff --git a/sw/airborne/fms/libeknav/test_libeknav_3.cpp b/sw/airborne/fms/libeknav/test_libeknav_3.cpp
index b21486518b..68188dfbf0 100644
--- a/sw/airborne/fms/libeknav/test_libeknav_3.cpp
+++ b/sw/airborne/fms/libeknav/test_libeknav_3.cpp
@@ -88,26 +88,8 @@ static void main_init(void) {
 
 static void main_periodic(int my_sig_num __attribute__ ((unused))) {
 
-  static uint32_t cnt;
-  cnt++;
-
   main_dialog_with_io_proc();
-
-  const double dt = 1./512.;
-  Vector3d gyro(0., 0., 0.);
-  Vector3d accelerometer(0., 0., 9.81);
-  ins.predict(gyro, accelerometer, dt);
-
-  if (cnt % 10 == 0) { /* update mag at 50Hz */
-    Vector3d magnetometer = Vector3d::UnitZ();
-    const double   mag_noise = std::pow(5 / 180.0 * M_PI, 2);
-    ins.obs_vector(magnetometer, magnetometer, mag_noise);
-  }
-  if (cnt % 128 == 0) /* update gps at 4 Hz */ {
-    const Vector3d gps_pos_noise = Vector3d::Ones()  *10*10;
-    const Vector3d gps_speed_noise = Vector3d::Ones()*0.1*0.1;
-    ins.obs_gps_pv_report(pos_0_ecef, speed_0_ecef, gps_pos_noise, gps_speed_noise);
-  }
+  main_run_ins();
 
 }
 
@@ -128,3 +110,26 @@ static void main_dialog_with_io_proc() {
   MAGS_FLOAT_OF_BFP(imu.mag, in->mag); 
 
 }
+
+static void main_run_ins() {
+
+  static uint32_t cnt;
+  cnt++;
+
+  const double dt = 1./512.;
+  Vector3d gyro(0., 0., 0.);
+  Vector3d accelerometer(0., 0., 9.81);
+  ins.predict(gyro, accelerometer, dt);
+  
+  if (cnt % 10 == 0) { /* update mag at 50Hz */
+    Vector3d magnetometer = Vector3d::UnitZ();
+    const double   mag_noise = std::pow(5 / 180.0 * M_PI, 2);
+    ins.obs_vector(magnetometer, magnetometer, mag_noise);
+  }
+  if (cnt % 128 == 0) /* update gps at 4 Hz */ {
+    const Vector3d gps_pos_noise = Vector3d::Ones()  *10*10;
+    const Vector3d gps_speed_noise = Vector3d::Ones()*0.1*0.1;
+    ins.obs_gps_pv_report(pos_0_ecef, speed_0_ecef, gps_pos_noise, gps_speed_noise);
+  }
+  
+}