diff --git a/conf/airframes/ENAC/fixed-wing/drops.xml b/conf/airframes/ENAC/fixed-wing/drops.xml
index ee2d642f6c..f8dfb778d5 100644
--- a/conf/airframes/ENAC/fixed-wing/drops.xml
+++ b/conf/airframes/ENAC/fixed-wing/drops.xml
@@ -258,8 +258,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl_a.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += bomb.c
 
@@ -272,7 +272,7 @@ sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLIN
 sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += bomb.c
 
diff --git a/conf/airframes/ENAC/fixed-wing/firestorm.xml b/conf/airframes/ENAC/fixed-wing/firestorm.xml
index 2ffaed1292..29304ed723 100644
--- a/conf/airframes/ENAC/fixed-wing/firestorm.xml
+++ b/conf/airframes/ENAC/fixed-wing/firestorm.xml
@@ -247,8 +247,8 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
 
 ap.CFLAGS += -DGYRO -DADXRS150
-ap.srcs += gyro.c nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += gyro.c subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 # Current sensor; Neutral=162
 #ap.CFLAGS += -DUSE_ADC_7 -DUSE_ADC_GENERIC -DADC_CHANNEL_GENERIC1=ADC_7 -DADC_CHANNEL_GENERIC_NB_SAMPLES=16
@@ -266,7 +266,7 @@ sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c inf
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.CFLAGS += -DUSE_MODULES
   </makefile>
diff --git a/conf/airframes/ENAC/fixed-wing/funjet1.xml b/conf/airframes/ENAC/fixed-wing/funjet1.xml
index 7f464a7866..d90dd7bde5 100644
--- a/conf/airframes/ENAC/fixed-wing/funjet1.xml
+++ b/conf/airframes/ENAC/fixed-wing/funjet1.xml
@@ -269,8 +269,8 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
 
 ap.CFLAGS += -DGYRO -DADXRS150
-ap.srcs += gyro.c nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += gyro.c subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 # chemotaxis.c anemotaxis.c discsurvey.c
 
 ########## Barometer (SPI)
@@ -303,7 +303,7 @@ sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c inf
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 # -DENOSE
 #sim.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 #sim.CFLAGS += -DUSE_I2C -DUSE_I2C0
diff --git a/conf/airframes/ENAC/fixed-wing/minimag1.xml b/conf/airframes/ENAC/fixed-wing/minimag1.xml
index 8054cc80d7..517658d4a3 100644
--- a/conf/airframes/ENAC/fixed-wing/minimag1.xml
+++ b/conf/airframes/ENAC/fixed-wing/minimag1.xml
@@ -207,12 +207,12 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/slayer2.xml b/conf/airframes/ENAC/fixed-wing/slayer2.xml
index efa6aa0c3d..7f6e7bb37d 100644
--- a/conf/airframes/ENAC/fixed-wing/slayer2.xml
+++ b/conf/airframes/ENAC/fixed-wing/slayer2.xml
@@ -238,7 +238,7 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c traffic_info.c nav_line.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c traffic_info.c subsystems/navigation/nav_line.c
 
 # Camera power switch on ADC_7 pin
 ap.CFLAGS += -DLED_3_BANK=0 -DLED_3_PIN=10 -DPOWER_SWITCH_LED=3
@@ -276,7 +276,7 @@ sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c inf
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN  -DUSE_BARO_MS5534A 
 
-sim.srcs += nav_survey_rectangle.c nav_line.c $(SRC_ARCH)/sim_baro.c 
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c $(SRC_ARCH)/sim_baro.c 
 
 sim.srcs += fw_h_ctl.c
 # sim.srcs += fw_h_ctl_a.c
diff --git a/conf/airframes/ENAC/fixed-wing/soarzi.xml b/conf/airframes/ENAC/fixed-wing/soarzi.xml
index 6298b647f7..6e0563dfc5 100644
--- a/conf/airframes/ENAC/fixed-wing/soarzi.xml
+++ b/conf/airframes/ENAC/fixed-wing/soarzi.xml
@@ -213,14 +213,14 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 ap.CFLAGS += -DUSE_ADC_6 -DADC_CHANNEL_CURRENT=ADC_6
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/solarstorm.xml b/conf/airframes/ENAC/fixed-wing/solarstorm.xml
index 643e1f63fe..4d29be8988 100644
--- a/conf/airframes/ENAC/fixed-wing/solarstorm.xml
+++ b/conf/airframes/ENAC/fixed-wing/solarstorm.xml
@@ -240,8 +240,8 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
 
 ap.CFLAGS += -DGYRO -DADXRS150
-ap.srcs += gyro.c nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += gyro.c subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ########## MPTT
 ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
@@ -258,7 +258,7 @@ sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c inf
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.CFLAGS += -DUSE_MODULES
   </makefile>
diff --git a/conf/airframes/ENAC/fixed-wing/spocII.xml b/conf/airframes/ENAC/fixed-wing/spocII.xml
index 9bcfcce7f7..1e3eb2313e 100644
--- a/conf/airframes/ENAC/fixed-wing/spocII.xml
+++ b/conf/airframes/ENAC/fixed-wing/spocII.xml
@@ -242,7 +242,7 @@ ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
-ap.srcs += nav_survey_rectangle.c nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 ap.CFLAGS += -DGYRO -DADXRS150
 ap.srcs += gyro.c
@@ -258,7 +258,7 @@ sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c inf
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Config for JSBSim simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl_jsbsim.makefile
@@ -267,7 +267,7 @@ jsbsim.CFLAGS += -I/home/cocoleon/usr/include/JSBSim
 jsbsim.LDFLAGS += -L/home/cocoleon/usr/lib/
 
 jsbsim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-jsbsim.srcs += nav_line.c nav_survey_rectangle.c
+jsbsim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
 
 
diff --git a/conf/airframes/ENAC/fixed-wing/twinjet2.xml b/conf/airframes/ENAC/fixed-wing/twinjet2.xml
index 8360ee69b9..2d0cef5c68 100644
--- a/conf/airframes/ENAC/fixed-wing/twinjet2.xml
+++ b/conf/airframes/ENAC/fixed-wing/twinjet2.xml
@@ -273,8 +273,8 @@ ap.srcs += infrared.c estimator.c
 ########## Nav
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DPITCH_TRIM
 ap.srcs += nav.c fw_h_ctl_a.c fw_v_ctl_n.c
-ap.srcs += nav_survey_rectangle.c
-ap.srcs += nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
 
 ########## SPI Master use slave0
 ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
@@ -308,7 +308,7 @@ sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c inf
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DPITCH_TRIM -DALT_KALMAN
 
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.CFLAGS += -DUSE_LIGHT
 sim.srcs += light.c
diff --git a/conf/airframes/ENAC/malolo_sim.xml b/conf/airframes/ENAC/malolo_sim.xml
index 86367482a9..47b1da3cd3 100644
--- a/conf/airframes/ENAC/malolo_sim.xml
+++ b/conf/airframes/ENAC/malolo_sim.xml
@@ -217,7 +217,7 @@ sim.CFLAGS += -I/home/cocoleon/usr/include/JSBSim
 sim.LDFLAGS += -L/home/cocoleon/usr/lib/
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 sim.srcs += $(SIMDIR)/sim_ac_fw.c
 
   </makefile>
diff --git a/conf/airframes/PPZUAV/fixed-wing/kalscott_easystar.xml b/conf/airframes/PPZUAV/fixed-wing/kalscott_easystar.xml
index c20d596a19..670e66e42f 100644
--- a/conf/airframes/PPZUAV/fixed-wing/kalscott_easystar.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/kalscott_easystar.xml
@@ -191,15 +191,15 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DLOITER_TRIM 
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c 
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DLOITER_TRIM -DALT_KALMAN -DUSE_MODULES 
-sim.srcs += nav_line.c nav_survey_rectangle.c 
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c 
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/Paul/minimag1.xml b/conf/airframes/Paul/minimag1.xml
index 9f871c74a0..a52d8c582c 100644
--- a/conf/airframes/Paul/minimag1.xml
+++ b/conf/airframes/Paul/minimag1.xml
@@ -203,12 +203,12 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DWIND_INFO
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 ap.CFLAGS += -DTELEMETRY_MODE_FBW=1
 
diff --git a/conf/airframes/Poine/easy_glider1.xml b/conf/airframes/Poine/easy_glider1.xml
index cacf15c754..ca25b382ec 100644
--- a/conf/airframes/Poine/easy_glider1.xml
+++ b/conf/airframes/Poine/easy_glider1.xml
@@ -196,7 +196,7 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Current sensor
 ap.CFLAGS += -DUSE_ADC_7 -DUSE_ADC_GENERIC -DADC_CHANNEL_GENERIC1=ADC_7 -DADC_CHANNEL_GENERIC_NB_SAMPLES=16
@@ -208,7 +208,7 @@ ap.srcs += adc_generic.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c traffic_info.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c traffic_info.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/Poine/test_libeknav.xml b/conf/airframes/Poine/test_libeknav.xml
index fa1209a8d4..4f7edfa135 100644
--- a/conf/airframes/Poine/test_libeknav.xml
+++ b/conf/airframes/Poine/test_libeknav.xml
@@ -63,7 +63,7 @@
     filter.ARCHDIR = omap
     filter.CXXFLAGS += $(LIB_EIGEN_CFLAGS)
     filter.CXXFLAGS += $(PAPARAZZI_INC)
-    filter.cpp_srcs  = fms/libeknav/libeknav_from_log.cpp
+    filter.cpp_srcs  = fms/libeknav/libeksubsystems/navigation/nav_from_log.cpp
     filter.CXXFLAGS += $(LIBEKNAV_CFLAGS)
     filter.cpp_srcs += $(LIBEKNAV_SRCS)
     filter.LDFLAGS  += -levent -lm
diff --git a/conf/airframes/TUDelft/Trip50A.xml b/conf/airframes/TUDelft/Trip50A.xml
index b8203b747a..c852c395de 100644
--- a/conf/airframes/TUDelft/Trip50A.xml
+++ b/conf/airframes/TUDelft/Trip50A.xml
@@ -286,7 +286,7 @@ ap.srcs += infrared.c estimator.c gyro.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 #ap.srcs += baro_bmp.c $(SRC_ARCH)/i2c_hw.c i2c.c
 #ap.CFLAGS += -DUSE_I2C0 -DUSE_BARO_BMP -DBARO_BMP_ACCEL
@@ -294,7 +294,7 @@ ap.srcs += nav_line.c nav_survey_rectangle.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/TUDelft/Trip50B.xml b/conf/airframes/TUDelft/Trip50B.xml
index d0194acc90..e24c6b4b79 100644
--- a/conf/airframes/TUDelft/Trip50B.xml
+++ b/conf/airframes/TUDelft/Trip50B.xml
@@ -291,7 +291,7 @@ ap.srcs += infrared.c estimator.c gyro.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += baro_bmp.c $(SRC_ARCH)/i2c_hw.c i2c.c
 ap.CFLAGS += -DUSE_I2C0 -DUSE_BARO_BMP -DBARO_BMP_ACCEL
@@ -303,7 +303,7 @@ ap.srcs += cam.c point.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/TUDelft/holiday50.xml b/conf/airframes/TUDelft/holiday50.xml
index 3edc74946e..2828a01882 100644
--- a/conf/airframes/TUDelft/holiday50.xml
+++ b/conf/airframes/TUDelft/holiday50.xml
@@ -291,7 +291,7 @@ ap.srcs += gyro.c
 
 ap.CFLAGS += -DUSE_MODULES
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 ap.srcs += traffic_info.c
 
 # camera control
@@ -306,7 +306,7 @@ ap.srcs += bomb.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO -DSTRONG_WIND
-sim.srcs += nav_survey_rectangle.c nav_line.c 
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c 
 sim.CFLAGS += -DTUNE_AGRESSIVE_CLIMB
 # traffic_info.c
 sim.CFLAGS += -DUSE_MODULES 
