diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95c16d1bdd..8ca5bfe03b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -388,6 +388,7 @@ add_subdirectory(msg)
 px4_generate_messages(TARGET msg_gen
 	MSG_FILES ${msg_files}
 	OS ${OS}
+	INCLUDES ${msg_include_paths}
 	DEPENDS git_genmsg git_gencpp prebuild_targets
 	)
 px4_generate_parameters_xml(OUT parameters.xml 
diff --git a/Tools/px_generate_uorb_topic_files.py b/Tools/px_generate_uorb_topic_files.py
index 1e6955f8f7..311eaa38c1 100755
--- a/Tools/px_generate_uorb_topic_files.py
+++ b/Tools/px_generate_uorb_topic_files.py
@@ -79,7 +79,7 @@ __email__ = "thomasgubler@gmail.com"
 TEMPLATE_FILE = ['msg.h.template', 'msg.cpp.template']
 TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template'
 OUTPUT_FILE_EXT = ['.h', '.cpp']
-INCL_DEFAULT = ['std_msgs:./msg/std_msgs','px4:%s'%(px4_msg_dir)]
+INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
 PACKAGE = 'px4'
 TOPICS_TOKEN = '# TOPICS '
 
@@ -279,7 +279,11 @@ def generate_topics_list_file_from_files(files, outputdir, templatedir):
         tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE)
         tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", ""))
         generate_by_template(tl_out_file, tl_template_file, tl_globals)
-        
+
+def append_to_include_path(path_to_append, curr_include):
+    for p in path_to_append:
+        curr_include.append("%s:%s" % (PACKAGE, p))
+
 if __name__ == "__main__":
         parser = argparse.ArgumentParser(
             description='Convert msg files to uorb headers/sources')
@@ -291,6 +295,9 @@ if __name__ == "__main__":
         parser.add_argument('-f', dest='file',
                             help="files to convert (use only without -d)",
                             nargs="+")
+        parser.add_argument('-i', dest="include_paths",
+                            help='Additional Include Paths', nargs="*",
+                            default=None)
         parser.add_argument('-e', dest='templatedir',
                             help='directory with template files',)
         parser.add_argument('-o', dest='outputdir',
@@ -305,6 +312,9 @@ if __name__ == "__main__":
                             ' name when converting directories')
         args = parser.parse_args()
 
+        if args.include_paths:
+            append_to_include_path(args.include_paths, INCL_DEFAULT)
+
         if args.headers:
             generate_idx = 0
         elif args.sources:
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index cdb867a52a..1fca803b7c 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -407,6 +407,7 @@ function(px4_generate_messages)
 			--headers
 			${QUIET}
 			-f ${MSG_FILES}
+			-i ${INCLUDES}
 			-o ${msg_out_path}
 			-e msg/templates/uorb
 			-t ${PX4_BINARY_DIR}/topics_temporary_header
@@ -428,6 +429,7 @@ function(px4_generate_messages)
 			--sources
 			${QUIET}
 			-f ${MSG_FILES}
+			-i ${INCLUDES}
 			-o ${msg_source_out_path}
 			-e msg/templates/uorb
 			-t ${PX4_BINARY_DIR}/topics_temporary_sources
@@ -458,6 +460,7 @@ function(px4_generate_messages)
 			--headers
 			${QUIET}
 			-f ${MSG_FILES}
+			-i ${INCLUDES}
 			-o ${msg_multi_out_path}
 			-e msg/templates/px4/uorb
 			-t ${PX4_BINARY_DIR}/multi_topics_temporary/${OS}
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 083c93a958..1ed3ef568b 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -129,10 +129,15 @@ set(msg_file_names
 
 # Get absolute paths
 set(msg_files)
+set(msg_include_paths)
+
 foreach(msg_file ${msg_file_names})
 	list(APPEND msg_files ${CMAKE_CURRENT_SOURCE_DIR}/${msg_file})
 endforeach()
 
+list(APPEND msg_include_paths ${CMAKE_CURRENT_SOURCE_DIR})
+set(msg_include_paths ${msg_include_paths} PARENT_SCOPE)
+
 set(msg_files ${msg_files} PARENT_SCOPE)
 
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :