diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py
index aef0ddeffc6c2e78c5b8180294a2305a2c27d88b..eb93480848b870a9c5ec1ff92190315ad9282dcc 100644
--- a/Tools/px4params/srcscanner.py
+++ b/Tools/px4params/srcscanner.py
@@ -14,20 +14,13 @@ class SourceScanner(object):
         Scans provided path and passes all found contents to the parser using
         parser.Parse method.
         """
-        extensions1 = tuple([".h"])
-        extensions2 = tuple([".cpp", ".c"])
+        extensions1 = tuple([".c"])
         for srcdir in srcdirs:
-            for dirname, dirnames, filenames in os.walk(srcdir):
-                for filename in filenames:
-                    if filename.endswith(extensions1):
-                        path = os.path.join(dirname, filename)
-                        if not self.ScanFile(path, parser):
-                            return False
-                for filename in filenames:
-                    if filename.endswith(extensions2):
-                        path = os.path.join(dirname, filename)
-                        if not self.ScanFile(path, parser):
-                            return False
+            for filename in os.listdir(srcdir):
+                if filename.endswith(extensions1):
+                    path = os.path.join(srcdir, filename)
+                    if not self.ScanFile(path, parser):
+                        return False
         return True
 
     def ScanFile(self, path, parser):
diff --git a/cmake/configs/nuttx_aerocore2_default.cmake b/cmake/configs/nuttx_aerocore2_default.cmake
index ec8e1eee5fd89e5ffa1ed2e9dcaabf2dea4d495e..75b70dcdba6f379cdf937835bf0a18bf1ff4dca2 100644
--- a/cmake/configs/nuttx_aerocore2_default.cmake
+++ b/cmake/configs/nuttx_aerocore2_default.cmake
@@ -59,7 +59,7 @@ set(config_module_list
 	#drivers/test_ppm
 	#lib/rc/rc_tests
 	#modules/commander/commander_tests
-	#modules/controllib_test
+	#lib/controllib/controllib_test
 	#modules/mavlink/mavlink_tests
 	#modules/unit_test
 	#modules/uORB/uORB_tests
diff --git a/cmake/configs/nuttx_auav-x21_default.cmake b/cmake/configs/nuttx_auav-x21_default.cmake
index 9155eb448f0dbab9cb14caac1611be135d9a3209..becbfc40b168872f123df0bad191cc7e3b7f981e 100644
--- a/cmake/configs/nuttx_auav-x21_default.cmake
+++ b/cmake/configs/nuttx_auav-x21_default.cmake
@@ -79,7 +79,7 @@ set(config_module_list
 	drivers/test_ppm
 	#lib/rc/rc_tests
 	modules/commander/commander_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/mc_pos_control/mc_pos_control_tests
 	modules/unit_test
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 93ff9ed5f453776d4cea9339e1ebe57940304747..aadad97e93999b27c6634a002c18512e874b920c 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -81,7 +81,7 @@ set(config_module_list
 	drivers/test_ppm
 	modules/commander/commander_tests
 	modules/mc_pos_control/mc_pos_control_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/unit_test
 	modules/uORB/uORB_tests
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index db04fd4f9125385ea53fd9dfe749d7dd11e53d9b..76d74530ebdbc5e26b5fc1dae9b482d86514efdb 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -81,7 +81,7 @@ set(config_module_list
 	#drivers/test_ppm
 	#lib/rc/rc_tests
 	#modules/commander/commander_tests
-	#modules/controllib_test
+	#lib/controllib/controllib_test
 	#modules/mavlink/mavlink_tests
 	#modules/unit_test
 	#modules/uORB/uORB_tests
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index dff5ed959958656dd5460c9de87393949a702c71..db89ab552d08a5bb0f478cd4e46a85394179da20 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -80,7 +80,7 @@ set(config_module_list
 	#lib/rc/rc_tests
 	modules/commander/commander_tests
 	modules/mc_pos_control/mc_pos_control_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/unit_test
 	modules/uORB/uORB_tests
diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake
index b89e66022d58a69cf570a3404cec073ca441bf78..9f039e5c1a9832e1a9e3229c01a1cb4891823730 100644
--- a/cmake/configs/nuttx_px4fmu-v3_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake
@@ -85,7 +85,7 @@ set(config_module_list
 	drivers/test_ppm
 	#lib/rc/rc_tests
 	modules/commander/commander_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/mc_pos_control/mc_pos_control_tests
 	modules/unit_test
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 8f88b33f25696a5548ec9674a2607775fefce96c..cdfa37609e0edae70003f1694063c7160c4cf224 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -85,7 +85,7 @@ set(config_module_list
 	drivers/test_ppm
 	modules/commander/commander_tests
 	modules/mc_pos_control/mc_pos_control_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/unit_test
 	modules/uORB/uORB_tests
diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
index 676979c0c5c871eec9a81231871a0e040e9391dc..f1b57514586c55dc1ab59082865321367814b3ab 100644
--- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
@@ -86,7 +86,7 @@ set(config_module_list
 	drivers/test_ppm
 	#lib/rc/rc_tests
 	modules/commander/commander_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/mc_pos_control/mc_pos_control_tests
 	modules/unit_test
diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake
index 9c21e8d16eaaf4dba91c8253568ebda5ae90d24b..4bf9cea8791fa922ff85d01144de39757379bbf8 100644
--- a/cmake/configs/nuttx_px4fmu-v5_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake
@@ -84,7 +84,7 @@ set(config_module_list
 	drivers/test_ppm
 	#lib/rc/rc_tests
 	modules/commander/commander_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/mc_pos_control/mc_pos_control_tests
 	modules/unit_test
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 6fff137a85286f4cfc83b5bf36c1537b4fc6a967..2bf991ca5b78fcf0879ce5e46a531be5116a05dc 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -48,7 +48,7 @@ set(config_module_list
 	#drivers/test_ppm
 	lib/rc/rc_tests
 	modules/commander/commander_tests
-	modules/controllib_test
+	lib/controllib/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/mc_pos_control/mc_pos_control_tests
 	modules/unit_test
diff --git a/src/modules/controllib_test/CMakeLists.txt b/src/lib/controllib/controllib_test/CMakeLists.txt
similarity index 97%
rename from src/modules/controllib_test/CMakeLists.txt
rename to src/lib/controllib/controllib_test/CMakeLists.txt
index defed2035f68f108e5ab243b818e2cea90be342b..e1cf608bcbbbaf9e42e90bf8d3f6e41838f6fdc9 100644
--- a/src/modules/controllib_test/CMakeLists.txt
+++ b/src/lib/controllib/controllib_test/CMakeLists.txt
@@ -31,7 +31,7 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE modules__controllib_test
+	MODULE lib__controllib__controllib_test
 	MAIN controllib_test
 	COMPILE_FLAGS
 	SRCS
diff --git a/src/modules/controllib_test/blocks.cpp b/src/lib/controllib/controllib_test/blocks.cpp
similarity index 100%
rename from src/modules/controllib_test/blocks.cpp
rename to src/lib/controllib/controllib_test/blocks.cpp
diff --git a/src/modules/controllib_test/controllib_test_main.cpp b/src/lib/controllib/controllib_test/controllib_test_main.cpp
similarity index 100%
rename from src/modules/controllib_test/controllib_test_main.cpp
rename to src/lib/controllib/controllib_test/controllib_test_main.cpp
diff --git a/src/modules/controllib_test/test_params.c b/src/lib/controllib/controllib_test/test_params.c
similarity index 100%
rename from src/modules/controllib_test/test_params.c
rename to src/lib/controllib/controllib_test/test_params.c
diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt
index 8c2a9222df255e33ea4347c8aee5f3a4f884db8b..8388d371bcbbec5275287b326ba114c66444ae28 100644
--- a/src/modules/position_estimator_inav/CMakeLists.txt
+++ b/src/modules/position_estimator_inav/CMakeLists.txt
@@ -39,7 +39,6 @@ px4_add_module(
 	COMPILE_FLAGS
 	SRCS
 		position_estimator_inav_main.cpp
-		position_estimator_inav_params.cpp
 		inertial_filter.cpp
 	DEPENDS
 		platforms__common
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp b/src/modules/position_estimator_inav/params.c
similarity index 73%
rename from src/modules/position_estimator_inav/position_estimator_inav_params.cpp
rename to src/modules/position_estimator_inav/params.c
index 331e1ba004769bf1921ff2e2c0a45c344a8251a5..d1558801f7be51eda1a42a626bdc0fa3df999d99 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp
+++ b/src/modules/position_estimator_inav/params.c
@@ -343,72 +343,3 @@ PARAM_DEFINE_FLOAT(INAV_LIDAR_OFF, 0.0f);
  * @group Position Estimator INAV
  */
 PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
-
-int inav_parameters_init(struct position_estimator_inav_param_handles *h)
-{
-	h->w_z_baro = param_find("INAV_W_Z_BARO");
-	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
-	h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
-	h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
-	h->w_z_lidar = param_find("INAV_W_Z_LIDAR");
-	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
-	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
-	h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
-	h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
-	h->w_mocap_p = param_find("INAV_W_MOC_P");
-	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
-	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
-	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
-	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
-	h->flow_k = param_find("INAV_FLOW_K");
-	h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
-	h->lidar_err = param_find("INAV_LIDAR_ERR");
-	h->land_t = param_find("INAV_LAND_T");
-	h->land_disp = param_find("INAV_LAND_DISP");
-	h->land_thr = param_find("INAV_LAND_THR");
-	h->no_vision = param_find("CBRK_NO_VISION");
-	h->delay_gps = param_find("INAV_DELAY_GPS");
-	h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
-	h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
-	h->disable_mocap = param_find("INAV_DISAB_MOCAP");
-	h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST");
-	h->lidar_calibration_offset = param_find("INAV_LIDAR_OFF");
-	h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M");
-
-	return 0;
-}
-
-int inav_parameters_update(const struct position_estimator_inav_param_handles *h,
-			   struct position_estimator_inav_params *p)
-{
-	param_get(h->w_z_baro, &(p->w_z_baro));
-	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
-	param_get(h->w_z_gps_v, &(p->w_z_gps_v));
-	param_get(h->w_z_vision_p, &(p->w_z_vision_p));
-	param_get(h->w_z_lidar, &(p->w_z_lidar));
-	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
-	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
-	param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
-	param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
-	param_get(h->w_mocap_p, &(p->w_mocap_p));
-	param_get(h->w_xy_flow, &(p->w_xy_flow));
-	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
-	param_get(h->w_gps_flow, &(p->w_gps_flow));
-	param_get(h->w_acc_bias, &(p->w_acc_bias));
-	param_get(h->flow_k, &(p->flow_k));
-	param_get(h->flow_q_min, &(p->flow_q_min));
-	param_get(h->lidar_err, &(p->lidar_err));
-	param_get(h->land_t, &(p->land_t));
-	param_get(h->land_disp, &(p->land_disp));
-	param_get(h->land_thr, &(p->land_thr));
-	param_get(h->no_vision, &(p->no_vision));
-	param_get(h->delay_gps, &(p->delay_gps));
-	param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
-	param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
-	param_get(h->disable_mocap, &(p->disable_mocap));
-	param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est));
-	param_get(h->lidar_calibration_offset, &(p->lidar_calibration_offset));
-	param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m));
-
-	return 0;
-}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index c1c53eda0861b7a3c00dd434d067ae7e4e15fe16..0997126a513acc7e4e136fea29c8c2c8a1462643 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -1405,3 +1405,73 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	thread_running = false;
 	return 0;
 }
+
+
+int inav_parameters_init(struct position_estimator_inav_param_handles *h)
+{
+	h->w_z_baro = param_find("INAV_W_Z_BARO");
+	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
+	h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
+	h->w_z_lidar = param_find("INAV_W_Z_LIDAR");
+	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
+	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
+	h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
+	h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
+	h->w_mocap_p = param_find("INAV_W_MOC_P");
+	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
+	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
+	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
+	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
+	h->flow_k = param_find("INAV_FLOW_K");
+	h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
+	h->lidar_err = param_find("INAV_LIDAR_ERR");
+	h->land_t = param_find("INAV_LAND_T");
+	h->land_disp = param_find("INAV_LAND_DISP");
+	h->land_thr = param_find("INAV_LAND_THR");
+	h->no_vision = param_find("CBRK_NO_VISION");
+	h->delay_gps = param_find("INAV_DELAY_GPS");
+	h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
+	h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
+	h->disable_mocap = param_find("INAV_DISAB_MOCAP");
+	h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST");
+	h->lidar_calibration_offset = param_find("INAV_LIDAR_OFF");
+	h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M");
+
+	return 0;
+}
+
+int inav_parameters_update(const struct position_estimator_inav_param_handles *h,
+			   struct position_estimator_inav_params *p)
+{
+	param_get(h->w_z_baro, &(p->w_z_baro));
+	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
+	param_get(h->w_z_gps_v, &(p->w_z_gps_v));
+	param_get(h->w_z_vision_p, &(p->w_z_vision_p));
+	param_get(h->w_z_lidar, &(p->w_z_lidar));
+	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
+	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
+	param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
+	param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
+	param_get(h->w_mocap_p, &(p->w_mocap_p));
+	param_get(h->w_xy_flow, &(p->w_xy_flow));
+	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
+	param_get(h->w_gps_flow, &(p->w_gps_flow));
+	param_get(h->w_acc_bias, &(p->w_acc_bias));
+	param_get(h->flow_k, &(p->flow_k));
+	param_get(h->flow_q_min, &(p->flow_q_min));
+	param_get(h->lidar_err, &(p->lidar_err));
+	param_get(h->land_t, &(p->land_t));
+	param_get(h->land_disp, &(p->land_disp));
+	param_get(h->land_thr, &(p->land_thr));
+	param_get(h->no_vision, &(p->no_vision));
+	param_get(h->delay_gps, &(p->delay_gps));
+	param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
+	param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
+	param_get(h->disable_mocap, &(p->disable_mocap));
+	param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est));
+	param_get(h->lidar_calibration_offset, &(p->lidar_calibration_offset));
+	param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m));
+
+	return 0;
+}