diff --git a/conf/airframes/demo_module.xml b/conf/airframes/demo_module.xml
index 9eb12183df..e3dc228eb5 100644
--- a/conf/airframes/demo_module.xml
+++ b/conf/airframes/demo_module.xml
@@ -198,15 +198,15 @@ ap.srcs += infrared.c estimator.c
 ########## Nav
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
-ap.srcs += nav_survey_rectangle.c
-ap.srcs += nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
 
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/hitl_usb.xml b/conf/airframes/hitl_usb.xml
index 08bc2a242b..64d65299d2 100644
--- a/conf/airframes/hitl_usb.xml
+++ b/conf/airframes/hitl_usb.xml
@@ -146,7 +146,7 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Configuration for Hardware In The Loop
 ap.CFLAGS += -DHITL
@@ -157,7 +157,7 @@ sim.ARCHDIR = $(ARCH)
 sim.CFLAGS += -DSITL -DAP -DFBW -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED -DNAV -DLED
 sim.srcs = latlong.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c traffic_info.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c traffic_info.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml
index d5024ad2e1..7b2f1d6d0b 100644
--- a/conf/airframes/jsbsim.xml
+++ b/conf/airframes/jsbsim.xml
@@ -200,7 +200,7 @@
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 
 #### Config for SITL simulation with JSBSim
@@ -208,7 +208,7 @@ SRC_FIRMWARE = firmwares/fixedwing
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl_jsbsim.makefile
 
 jsbsim.CFLAGS += -DBOARD_CONFIG=\"tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-jsbsim.srcs += nav_line.c nav_survey_rectangle.c
+jsbsim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
 
   </makefile>
diff --git a/conf/airframes/microjet_raw_makefile.xml b/conf/airframes/microjet_raw_makefile.xml
index f7f0c0fd2e..a62fd2c606 100644
--- a/conf/airframes/microjet_raw_makefile.xml
+++ b/conf/airframes/microjet_raw_makefile.xml
@@ -201,7 +201,7 @@
  ap.srcs += infrared.c estimator.c
  ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
  ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
- ap.srcs += nav_line.c nav_survey_rectangle.c
+ ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
  ap.CFLAGS += -DTRAFFIC_INFO -DALT_KALMAN
  sim.CFLAGS += -DTRAFFIC_INFO
diff --git a/conf/airframes/mm/extra/press_t.xml b/conf/airframes/mm/extra/press_t.xml
index 13ab9f25eb..899b15b4fb 100755
--- a/conf/airframes/mm/extra/press_t.xml
+++ b/conf/airframes/mm/extra/press_t.xml
@@ -262,8 +262,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -291,7 +291,7 @@ ap.srcs += adc_generic.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/drops.xml b/conf/airframes/mm/fixed-wing/drops.xml
index 6c72f9d4e0..aabb329cbb 100644
--- a/conf/airframes/mm/fixed-wing/drops.xml
+++ b/conf/airframes/mm/fixed-wing/drops.xml
@@ -280,8 +280,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl_a.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += bomb.c
 
@@ -297,7 +297,7 @@ sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLIN
 sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += bomb.c
 
diff --git a/conf/airframes/mm/fixed-wing/funjeteth1.xml b/conf/airframes/mm/fixed-wing/funjeteth1.xml
index 118e228abe..3196ec31f2 100644
--- a/conf/airframes/mm/fixed-wing/funjeteth1.xml
+++ b/conf/airframes/mm/fixed-wing/funjeteth1.xml
@@ -239,8 +239,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_cube.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_cube.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 
 ap.CFLAGS += -DMICROMAG_DRDY_VIC_SLOT=12 -DSSP_VIC_SLOT=11 -DUSE_MICROMAG_FW
@@ -256,7 +256,7 @@ ap.CFLAGS += -DUSE_I2C0 -DUSE_BARO_SCP
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c nav_cube.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_cube.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjeteth2.xml b/conf/airframes/mm/fixed-wing/funjeteth2.xml
index e97aa831b0..f27ba568e3 100644
--- a/conf/airframes/mm/fixed-wing/funjeteth2.xml
+++ b/conf/airframes/mm/fixed-wing/funjeteth2.xml
@@ -240,8 +240,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_cube.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_cube.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.CFLAGS += -DMICROMAG_DRDY_VIC_SLOT=12 -DSSP_VIC_SLOT=11 -DUSE_MICROMAG_FW
 ap.srcs += micromag_fw.c $(SRC_ARCH)/micromag_fw_hw.c
@@ -256,7 +256,7 @@ ap.CFLAGS += -DUSE_I2C0 -DUSE_BARO_SCP
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c nav_cube.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_cube.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/mm/fixed-wing/funjetfmi1.xml b/conf/airframes/mm/fixed-wing/funjetfmi1.xml
index 1827598db7..948fb81165 100644
--- a/conf/airframes/mm/fixed-wing/funjetfmi1.xml
+++ b/conf/airframes/mm/fixed-wing/funjetfmi1.xml
@@ -242,8 +242,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=30 -DSCK_PIN=4
@@ -269,7 +269,7 @@ ap.srcs += micromag_fw.c $(SRC_ARCH)/micromag_fw_hw.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 
   </makefile>
diff --git a/conf/airframes/mm/fixed-wing/funjetfmi2.xml b/conf/airframes/mm/fixed-wing/funjetfmi2.xml
index c366722814..8182fd7ce5 100644
--- a/conf/airframes/mm/fixed-wing/funjetfmi2.xml
+++ b/conf/airframes/mm/fixed-wing/funjetfmi2.xml
@@ -241,8 +241,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=30 -DSCK_PIN=4
@@ -265,7 +265,7 @@ ap.srcs += micromag_fw.c $(SRC_ARCH)/micromag_fw_hw.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 
   </makefile>
diff --git a/conf/airframes/mm/fixed-wing/funjetfmi3.xml b/conf/airframes/mm/fixed-wing/funjetfmi3.xml
index dfc68c5333..2500f1aaaf 100644
--- a/conf/airframes/mm/fixed-wing/funjetfmi3.xml
+++ b/conf/airframes/mm/fixed-wing/funjetfmi3.xml
@@ -241,8 +241,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=30 -DSCK_PIN=4
@@ -265,7 +265,7 @@ ap.srcs += micromag_fw.c $(SRC_ARCH)/micromag_fw_hw.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 
   </makefile>
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi1.xml b/conf/airframes/mm/fixed-wing/funjetgfi1.xml
index 7e953393eb..a4d6d58929 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi1.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi1.xml
@@ -219,8 +219,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -234,7 +234,7 @@ ap.CFLAGS += -DUSE_JOYSTICK
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi3.xml b/conf/airframes/mm/fixed-wing/funjetgfi3.xml
index f3beff1ac5..ddb86be216 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi3.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi3.xml
@@ -229,7 +229,7 @@ ap.CFLAGS += -DINFRARED
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c nav_survey_rectangle.c nav_line.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 
 #ap.CFLAGS += -DGYRO -DADXRS150
@@ -265,7 +265,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi4.xml b/conf/airframes/mm/fixed-wing/funjetgfi4.xml
index 852981c359..2f9429a0ac 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi4.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi4.xml
@@ -220,8 +220,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -235,7 +235,7 @@ ap.CFLAGS += -DUSE_JOYSTICK
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi5.xml b/conf/airframes/mm/fixed-wing/funjetgfi5.xml
index 4a32f306fc..9befef55da 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi5.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi5.xml
@@ -221,8 +221,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -236,7 +236,7 @@ ap.CFLAGS += -DUSE_JOYSTICK
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi6.xml b/conf/airframes/mm/fixed-wing/funjetgfi6.xml
index 949af4eb4a..1a2f44d223 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi6.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi6.xml
@@ -221,8 +221,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -236,7 +236,7 @@ ap.CFLAGS += -DUSE_JOYSTICK
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi7.xml b/conf/airframes/mm/fixed-wing/funjetgfi7.xml
index f340021da8..e6be130670 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi7.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi7.xml
@@ -221,8 +221,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -236,7 +236,7 @@ ap.CFLAGS += -DUSE_JOYSTICK
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi8.xml b/conf/airframes/mm/fixed-wing/funjetgfi8.xml
index 038601ddd6..8b7a2c1049 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi8.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi8.xml
@@ -221,8 +221,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -236,7 +236,7 @@ ap.CFLAGS += -DUSE_JOYSTICK
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi9.xml b/conf/airframes/mm/fixed-wing/funjetgfi9.xml
index 19ff4525c9..f470242846 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi9.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi9.xml
@@ -221,8 +221,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3  -DSCK_PIN=2
@@ -236,7 +236,7 @@ ap.CFLAGS += -DUSE_JOYSTICK
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK
diff --git a/conf/airframes/mm/fixed-wing/funjetmm2.xml b/conf/airframes/mm/fixed-wing/funjetmm2.xml
index 835eaaaef4..fbfbdb2b39 100644
--- a/conf/airframes/mm/fixed-wing/funjetmm2.xml
+++ b/conf/airframes/mm/fixed-wing/funjetmm2.xml
@@ -248,8 +248,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += humid_sht.c
 ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=30  -DSCK_PIN=4
@@ -265,7 +265,7 @@ ap.srcs += adc_generic.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += bomb.c
 
diff --git a/conf/airframes/mm/fixed-wing/merlin.xml b/conf/airframes/mm/fixed-wing/merlin.xml
index 9a9c578386..5f89367edb 100644
--- a/conf/airframes/mm/fixed-wing/merlin.xml
+++ b/conf/airframes/mm/fixed-wing/merlin.xml
@@ -216,8 +216,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += snav.c
 
@@ -228,7 +228,7 @@ sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLIN
 sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/mm/fixed-wing/miniwing.xml b/conf/airframes/mm/fixed-wing/miniwing.xml
index ce71ac7576..ceaaea119e 100644
--- a/conf/airframes/mm/fixed-wing/miniwing.xml
+++ b/conf/airframes/mm/fixed-wing/miniwing.xml
@@ -210,8 +210,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 #ap.CFLAGS += -DTELEMETRY_MODE_FBW=1
 
@@ -220,7 +220,7 @@ ap.CFLAGS += -DUSE_ACCEL_MEMSIC -DUSE_ADC_3 -DUSE_ADC_4
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/mm/fixed-wing/slowfast.xml b/conf/airframes/mm/fixed-wing/slowfast.xml
index 764f7af860..388bc39741 100644
--- a/conf/airframes/mm/fixed-wing/slowfast.xml
+++ b/conf/airframes/mm/fixed-wing/slowfast.xml
@@ -262,8 +262,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 ap.srcs += snav.c
 
 ap.CFLAGS += -DGYRO -DADXRS150
@@ -272,7 +272,7 @@ ap.srcs += gyro.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 -->
diff --git a/conf/airframes/mm/fixed-wing/slowfast2.xml b/conf/airframes/mm/fixed-wing/slowfast2.xml
index ff61ea0c45..47e50de240 100644
--- a/conf/airframes/mm/fixed-wing/slowfast2.xml
+++ b/conf/airframes/mm/fixed-wing/slowfast2.xml
@@ -276,8 +276,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 ap.srcs += snav.c
 
 ap.CFLAGS += -DGYRO -DADXRS150
@@ -286,7 +286,7 @@ ap.srcs += gyro.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 -->
diff --git a/conf/airframes/mm/fixed-wing/twinstarmm.xml b/conf/airframes/mm/fixed-wing/twinstarmm.xml
index 8141189ffb..197fb366bc 100644
--- a/conf/airframes/mm/fixed-wing/twinstarmm.xml
+++ b/conf/airframes/mm/fixed-wing/twinstarmm.xml
@@ -237,8 +237,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += bomb.c
 
@@ -251,7 +251,7 @@ sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLIN
 sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += bomb.c
 
diff --git a/conf/airframes/mm/hangar/glass_one1.xml b/conf/airframes/mm/hangar/glass_one1.xml
index 94e2163843..076b032d24 100644
--- a/conf/airframes/mm/hangar/glass_one1.xml
+++ b/conf/airframes/mm/hangar/glass_one1.xml
@@ -239,7 +239,7 @@ ap.srcs += infrared.c estimator.c
 # -DALT_KALMAN
 
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c nav_survey_rectangle.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c subsystems/navigation/nav_survey_rectangle.c
 
 #ap.CFLAGS += -DGYRO -DADXRS150
 #ap.srcs += gyro.c
@@ -247,7 +247,7 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c nav_survey_rectangle.c
 #ap.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH
 #ap.srcs += point.c cam.c
 
-ap.srcs += traffic_info.c nav_line.c
+ap.srcs += traffic_info.c subsystems/navigation/nav_line.c
 
 # Video switch
 ap.CFLAGS += -DUSE_GPIO
@@ -261,9 +261,9 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM
-sim.srcs += bomb.c nav_survey_rectangle.c
+sim.srcs += bomb.c subsystems/navigation/nav_survey_rectangle.c
 
-sim.srcs += traffic_info.c nav_line.c
+sim.srcs += traffic_info.c subsystems/navigation/nav_line.c
 
 
   </makefile>
diff --git a/conf/airframes/mm/hangar/glass_one2.xml b/conf/airframes/mm/hangar/glass_one2.xml
index 23e636a6fe..186d698a1b 100644
--- a/conf/airframes/mm/hangar/glass_one2.xml
+++ b/conf/airframes/mm/hangar/glass_one2.xml
@@ -213,12 +213,12 @@ ap.srcs += infrared.c estimator.c
 # -DALT_KALMAN
 
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c nav_survey_rectangle.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c subsystems/navigation/nav_survey_rectangle.c
 
 #ap.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH
 #ap.srcs += point.c cam.c
 
-ap.srcs += traffic_info.c nav_line.c
+ap.srcs += traffic_info.c subsystems/navigation/nav_line.c
 
 # Video switch
 ap.CFLAGS += -DUSE_GPIO
@@ -232,9 +232,9 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM
-sim.srcs += bomb.c nav_survey_rectangle.c
+sim.srcs += bomb.c subsystems/navigation/nav_survey_rectangle.c
 
-sim.srcs += traffic_info.c nav_line.c
+sim.srcs += traffic_info.c subsystems/navigation/nav_line.c
 
 
   </makefile>
diff --git a/conf/airframes/mm/hangar/glass_one3.xml b/conf/airframes/mm/hangar/glass_one3.xml
index 19cd019532..01f53eb289 100644
--- a/conf/airframes/mm/hangar/glass_one3.xml
+++ b/conf/airframes/mm/hangar/glass_one3.xml
@@ -240,12 +240,12 @@ ap.srcs += infrared.c estimator.c
 # -DALT_KALMAN
 
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c nav_survey_rectangle.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c subsystems/navigation/nav_survey_rectangle.c
 
 #ap.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH
 #ap.srcs += point.c cam.c
 
-ap.srcs += traffic_info.c nav_line.c
+ap.srcs += traffic_info.c subsystems/navigation/nav_line.c
 
 # Video switch
 ap.CFLAGS += -DUSE_GPIO
@@ -259,9 +259,9 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM
-sim.srcs += bomb.c nav_survey_rectangle.c
+sim.srcs += bomb.c subsystems/navigation/nav_survey_rectangle.c
 
-sim.srcs += traffic_info.c nav_line.c
+sim.srcs += traffic_info.c subsystems/navigation/nav_line.c
 
 
   </makefile>
diff --git a/conf/airframes/mm/hangar/lila.xml b/conf/airframes/mm/hangar/lila.xml
index 42926a25e9..1374297a4d 100644
--- a/conf/airframes/mm/hangar/lila.xml
+++ b/conf/airframes/mm/hangar/lila.xml
@@ -240,8 +240,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl_a.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += snav.c
 
@@ -251,6 +251,6 @@ sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLIN
 sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
   </makefile>
 </airframe>
diff --git a/conf/airframes/mm/hangar/red_one.xml b/conf/airframes/mm/hangar/red_one.xml
index 3e3542f543..a8ef34aa96 100644
--- a/conf/airframes/mm/hangar/red_one.xml
+++ b/conf/airframes/mm/hangar/red_one.xml
@@ -260,7 +260,7 @@ ap.srcs += infrared.c estimator.c
 # -DALT_KALMAN
 
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c nav_survey_rectangle.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c latlong.c subsystems/navigation/nav_survey_rectangle.c
 
 ap.CFLAGS += -DGYRO -DADXRS150
 ap.srcs += gyro.c
@@ -268,7 +268,7 @@ ap.srcs += gyro.c
 ap.srcs += bomb.c
 
 ap.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH
-ap.srcs += traffic_info.c point.c cam.c nav_line.c
+ap.srcs += traffic_info.c point.c cam.c subsystems/navigation/nav_line.c
 
 ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
 ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
@@ -288,10 +288,10 @@ ap.CFLAGS += -DUSE_CURRENT
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM
-sim.srcs += bomb.c nav_survey_rectangle.c
+sim.srcs += bomb.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH
-sim.srcs += traffic_info.c point.c cam.c nav_line.c
+sim.srcs += traffic_info.c point.c cam.c subsystems/navigation/nav_line.c
 
 
   </makefile>
diff --git a/conf/airframes/obsolete/easystar2.xml b/conf/airframes/obsolete/easystar2.xml
index eb4332ab87..fd0de7f3e1 100644
--- a/conf/airframes/obsolete/easystar2.xml
+++ b/conf/airframes/obsolete/easystar2.xml
@@ -213,8 +213,8 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 # Camera control
 #ap.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH_ROLL
@@ -228,7 +228,7 @@ ap.srcs += airspeed.c airspeed_ets.c baro_ets.c i2c.c $(SRC_ARCH)/i2c_hw.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DLOITER_TRIM -DALT_KALMAN -DUSE_MODULES
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 sim.CFLAGS += -DUSE_AIRSPEED_ETS -DUSE_AIRSPEED -DUSE_BARO_ETS -DUSE_I2C0 -DAGR_CLIMB
 sim.srcs += airspeed.c airspeed_ets.c baro_ets.c i2c.c $(SRC_ARCH)/i2c_hw.c
 
diff --git a/conf/airframes/obsolete/kalscott_easystar.xml b/conf/airframes/obsolete/kalscott_easystar.xml
index a528d3a20c..ea3a016248 100644
--- a/conf/airframes/obsolete/kalscott_easystar.xml
+++ b/conf/airframes/obsolete/kalscott_easystar.xml
@@ -193,14 +193,14 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DLOITER_TRIM -DALT_KALMAN -DUSE_MODULES
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/malolo_sim.xml b/conf/airframes/obsolete/malolo_sim.xml
index 3398a09c8c..3086098e87 100644
--- a/conf/airframes/obsolete/malolo_sim.xml
+++ b/conf/airframes/obsolete/malolo_sim.xml
@@ -234,7 +234,7 @@ sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLIN
 
 sim.srcs = $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/ivy_transport.c
 sim.srcs += latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c
 
diff --git a/conf/airframes/obsolete/microjet5.xml b/conf/airframes/obsolete/microjet5.xml
index e7d2167aa2..6cd330322a 100644
--- a/conf/airframes/obsolete/microjet5.xml
+++ b/conf/airframes/obsolete/microjet5.xml
@@ -206,11 +206,11 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
 ap.CFLAGS += -DGYRO -DADXRS150
 ap.srcs += gyro.c
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DTRAFFIC_INFO
-sim.srcs += nav_survey_rectangle.c traffic_info.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c traffic_info.c subsystems/navigation/nav_line.c
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/microjet5_tp_auto.xml b/conf/airframes/obsolete/microjet5_tp_auto.xml
index 0e876087fb..16767e07d4 100644
--- a/conf/airframes/obsolete/microjet5_tp_auto.xml
+++ b/conf/airframes/obsolete/microjet5_tp_auto.xml
@@ -227,7 +227,7 @@ ap.srcs += gyro.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\"
-sim.srcs += nav_survey_rectangle.c traffic_info.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c traffic_info.c subsystems/navigation/nav_line.c
 sim.CFLAGS += -DGYRO -DADXRS150
 sim.srcs += gyro.c
 
diff --git a/conf/airframes/obsolete/microjet6.xml b/conf/airframes/obsolete/microjet6.xml
index 4611a96748..4e5fb635f4 100644
--- a/conf/airframes/obsolete/microjet6.xml
+++ b/conf/airframes/obsolete/microjet6.xml
@@ -235,7 +235,7 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN -DWIND_INFO
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c
 
 
 ap.CFLAGS += -DGYRO -DADXRS150
@@ -244,7 +244,7 @@ ap.srcs += gyro.c
 ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
 ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
 
-ap.srcs += nav_line.c
+ap.srcs += subsystems/navigation/nav_line.c
 
 # Harware In The Loop
 
@@ -254,7 +254,7 @@ ap.srcs += nav_line.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DUSE_BARO_MS5534A
-sim.srcs += nav_survey_rectangle.c $(SRC_ARCH)/sim_baro.c bomb.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c $(SRC_ARCH)/sim_baro.c bomb.c subsystems/navigation/nav_line.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/microjetI.xml b/conf/airframes/obsolete/microjetI.xml
index af6e82931b..bb85ad2a5b 100644
--- a/conf/airframes/obsolete/microjetI.xml
+++ b/conf/airframes/obsolete/microjetI.xml
@@ -227,7 +227,7 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 ap.CFLAGS += -DGYRO -DADXRS150
 ap.srcs += gyro.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 ap.srcs += light.c
 ap.CFLAGS += -DUSE_LIGHT
@@ -239,7 +239,7 @@ ap.srcs += adc_generic.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 sim.srcs += light.c
 sim.CFLAGS += -DUSE_LIGHT
diff --git a/conf/airframes/obsolete/microjetII.xml b/conf/airframes/obsolete/microjetII.xml
index 78863488e0..642fd245cb 100644
--- a/conf/airframes/obsolete/microjetII.xml
+++ b/conf/airframes/obsolete/microjetII.xml
@@ -226,7 +226,7 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 ap.CFLAGS += -DGYRO -DADXRS150
 ap.srcs += gyro.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 ap.srcs += traffic_info.c
 
 ap.srcs += light.c
@@ -235,7 +235,7 @@ ap.CFLAGS += -DUSE_LIGHT
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 sim.srcs += traffic_info.c
 
 
diff --git a/conf/airframes/obsolete/microjetIII.xml b/conf/airframes/obsolete/microjetIII.xml
index 6abce9569b..b05ea95208 100644
--- a/conf/airframes/obsolete/microjetIII.xml
+++ b/conf/airframes/obsolete/microjetIII.xml
@@ -217,7 +217,7 @@ ap.CFLAGS += -DGYRO -DADXRS150
 # -DESTIMATOR_COMP_FILTER
 ap.srcs += gyro.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 ap.srcs += traffic_info.c
 
 # PID sensor
@@ -228,7 +228,7 @@ ap.srcs += adc_generic.c
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 # -DESTIMATOR_COMP_FILTER
-sim.srcs += nav_survey_rectangle.c nav_line.c traffic_info.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c traffic_info.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/minimag_fs.xml b/conf/airframes/obsolete/minimag_fs.xml
index af6e79790b..3e23ac1a0a 100644
--- a/conf/airframes/obsolete/minimag_fs.xml
+++ b/conf/airframes/obsolete/minimag_fs.xml
@@ -212,7 +212,7 @@ ap.CFLAGS += -DINFRARED
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 ap.CFLAGS += -DDIGITAL_CAM
 ap.srcs += dc.c
@@ -220,7 +220,7 @@ ap.srcs += dc.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 sim.srcs += dc.c
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/slayer1.xml b/conf/airframes/obsolete/slayer1.xml
index 0bc85ef25a..9bd6243193 100644
--- a/conf/airframes/obsolete/slayer1.xml
+++ b/conf/airframes/obsolete/slayer1.xml
@@ -226,7 +226,7 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 ap.CFLAGS += -DGYRO -DIDG300
 ap.srcs += gyro.c bomb.c
@@ -244,7 +244,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += bomb.c nav_survey_rectangle.c nav_line.c
+sim.srcs += bomb.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/slayer3.xml b/conf/airframes/obsolete/slayer3.xml
index 97b3a7090a..69722dc5ee 100644
--- a/conf/airframes/obsolete/slayer3.xml
+++ b/conf/airframes/obsolete/slayer3.xml
@@ -245,7 +245,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN -DIR_360
-sim.srcs += nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/slayerJH.xml b/conf/airframes/obsolete/slayerJH.xml
index 9d212d1752..ba1d85efc4 100644
--- a/conf/airframes/obsolete/slayerJH.xml
+++ b/conf/airframes/obsolete/slayerJH.xml
@@ -209,7 +209,7 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c snav.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c snav.c
 
 
 ap.CFLAGS += -DGYRO -DADXRS150
@@ -228,7 +228,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c snav.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c snav.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/slicer1.xml b/conf/airframes/obsolete/slicer1.xml
index e191489c74..cd768de2ed 100644
--- a/conf/airframes/obsolete/slicer1.xml
+++ b/conf/airframes/obsolete/slicer1.xml
@@ -222,7 +222,7 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 ap.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH_ROLL
 # -DTEST_CAM
@@ -232,7 +232,7 @@ ap.srcs += cam.c point.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 sim.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH_ROLL
 sim.srcs += cam.c point.c traffic_info.c
 
diff --git a/conf/airframes/obsolete/slicer2.xml b/conf/airframes/obsolete/slicer2.xml
index bdf835c724..055be1ac30 100644
--- a/conf/airframes/obsolete/slicer2.xml
+++ b/conf/airframes/obsolete/slicer2.xml
@@ -204,12 +204,12 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/spirit.xml b/conf/airframes/obsolete/spirit.xml
index 7895604b5f..4c6236b313 100644
--- a/conf/airframes/obsolete/spirit.xml
+++ b/conf/airframes/obsolete/spirit.xml
@@ -221,7 +221,7 @@ ap.srcs += gyro.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Current sensor
 ap.CFLAGS += -DUSE_ADC_7 -DUSE_ADC_GENERIC -DADC_CHANNEL_GENERIC1=ADC_7 -DADC_CHANNEL_GENERIC_NB_SAMPLES=16
@@ -231,7 +231,7 @@ ap.srcs += adc_generic.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c traffic_info.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c traffic_info.c
 
 ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
 ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
diff --git a/conf/airframes/obsolete/spirit_proto.xml b/conf/airframes/obsolete/spirit_proto.xml
index cc28f09604..309493a731 100644
--- a/conf/airframes/obsolete/spirit_proto.xml
+++ b/conf/airframes/obsolete/spirit_proto.xml
@@ -206,7 +206,7 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Current sensor
 ap.CFLAGS += -DUSE_ADC_7 -DUSE_ADC_GENERIC -DADC_CHANNEL_GENERIC1=ADC_7 -DADC_CHANNEL_GENERIC_NB_SAMPLES=16
@@ -218,7 +218,7 @@ ap.srcs += adc_generic.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c traffic_info.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c traffic_info.c
 
 ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
 ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
diff --git a/conf/airframes/obsolete/storm1.xml b/conf/airframes/obsolete/storm1.xml
index ba0df05a65..43581594db 100644
--- a/conf/airframes/obsolete/storm1.xml
+++ b/conf/airframes/obsolete/storm1.xml
@@ -274,7 +274,7 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 # ap.CFLAGS += -DGYRO -DADXRS150
 # ap.srcs += gyro.c
 
-ap.srcs += traffic_info.c nav_survey_rectangle.c
+ap.srcs += traffic_info.c subsystems/navigation/nav_survey_rectangle.c
 ap.srcs += bomb.c
 
 # Hack to use the same tuning file than slayer1
@@ -291,7 +291,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN -DIR_360
 sim.srcs += traffic_info.c
-sim.srcs += bomb.c  nav_survey_rectangle.c
+sim.srcs += bomb.c  subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/tiny2.xml b/conf/airframes/obsolete/tiny2.xml
index 2afaff58bd..7aeaf9d9c1 100644
--- a/conf/airframes/obsolete/tiny2.xml
+++ b/conf/airframes/obsolete/tiny2.xml
@@ -231,7 +231,7 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN
 ap.srcs += infrared.c estimator.c
 
 ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
 # Harware In The Loop
 
@@ -241,7 +241,7 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/twinjet1.xml b/conf/airframes/obsolete/twinjet1.xml
index 20e828db3f..508c5d3c5b 100644
--- a/conf/airframes/obsolete/twinjet1.xml
+++ b/conf/airframes/obsolete/twinjet1.xml
@@ -224,10 +224,10 @@ ap.srcs += gyro.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
+ap.srcs += subsystems/navigation/nav_line.c
 # chemotaxis.c anemotaxis.c discsurvey.c
 
-ap.srcs += nav_survey_rectangle.c snav.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c snav.c
 
 ap.srcs += light.c
 ap.CFLAGS += -DUSE_LIGHT
@@ -244,9 +244,9 @@ test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_tran
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"classix.h\" -DAGR_CLIMB -DLOITER_TRIM
 #sim.srcs += traffic_info.c
-#sim.srcs += nav_line.c chemotaxis.c anemotaxis.c discsurvey.c bomb.c nav_survey_rectangle.c
+#sim.srcs += subsystems/navigation/nav_line.c chemotaxis.c anemotaxis.c discsurvey.c bomb.c subsystems/navigation/nav_survey_rectangle.c
 
-sim.srcs += nav_survey_rectangle.c nav_line.c light.c snav.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c light.c snav.c
 sim.CFLAGS += -DUSE_LIGHT
 
 sim.CFLAGS += -DUSE_ADC_GENERIC
diff --git a/conf/airframes/obsolete/twinstar1.xml b/conf/airframes/obsolete/twinstar1.xml
index 793ac767fa..ea64536b22 100644
--- a/conf/airframes/obsolete/twinstar1.xml
+++ b/conf/airframes/obsolete/twinstar1.xml
@@ -190,7 +190,7 @@ ap.CFLAGS += -DDATALINK -DPPRZ_INPUT -DPPRZ_UART=Uart0
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.srcs += nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c
 
  </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/twinstar6.xml b/conf/airframes/obsolete/twinstar6.xml
index fd1aae9dc9..c6d81db49e 100644
--- a/conf/airframes/obsolete/twinstar6.xml
+++ b/conf/airframes/obsolete/twinstar6.xml
@@ -223,9 +223,9 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
+ap.srcs += subsystems/navigation/nav_line.c
 
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 ap.CFLAGS += -DUSE_GPIO
 ap.srcs += $(SRC_ARCH)/gpio.c
@@ -244,8 +244,8 @@ test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_tran
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"classix.h\" -DAGR_CLIMB -DLOITER_TRIM
 #sim.srcs += traffic_info.c
-sim.srcs += nav_line.c nav_survey_rectangle.c dc.c
-# chemotaxis.c anemotaxis.c discsurvey.c bomb.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c dc.c
+# chemotaxis.c anemotaxis.c discsurvey.c bomb.c subsystems/navigation/nav_survey_rectangle.c
 
 
   </makefile>
diff --git a/conf/airframes/obsolete/tyto1.xml b/conf/airframes/obsolete/tyto1.xml
index 5f54570750..01f4fc8d75 100644
--- a/conf/airframes/obsolete/tyto1.xml
+++ b/conf/airframes/obsolete/tyto1.xml
@@ -240,13 +240,13 @@ ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
 ap.CFLAGS += -DGYRO -DADXRS150
-ap.srcs += gyro.c nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += gyro.c subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/obsolete/xxx1.xml b/conf/airframes/obsolete/xxx1.xml
index 80075e1d6e..96868c4c00 100644
--- a/conf/airframes/obsolete/xxx1.xml
+++ b/conf/airframes/obsolete/xxx1.xml
@@ -245,8 +245,8 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
 
 ap.CFLAGS += -DGYRO -DADXRS150
-ap.srcs += gyro.c nav_line.c
-ap.srcs += nav_survey_rectangle.c
+ap.srcs += gyro.c subsystems/navigation/nav_line.c
+ap.srcs += subsystems/navigation/nav_survey_rectangle.c
 
 
 # Hack to use the same tuning file than slayer1
@@ -271,7 +271,7 @@ ap.CFLAGS += -DMOBILE_CAM
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM
 # -DMOBILE_CAM
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 # cam_roll.c jeremy_tuning.c
 
 
diff --git a/conf/airframes/tiny_hitl.xml b/conf/airframes/tiny_hitl.xml
index c2fd383b88..b3b8cd44cd 100644
--- a/conf/airframes/tiny_hitl.xml
+++ b/conf/airframes/tiny_hitl.xml
@@ -198,7 +198,7 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c nav_survey_rectangle.c
+ap.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 # Configuration for Hardware In The Loop
 ap.CFLAGS += -DHITL -UGPS_CONFIGURE -UUART0_BAUD -DUART0_BAUD=B57600
@@ -206,7 +206,7 @@ ap.CFLAGS += -DHITL -UGPS_CONFIGURE -UUART0_BAUD -DUART0_BAUD=B57600
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c traffic_info.c
+sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c traffic_info.c
 
   </makefile>
 </airframe>
diff --git a/conf/autopilot/subsystems/fixedwing/navigation.makefile b/conf/autopilot/subsystems/fixedwing/navigation.makefile
index 195cfe2a5b..d90cfd35c9 100644
--- a/conf/autopilot/subsystems/fixedwing/navigation.makefile
+++ b/conf/autopilot/subsystems/fixedwing/navigation.makefile
@@ -6,6 +6,6 @@
 $(TARGET).CFLAGS += -DNAV
 $(TARGET).srcs += $(SRC_FIXEDWING)/nav.c
 $(TARGET).srcs += $(SRC_FIXEDWING)/traffic_info.c
-$(TARGET).srcs += $(SRC_FIXEDWING)/nav_survey_rectangle.c $(SRC_FIXEDWING)/nav_line.c
+$(TARGET).srcs += $(SRC_FIXEDWING)/subsystems/navigation/nav_survey_rectangle.c $(SRC_FIXEDWING)/subsystems/navigation/nav_line.c
 
 
diff --git a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
index 613a308a7c..bdbba7384d 100644
--- a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
+++ b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
@@ -6,9 +6,9 @@
 $(TARGET).CFLAGS += -DNAV
 $(TARGET).srcs += $(SRC_FIXEDWING)/nav.c
 $(TARGET).srcs += $(SRC_FIXEDWING)/traffic_info.c
-$(TARGET).srcs += $(SRC_FIXEDWING)/nav_survey_rectangle.c $(SRC_FIXEDWING)/nav_line.c
+$(TARGET).srcs += $(SRC_FIXEDWING)/subsystems/navigation/nav_survey_rectangle.c $(SRC_FIXEDWING)/subsystems/navigation/nav_line.c
 
-$(TARGET).srcs += $(SRC_FIXEDWING)/nav_cube.c
+$(TARGET).srcs += $(SRC_FIXEDWING)/subsystems/navigation/nav_cube.c
 $(TARGET).srcs += $(SRC_FIXEDWING)/discsurvey.c
 $(TARGET).srcs += $(SRC_FIXEDWING)/OSAMNav.c
 $(TARGET).srcs += $(SRC_FIXEDWING)/snav.c
diff --git a/conf/autopilot/twin_mcu.makefile b/conf/autopilot/twin_mcu.makefile
index 3fb0f7d7d7..c6544a96cb 100644
--- a/conf/autopilot/twin_mcu.makefile
+++ b/conf/autopilot/twin_mcu.makefile
@@ -1,4 +1,4 @@
-ap.srcs += $(SRC_FIRMWARE)/main_ap.c sys_time.c $(SRC_FIRMWARE)/main.c inter_mcu.c link_mcu.c gps_ubx.c gps.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c cam.c spi.c rc_settings.c latlong.c nav_survey_rectangle.c
+ap.srcs += $(SRC_FIRMWARE)/main_ap.c sys_time.c $(SRC_FIRMWARE)/main.c inter_mcu.c link_mcu.c gps_ubx.c gps.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c cam.c spi.c rc_settings.c latlong.c subsystems/navigation/nav_survey_rectangle.c
 ap.CFLAGS += -DMCU_SPI_LINK -DGPS -DUBX -DINFRARED -DRADIO_CONTROL -DINTER_MCU -DSPI_MASTER -DUSE_SPI -DNAV -DRADIO_CONTROL_SETTINGS
 
 fbw.srcs +=  sys_time.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main.c commands.c radio_control.c pprz_transport.c downlink.c  inter_mcu.c spi.c link_mcu.c