diff --git a/.gitmodules b/.gitmodules
index c5188f49e9..8b8536f09c 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -13,9 +13,6 @@
 [submodule "Tools/gencpp"]
 	path = Tools/gencpp
 	url = https://github.com/ros/gencpp.git
-[submodule "src/lib/dspal"]
-	path = src/lib/dspal
-	url = https://github.com/ATLFlight/dspal.git
 [submodule "Tools/jMAVSim"]
 	path = Tools/jMAVSim
 	url = https://github.com/PX4/jMAVSim.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9bd26e839c..e2ce018f91 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -243,12 +243,12 @@ px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
 px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
 px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
 px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
-px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
 px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework")
 px4_add_git_submodule(TARGET git_ecl PATH "src/lib/ecl")
 px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
 px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
 px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
+px4_add_git_submodule(TARGET git_cmake_hexagon PATH "cmake/cmake_hexagon")
 
 add_custom_target(submodule_clean
 	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
@@ -293,6 +293,7 @@ px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags}" GLUE " ")
 px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
 
 include_directories(${include_dirs})
+message("INCLUDE_DIRS=${include_dirs}")
 link_directories(${link_dirs})
 add_definitions(${definitions})
 
@@ -329,7 +330,11 @@ execute_process(COMMAND cmake -E make_directory ${ep_base}/Install/include)
 #
 set(module_libraries)
 foreach(module ${config_module_list})
-	add_subdirectory(src/${module})
+	if(module MATCHES "Eagle")
+		add_subdirectory(${module} ${CMAKE_BINARY_DIR}/${module})
+	else() 
+		add_subdirectory(src/${module})
+	endif()
 	px4_mangle_name(${module} mangled_name)
 	list(APPEND module_libraries ${mangled_name})
 	#message(STATUS "adding module: ${module}")
diff --git a/Makefile b/Makefile
index 1d2e4cb1cc..919fabac24 100644
--- a/Makefile
+++ b/Makefile
@@ -155,6 +155,9 @@ posix_sitl_ekf2:
 ros_sitl_default:
 	@echo "This target is deprecated. Use make 'posix_sitl_default gazebo' instead."
 
+ros_sitl_default:
+	$(call cmake-build,$@)
+
 qurt_eagle_travis:
 	$(call cmake-build,$@)
 
@@ -163,7 +166,7 @@ qurt_eagle_release:
 
 posix_eagle_release:
 	$(call cmake-build,$@)
-
+	
 qurt_eagle_default:
 	$(call cmake-build,$@)
 
diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index 829f22eff3..720078ffe4 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit 829f22eff345c934ca8939b2385768ca5e33794c
+Subproject commit 720078ffe4964ee87742248f4563633d1f2b852c
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 82e417f48f..0cf6b6504c 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -90,7 +90,7 @@ include(CMakeParseArguments)
 #		endfunction()
 #
 #		test(NAME "hello" LIST a b c)
-#		
+#
 #		OUTPUT:
 #			name: hello
 #			list: a b c
@@ -349,11 +349,11 @@ function(px4_generate_messages)
 		list(APPEND msg_files_out ${msg_out_path}/${msg}.h)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_files_out}
-		COMMAND ${PYTHON_EXECUTABLE} 
+		COMMAND ${PYTHON_EXECUTABLE}
 			Tools/px_generate_uorb_topic_headers.py
 			${QUIET}
 			-d msg
-			-o ${msg_out_path} 
+			-o ${msg_out_path}
 			-e msg/templates/uorb
 			-t ${CMAKE_BINARY_DIR}/topics_temporary
 		DEPENDS ${DEPENDS} ${MSG_FILES}
@@ -370,11 +370,11 @@ function(px4_generate_messages)
 		list(APPEND msg_multi_files_out ${msg_multi_out_path}/px4_${msg}.h)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_multi_files_out}
-		COMMAND ${PYTHON_EXECUTABLE} 
+		COMMAND ${PYTHON_EXECUTABLE}
 			Tools/px_generate_uorb_topic_headers.py
 			${QUIET}
 			-d msg
-			-o ${msg_multi_out_path} 
+			-o ${msg_multi_out_path}
 			-e msg/templates/px4/uorb
 			-t ${CMAKE_BINARY_DIR}/multi_topics_temporary/${OS}
 			-p "px4_"
@@ -556,6 +556,7 @@ function(px4_add_common_flags)
 			-funsafe-math-optimizations
 			-ffunction-sections
 			-fdata-sections
+			-fPIC
 			)
 	endif()
 
@@ -685,7 +686,7 @@ endfunction()
 #	Input:
 #		dirname					: path to module dir
 #
-#	Output: 
+#	Output:
 #		newname					: module name
 #
 #	Example:
@@ -716,8 +717,8 @@ endfunction()
 function(px4_create_git_hash_header)
 	px4_parse_function_args(
 		NAME px4_create_git_hash_header
-		ONE_VALUE HEADER 
-		REQUIRED HEADER 
+		ONE_VALUE HEADER
+		REQUIRED HEADER
 		ARGN ${ARGN})
 	execute_process(
 		COMMAND git rev-parse HEAD
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 9b2c674167..edf597406e 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -83,6 +83,7 @@ set(config_module_list
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
+	modules/ekf2
 
 	#
 	# Vehicle Control
diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake
index 9de00fb694..98b25b612f 100644
--- a/cmake/configs/posix_eagle_default.cmake
+++ b/cmake/configs/posix_eagle_default.cmake
@@ -1,6 +1,9 @@
 include(posix/px4_impl_posix)
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake)
+
+set(CONFIG_SHMEM "1")
 
 set(config_module_list
 	drivers/device
diff --git a/cmake/configs/posix_eagle_release.cmake b/cmake/configs/posix_eagle_release.cmake
index b0b2a9171d..cb1a005ed0 100644
--- a/cmake/configs/posix_eagle_release.cmake
+++ b/cmake/configs/posix_eagle_release.cmake
@@ -16,8 +16,14 @@ set(CMAKE_PROGRAM_PATH
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
 
+set(CONFIG_SHMEM "1")
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DENABLE_SHMEM")
+
 set(config_module_list
 	drivers/device
+	drivers/boards/sitl
+	drivers/led
 
 	systemcmds/param
 	systemcmds/ver
@@ -28,6 +34,9 @@ set(config_module_list
 	modules/systemlib
 	modules/uORB
 	modules/dataman
+	modules/sdlog2
+	modules/simulator
+	modules/commander
 
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake
index 1e7beed49c..7517df4e22 100644
--- a/cmake/configs/qurt_eagle_release.cmake
+++ b/cmake/configs/qurt_eagle_release.cmake
@@ -1,18 +1,38 @@
 include(qurt/px4_impl_qurt)
 
-if ("${HEXAGON_DRIVERS_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set")
+if ("$ENV{EAGLE_ADDON_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Enviroment variable EAGLE_ADDON_ROOT must be set")
+else()
+	set(EAGLE_ADDON_ROOT $ENV{EAGLE_ADDON_ROOT})
 endif()
 
-if ("${EAGLE_DRIVERS_SRC}" STREQUAL "")
-	message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set")
+if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
+else()
+	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-include_directories(${HEXAGON_DRIVERS_ROOT}/inc)
+if ("$ENV{EAGLE_DRIVERS_SRC}" STREQUAL "")
+	message(FATAL_ERROR "Environment variable EAGLE_DRIVERS_SRC must be set")
+else()
+	set(EAGLE_DRIVERS_SRC $ENV{EAGLE_DRIVERS_SRC})
+endif()
+
+STRING(REGEX REPLACE "//" "/" EAGLE_DRIVERS_SRC ${EAGLE_DRIVERS_SRC})
+STRING(REGEX REPLACE "/" "__" EAGLE_DRIVERS_MODULE_PREFIX ${EAGLE_DRIVERS_SRC})
+
+include_directories(${EAGLE_ADDON_ROOT}/flight_controller/hexagon/inc)
+include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
+
+message("hexagon_sdk_root is ${HEXAGON_SDK_ROOT}")
+
+set(QURT_ENABLE_STUBS "0")
+
+set(CONFIG_SHMEM "1")
 
 # For Actual flight we need to link against the driver dynamic libraries
 set(target_libraries
-	-L${HEXAGON_DRIVERS_ROOT}/libs 
+	-L${EAGLE_ADDON_ROOT}/flight_controller/hexagon/libs 
 	mpu9x50
 	uart_esc
 	csr_gps
@@ -29,10 +49,10 @@ set(config_module_list
 	#
 	drivers/device
 	modules/sensors
-	$(EAGLE_DRIVERS_SRC)/mpu9x50
-	$(EAGLE_DRIVERS_SRC)/uart_esc
-	$(EAGLE_DRIVERS_SRC)/rc_receiver
-	$(EAGLE_DRIVERS_SRC)/csr_gps
+	${EAGLE_DRIVERS_SRC}/mpu9x50
+	${EAGLE_DRIVERS_SRC}/uart_esc
+	${EAGLE_DRIVERS_SRC}/rc_receiver
+	${EAGLE_DRIVERS_SRC}/csr_gps
 
 	#
 	# System commands
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index 4550c504cc..ecae71721c 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -1,11 +1,19 @@
 include(qurt/px4_impl_qurt)
 
 # Run a full link with build stubs to make sure qurt target isn't broken
-set(QURT_ENABLE_STUBS "1")
+set(QURT_ENABLE_STUBS "0")
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake)
 
+if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
+else()
+	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
+endif()
+
+include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
+
 set(config_module_list
 	drivers/device
 	drivers/boards/sitl
diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake
index 4c2dfbb709..9dbc5ccdb2 100644
--- a/cmake/posix/px4_impl_posix.cmake
+++ b/cmake/posix/px4_impl_posix.cmake
@@ -182,6 +182,7 @@ if(UNIX AND APPLE)
 
 else()
 
+
         set(added_definitions
 		-D__PX4_POSIX
 		-D__PX4_LINUX 
@@ -196,12 +197,33 @@ else()
 		)
 
 endif()
+
+if ("${BOARD}" STREQUAL "eagle")
+
+	if ("$ENV{HEXAGON_ARM_SYSROOT}" STREQUAL "")
+		message(FATAL_ERROR "HEXAGON_ARM_SYSROOT not set")
+	else()
+		set(HEXAGON_ARM_SYSROOT $ENV{HEXAGON_ARM_SYSROOT})
+	endif()
 	
+	# Add the toolchain specific flags
+	set(HEXAGON_ARM_SYSROOT ${HEXAGON_SDK_ROOT}/sysroot)
+        set(added_cflags ${POSIX_CMAKE_C_FLAGS} --sysroot=${HEXAGON_ARM_SYSROOT})
+        set(added_cxx_flags ${POSIX_CMAKE_CXX_FLAGS} --sysroot=${HEXAGON_ARM_SYSROOT})
+
+        list(APPEND added_exe_linker_flags
+		-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib/arm-linux-gnueabihf
+		-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib/arm-linux-gnueabihf
+		--sysroot=${HEXAGON_ARM_SYSROOT}
+		)
+else()
 
 	# Add the toolchain specific flags
         set(added_cflags ${POSIX_CMAKE_C_FLAGS})
         set(added_cxx_flags ${POSIX_CMAKE_CXX_FLAGS})
 
+endif()
+
 	# output
 	foreach(var ${inout_vars})
 		string(TOLOWER ${var} lower_var)
diff --git a/cmake/qurt/apps.h_in b/cmake/qurt/apps.h_in
index f4d240d529..ff06f3e370 100644
--- a/cmake/qurt/apps.h_in
+++ b/cmake/qurt/apps.h_in
@@ -36,9 +36,9 @@ ${builtin_apps_string}
 
 void list_builtins(map<string,px4_main_t> &apps)
 {
-	printf("Builtin Commands:\\n");
+	PX4_INFO("Builtin Commands:\\n");
 	for (map<string,px4_main_t>::iterator it=apps.begin(); it!=apps.end(); ++it)
-		printf("\\t%s\\n", (it->first).c_str());
+		PX4_INFO("%s : 0x%x\n", (it->first).c_str(), it->second);
 }
 
 static int shutdown_main(int argc, char *argv[])
diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake
index 0f7441c158..557b428ed9 100644
--- a/cmake/qurt/px4_impl_qurt.cmake
+++ b/cmake/qurt/px4_impl_qurt.cmake
@@ -158,7 +158,7 @@ function(px4_os_add_flags)
 		LINK_DIRS ${LINK_DIRS}
 		DEFINITIONS ${DEFINITIONS})
 
-        set(DSPAL_ROOT src/lib/dspal)
+        set(DSPAL_ROOT src/lib/DriverFramework/dspal)
         set(added_include_dirs
                 ${DSPAL_ROOT}/include 
                 ${DSPAL_ROOT}/sys 
@@ -222,7 +222,7 @@ function(px4_os_prebuild_targets)
 			ONE_VALUE OUT BOARD THREADS
 			REQUIRED OUT BOARD
 			ARGN ${ARGN})
-	add_custom_target(${OUT} DEPENDS git_dspal)
+	add_custom_target(${OUT} DEPENDS git_driverframework)
 
 endfunction()
 
diff --git a/cmake/qurt/qurt_funcs.cmake b/cmake/qurt/qurt_funcs.cmake
index 4690799b2b..9987b84c07 100644
--- a/cmake/qurt/qurt_funcs.cmake
+++ b/cmake/qurt/qurt_funcs.cmake
@@ -45,4 +45,3 @@
 #
 # The macros are called from the top level CMakeLists.txt
 #
-px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
index eb48554b16..597c6c074e 100644
--- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
+++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
@@ -17,6 +17,12 @@
 
 include(CMakeForceCompiler)
 
+if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
+        message(FATAL_ERROR "HEXAGON_SDK_ROOT not set")
+else()
+        set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_SDK_ROOT})
+endif()
+
 # this one is important
 set(CMAKE_SYSTEM_NAME Generic)
 
@@ -24,13 +30,19 @@ set(CMAKE_SYSTEM_NAME Generic)
 set(CMAKE_SYSTEM_VERSION 1)
 
 # specify the cross compiler
-find_program(C_COMPILER arm-linux-gnueabihf-gcc)
+find_program(C_COMPILER arm-linux-gnueabihf-gcc
+	PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
+	NO_DEFAULT_PATH
+	)
 if(NOT C_COMPILER)
 	message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler")
 endif()
 cmake_force_c_compiler(${C_COMPILER} GNU)
 
-find_program(CXX_COMPILER arm-linux-gnueabihf-g++)
+find_program(CXX_COMPILER arm-linux-gnueabihf-g++
+	PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
+	NO_DEFAULT_PATH
+	)
 if(NOT CXX_COMPILER)
 	message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler")
 endif()
@@ -39,7 +51,10 @@ cmake_force_cxx_compiler(${CXX_COMPILER} GNU)
 # compiler tools
 foreach(tool objcopy nm ld)
 	string(TOUPPER ${tool} TOOL)
-	find_program(${TOOL} arm-linux-gnueabihf-${tool})
+	find_program(${TOOL} arm-linux-gnueabihf-${tool}
+		PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
+		NO_DEFAULT_PATH
+		)
 	if(NOT ${TOOL})
 		message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}")
 	endif()
@@ -54,8 +69,11 @@ foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
 	endif()
 endforeach()
 
+set(C_FLAGS "--sysroot=${HEXAGON_SDK_ROOT}/sysroot")
 set(LINKER_FLAGS "-Wl,-gc-sections")
 set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS})
+set(CMAKE_C_FLAGS ${C_FLAGS})
+set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS})
 
 # where is the target environment 
 set(CMAKE_FIND_ROOT_PATH  get_file_component(${C_COMPILER} PATH))
diff --git a/posix-configs/eagle/200qx/mainapp-calib.config b/posix-configs/eagle/200qx/mainapp-calib.config
new file mode 100644
index 0000000000..7130d85ded
--- /dev/null
+++ b/posix-configs/eagle/200qx/mainapp-calib.config
@@ -0,0 +1,8 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
+commander start
diff --git a/posix-configs/eagle/200qx/mainapp-flight.config b/posix-configs/eagle/200qx/mainapp-flight.config
new file mode 100644
index 0000000000..b511bb1121
--- /dev/null
+++ b/posix-configs/eagle/200qx/mainapp-flight.config
@@ -0,0 +1,7 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
diff --git a/posix-configs/eagle/200qx/px4-calib.config b/posix-configs/eagle/200qx/px4-calib.config
new file mode 100644
index 0000000000..5cda1c9323
--- /dev/null
+++ b/posix-configs/eagle/200qx/px4-calib.config
@@ -0,0 +1,72 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+param set RC_RECEIVER_TYPE 1
+rc_receiver start -D /dev/tty-1
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+sleep 1
+param set SYS_AUTOSTART 4001
+param set SYS_AUTOCONFIG 1
+param set MAV_TYPE 2​
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+sensors start
+param set MC_YAW_P 7.0
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.0001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.0001
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set SENS_BOARD_ROT 0
+param set MPU_GYRO_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SAMPLE_RATE_ENUM 2
+sleep 1
+mpu9x50 start -D /dev/spi-1
+uart_esc start -D /dev/tty-2
+csr_gps start -D /dev/tty-3
+list_devices
+list_files
+list_tasks
+list_topics
diff --git a/posix-configs/eagle/200qx/px4-flight.config b/posix-configs/eagle/200qx/px4-flight.config
new file mode 100644
index 0000000000..814f375ae8
--- /dev/null
+++ b/posix-configs/eagle/200qx/px4-flight.config
@@ -0,0 +1,80 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+commander start
+param set RC_RECEIVER_TYPE 1
+rc_receiver start -D /dev/tty-1
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+sleep 1
+param set SYS_AUTOSTART 4001
+param set SYS_AUTOCONFIG 1
+param set MAV_TYPE 2​
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+sensors start
+param set MC_YAW_P 7.0
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.0001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.0001
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set SENS_BOARD_ROT 0
+param set MPU_GYRO_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SAMPLE_RATE_ENUM 2
+sleep 1
+mpu9x50 start -D /dev/spi-1
+param set UART_ESC_MODEL 0
+param set UART_ESC_BAUDRATE 250000
+param set UART_ESC_PX4MOTOR1 2
+param set UART_ESC_PX4MOTOR2 4
+param set UART_ESC_PX4MOTOR3 1
+param set UART_ESC_PX4MOTOR4 3
+sleep 1
+uart_esc start -D /dev/tty-2
+csr_gps start -D /dev/tty-3
+list_devices
+list_files
+list_tasks
+list_topics
diff --git a/posix-configs/eagle/210qc/mainapp-calib.config b/posix-configs/eagle/210qc/mainapp-calib.config
new file mode 100644
index 0000000000..7130d85ded
--- /dev/null
+++ b/posix-configs/eagle/210qc/mainapp-calib.config
@@ -0,0 +1,8 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
+commander start
diff --git a/posix-configs/eagle/210qc/mainapp-flight.config b/posix-configs/eagle/210qc/mainapp-flight.config
new file mode 100644
index 0000000000..b511bb1121
--- /dev/null
+++ b/posix-configs/eagle/210qc/mainapp-flight.config
@@ -0,0 +1,7 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
diff --git a/posix-configs/eagle/210qc/px4-calib.config b/posix-configs/eagle/210qc/px4-calib.config
new file mode 100644
index 0000000000..5cda1c9323
--- /dev/null
+++ b/posix-configs/eagle/210qc/px4-calib.config
@@ -0,0 +1,72 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+param set RC_RECEIVER_TYPE 1
+rc_receiver start -D /dev/tty-1
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+sleep 1
+param set SYS_AUTOSTART 4001
+param set SYS_AUTOCONFIG 1
+param set MAV_TYPE 2​
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+sensors start
+param set MC_YAW_P 7.0
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.0001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.0001
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set SENS_BOARD_ROT 0
+param set MPU_GYRO_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SAMPLE_RATE_ENUM 2
+sleep 1
+mpu9x50 start -D /dev/spi-1
+uart_esc start -D /dev/tty-2
+csr_gps start -D /dev/tty-3
+list_devices
+list_files
+list_tasks
+list_topics
diff --git a/posix-configs/eagle/210qc/px4-flight.config b/posix-configs/eagle/210qc/px4-flight.config
new file mode 100644
index 0000000000..4d152f593d
--- /dev/null
+++ b/posix-configs/eagle/210qc/px4-flight.config
@@ -0,0 +1,95 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+commander start
+param set RC_RECEIVER_TYPE 1
+rc_receiver start -D /dev/tty-1
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+param set MC_YAW_P 12
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.001
+sleep 1
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+sensors start
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set SENS_BOARD_ROT 0
+param set MPU_GYRO_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SAMPLE_RATE_ENUM 2
+param set GYRO0_XOFF -0.0028
+param set GYRO0_YOFF -0.0047
+param set GYRO0_ZOFF -0.0034
+param set GYRO0_XSCALE 1.0000
+param set GYRO0_YSCALE 1.0000
+param set GYRO0_ZSCALE 1.0000
+param set MAG0_XOFF 0.0000
+param set MAG0_YOFF 0.0000
+param set MAG0_ZOFF 0.0000
+param set MAG0_XSCALE 1.0000
+param set MAG0_YSCALE 1.0000
+param set MAG0_ZSCALE 1.0000
+param set ACC0_XOFF -0.0941
+param set ACC0_YOFF 0.1168
+param set ACC0_ZOFF -0.0398
+param set ACC0_XSCALE 1.0004
+param set ACC0_YSCALE 0.9974
+param set ACC0_ZSCALE 0.9951
+sleep 1
+mpu9x50 start -D /dev/spi-1
+param set UART_ESC_MODEL 2
+param set UART_ESC_BAUDRATE 250000
+param set UART_ESC_PX4MOTOR1 4
+param set UART_ESC_PX4MOTOR2 2
+param set UART_ESC_PX4MOTOR3 1
+param set UART_ESC_PX4MOTOR4 3
+sleep 1
+uart_esc start -D /dev/tty-2
+csr_gps start -D /dev/tty-3
+list_devices
+list_files
+list_tasks
+list_topics
diff --git a/posix-configs/eagle/flight/mainapp-calib.config b/posix-configs/eagle/flight/mainapp-calib.config
new file mode 100644
index 0000000000..7130d85ded
--- /dev/null
+++ b/posix-configs/eagle/flight/mainapp-calib.config
@@ -0,0 +1,8 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
+commander start
diff --git a/posix-configs/eagle/flight/mainapp-flight.config b/posix-configs/eagle/flight/mainapp-flight.config
new file mode 100644
index 0000000000..b511bb1121
--- /dev/null
+++ b/posix-configs/eagle/flight/mainapp-flight.config
@@ -0,0 +1,7 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
diff --git a/posix-configs/eagle/flight/px4-calib.config b/posix-configs/eagle/flight/px4-calib.config
new file mode 100644
index 0000000000..ccfd8dc525
--- /dev/null
+++ b/posix-configs/eagle/flight/px4-calib.config
@@ -0,0 +1,56 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+param set RC_RECEIVER_TYPE 1
+rc_receiver start -D /dev/tty-1
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+sleep 1
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+sensors start
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set MPU_GYRO_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SAMPLE_RATE_ENUM 2
+sleep 1
+mpu9x50 start -D /dev/spi-1
+uart_esc start -D /dev/tty-2
+csr_gps start -D /dev/tty-3
+list_devices
+list_files
+list_tasks
+list_topics
diff --git a/posix-configs/eagle/flight/px4-flight.config b/posix-configs/eagle/flight/px4-flight.config
new file mode 100644
index 0000000000..4966e0d9c8
--- /dev/null
+++ b/posix-configs/eagle/flight/px4-flight.config
@@ -0,0 +1,58 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+commander start
+param set RC_RECEIVER_TYPE 1
+rc_receiver start -D /dev/tty-1
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+sleep 1
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+sensors start
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set MPU_GYRO_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SAMPLE_RATE_ENUM 2
+sleep 1
+mpu9x50 start -D /dev/spi-1
+uart_esc start -D /dev/tty-2
+csr_gps start -D /dev/tty-3
+pressure start -D /dev/i2c-2
+list_devices
+list_files
+list_tasks
+list_topics
diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config
new file mode 100644
index 0000000000..743307e42f
--- /dev/null
+++ b/posix-configs/eagle/hil/mainapphil.config
@@ -0,0 +1,8 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
+simulator start -p
diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config
new file mode 100644
index 0000000000..8b626bd322
--- /dev/null
+++ b/posix-configs/eagle/hil/px4.config
@@ -0,0 +1,53 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+commander start -hil
+sensors start
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+sleep 1
+pwm_out_sim mode_pwm
+param set RC1_MAX 2015
+param set RC1_MIN 996
+param set RC1_TRIM 1502
+param set RC1_REV -1
+param set RC2_MAX 2016
+param set RC2_MIN 995
+param set RC2_TRIM 1500
+param set RC3_MAX 2003
+param set RC3_MIN 992
+param set RC3_TRIM 992
+param set RC4_MAX 2011
+param set RC4_MIN 997
+param set RC4_TRIM 1504
+param set RC4_REV -1
+param set RC6_MAX 2016
+param set RC6_MIN 992
+param set RC6_TRIM 1504
+param set RC_CHAN_CNT 8
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 7
+param set RC_MAP_RETURN_SW 8
+param set MC_YAW_P 1.5
+param set MC_PITCH_P 3.0
+param set MC_ROLL_P 3.0
+param set MC_YAWRATE_P 0.2
+param set MC_PITCHRATE_P 0.03
+param set MC_ROLLRATE_P 0.03
+param set ATT_W_ACC 0.0002
+param set ATT_W_MAG 0.002
+param set ATT_W_GYRO_BIAS 0.05
+sleep 1
+param set MAV_TYPE 2
+mixer load /dev/pwm_output0 /startup/quad_x.main.mix
+list_devices
+list_files
+list_tasks
+list_topics
+sleep 10
+list_tasks
+sleep 10
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 88f4c383b6..f6678092b7 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -1,17 +1,20 @@
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 
-
 px4_posix_generate_builtin_commands(
 	OUT apps.h
 	MODULE_LIST ${module_libraries})
 
-add_executable(mainapp
-	${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
-	apps.h
-	)
+if ("${BOARD}" STREQUAL "eagle")
+	FASTRPC_STUB_GEN(../qurt/px4muorb.idl)
 
-if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE)
-	target_link_libraries(mainapp
+	QURT_APP(
+		APP_NAME mainapp
+		IDL_NAME px4muorb
+		APPS_DEST "/home/linaro"
+		SOURCES
+		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+		apps.h
+		LINK_LIBS
 		-Wl,--start-group
 		${module_libraries}
 		df_driver_framework
@@ -20,10 +23,27 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE)
 		-Wl,--end-group
 		)
 else()
-	target_link_libraries(mainapp
-		${module_libraries}
-		pthread m
+	add_executable(mainapp
+		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+		apps.h
 		)
+	if (${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE)
+		target_link_libraries(mainapp
+			-Wl,--start-group
+			${module_libraries}
+			df_driver_framework
+			${df_driver_libraries}
+			pthread m rt
+			-Wl,--end-group
+			)
+	else()
+		target_link_libraries(mainapp
+			${module_libraries}
+			df_driver_framework
+			${df_driver_libraries}
+			pthread m
+			)
+	endif()
 endif()
 
 add_custom_target(run_config
diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt
index 34832a15e6..b6e8c5f8de 100644
--- a/src/firmware/qurt/CMakeLists.txt
+++ b/src/firmware/qurt/CMakeLists.txt
@@ -6,6 +6,8 @@ px4_qurt_generate_builtin_commands(
 	OUT ${CMAKE_BINARY_DIR}/apps.h
 	MODULE_LIST ${module_libraries})
 
+FASTRPC_STUB_GEN(px4muorb.idl)
+
 # Enable build without HexagonSDK to check link dependencies
 if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
 	add_definitions(-DQURT_EXE_BUILD)
@@ -13,12 +15,17 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
 		${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c
 		${CMAKE_BINARY_DIR}/apps.h)
 else()
-	QURT_BUNDLE(APP_NAME
-		mainapp
-		DSP_SOURCES
+
+	message("module_libraries = ${module_libraries}")
+	message("target_libraries = ${target_libraries}")
+	# Generate the DSP lib and stubs but not the apps side executable
+	# The Apps side executable is generated via the posix_eagle_xxxx target
+	QURT_LIB(APP_NAME mainapp
+		IDL_NAME px4muorb
+		SOURCES
 		${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c
 		${CMAKE_BINARY_DIR}/apps.h
-		DSP_LINK_LIBS
+		LINK_LIBS
 		${module_libraries}
 		${target_libraries}
 		df_driver_framework
diff --git a/src/firmware/qurt/mainapp.idl b/src/firmware/qurt/mainapp.idl
deleted file mode 100755
index c86adac37b..0000000000
--- a/src/firmware/qurt/mainapp.idl
+++ /dev/null
@@ -1,41 +0,0 @@
-/****************************************************************************
- *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name ATLFlight nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-#ifndef MAINAPP_IDL
-#define MAINAPP_IDL
-
-#include "AEEStdDef.idl"
-
-interface mainapp{
-    uint32 test();
-};
-
-#endif /*MAINAPP_IDL*/
diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
index f0141d8a17..28ee6457a0 100644
--- a/src/include/mavlink/mavlink_log.h
+++ b/src/include/mavlink/mavlink_log.h
@@ -110,7 +110,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
 		fprintf(stderr, "telem> "); \
 		fprintf(stderr, _text, ##__VA_ARGS__); \
 		fprintf(stderr, "\n"); } while(0);
-
+		
 /**
  * Send a mavlink critical message and print to console.
  *
@@ -121,7 +121,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
 		fprintf(stderr, "telem> "); \
 		fprintf(stderr, _text, ##__VA_ARGS__); \
 		fprintf(stderr, "\n");  } while(0);
-
+		
 /**
  * Send a mavlink emergency message and print to console.
  *
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index fb0dce7ca5..e35bb9bd89 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -400,6 +400,35 @@ void AttitudeEstimatorQ::task_main()
 
 			if (!_failsafe) {
 				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
+#ifdef __PX4_POSIX
+				perf_end(_perf_accel);
+				perf_end(_perf_mpu);
+				perf_end(_perf_mag);
+#endif
+
+				if (_voter_gyro.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_gyro.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
+									  _voter_gyro.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
+
+				if (_voter_accel.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_accel.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
+									  _voter_accel.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
 
 #ifdef __PX4_POSIX
 				perf_end(_perf_accel);
@@ -607,8 +636,11 @@ void AttitudeEstimatorQ::task_main()
 
 		/* attitude quaternions for control state */
 		ctrl_state.q[0] = _q(0);
+
 		ctrl_state.q[1] = _q(1);
+
 		ctrl_state.q[2] = _q(2);
+
 		ctrl_state.q[3] = _q(3);
 
 		/* attitude rates for control state */
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index 57f43959bd..dcc251f4a0 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -539,6 +539,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		}
 	}
 
+
+#ifdef __PX4_QURT
+	// WARNING: Preflight checks are important and should be added back when
+	// all the sensors are supported
+	PX4_WARN("WARNING: Preflight checks PASS always.");
+	failed = false;
+#endif
+
 	/* Report status */
 	return !failed;
 }
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 157762ec7c..a6dfd46456 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -687,7 +687,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				}
 			}
 
-			if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
+			if ((hil_ret != TRANSITION_DENIED) && (arming_ret != TRANSITION_DENIED) && (main_ret != TRANSITION_DENIED)) {
 				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
@@ -769,7 +769,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
+				mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
 						     (double)cmd->param1,
 						     (double)cmd->param2,
 						     (double)cmd->param3,
@@ -1431,12 +1431,15 @@ int commander_thread_main(int argc, char *argv[])
 	pthread_attr_init(&commander_low_prio_attr);
 	pthread_attr_setstacksize(&commander_low_prio_attr, 2880);
 
+#ifndef __PX4_QURT
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
 
 	/* low priority */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
 	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+#endif
+
 	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
 	pthread_attr_destroy(&commander_low_prio_attr);
 
@@ -1607,6 +1610,11 @@ int commander_thread_main(int argc, char *argv[])
 					_usb_telemetry_active = true;
 				}
 
+				/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
+				if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
+					_usb_telemetry_active = true;
+				}
+
 				if (telemetry.heartbeat_time > 0) {
 					telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
 				}
@@ -2188,8 +2196,13 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* RC input check */
+#ifndef __PX4_QURT
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
+#else
+		// HACK: remove old data check due to timestamp issue in QURT
+		if (!status.rc_input_blocked && sp_man.timestamp != 0) {
+#endif
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -2969,7 +2982,7 @@ set_control_mode()
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
 	control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
-	control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
+	//control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
 	switch (status.nav_state) {
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d6ca5d3c90..95e1149504 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -993,6 +993,10 @@ void MulticopterPositionControl::control_auto(float dt)
 	}
 
 	if (current_setpoint_valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
 		/* scaled space: 1 == position error resulting max allowed speed */
 		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
 
@@ -1187,6 +1191,13 @@ MulticopterPositionControl::task_main()
 
 		poll_subscriptions();
 
+		/* get current rotation matrix and euler angles from control state quaternions */
+		math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+		_R = q_att.to_dcm();
+		math::Vector<3> euler_angles;
+		euler_angles = _R.to_euler();
+		_yaw     = euler_angles(2);
+
 		parameters_update(false);
 
 		hrt_abstime t = hrt_absolute_time();
diff --git a/src/modules/muorb/adsp/module.mk b/src/modules/muorb/adsp/module.mk
index d31ca360db..61d50d65d2 100644
--- a/src/modules/muorb/adsp/module.mk
+++ b/src/modules/muorb/adsp/module.mk
@@ -41,6 +41,11 @@ ifeq ($(PX4_TARGET_OS),qurt)
 SRCS		= \
 		  px4muorb.cpp \
 		  uORBFastRpcChannel.cpp
+SRCS		=  param/param.c 
+endif
+endif
+
+SRCS		+= perf_counter.c \
 
 INCLUDE_DIRS	+= \
 		  ${PX4_BASE}/src/modules/uORB
diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt
index 1c1d6605ac..2b1f8e299a 100644
--- a/src/modules/muorb/krait/CMakeLists.txt
+++ b/src/modules/muorb/krait/CMakeLists.txt
@@ -30,9 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-if("${DSPAL_STUBS_ENABLE}" STREQUAL "1")
-	include_directories(../krait-stubs)
-endif()
+#if("${DSPAL_STUBS_ENABLE}" STREQUAL "1")
+#	include_directories(../krait-stubs)
+#endif()
 
 px4_add_module(
 	MODULE modules__muorb__krait
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 4eda2f7722..01ec7de7e2 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -417,6 +417,32 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 	}
 }
 
+void
+MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
+{
+	item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+
+	/* use current position and use return altitude as clearance */
+	item->lat = _navigator->get_global_position()->lat;
+	item->lon = _navigator->get_global_position()->lon;
+	item->altitude = _navigator->get_global_position()->alt;
+
+	if (min_clearance > 0.0f) {
+		item->altitude += min_clearance;
+	}
+
+	item->altitude_is_relative = false;
+	item->yaw = NAN;
+	item->loiter_radius = _navigator->get_loiter_radius();
+	item->loiter_direction = 1;
+	item->acceptance_radius = (_navigator->get_acceptance_radius() > min_clearance / 2.0f) ?
+					(min_clearance / 2) : _navigator->get_acceptance_radius();
+	item->time_inside = 0.0f;
+	item->pitch_min = min_pitch;
+	item->autocontinue = false;
+	item->origin = ORIGIN_ONBOARD;
+}
+
 void
 MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
 {
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 aaf57b42a3..5de0a389dd 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -81,6 +81,7 @@
 #define MIN_VALID_W 0.00001f
 #define PUB_INTERVAL 10000	// limit publish rate to 100 Hz
 #define EST_BUF_SIZE 250000 / PUB_INTERVAL		// buffer size is 0.5s
+#define MAX_WAIT_FOR_BARO_SAMPLE 3000000 // wait 3 secs for the baro to respond
 
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
@@ -410,10 +411,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	/* wait for initial baro value */
 	bool wait_baro = true;
-
 	TerrainEstimator *terrain_estimator = new TerrainEstimator();
 
 	thread_running = true;
+	hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();
 
 	while (wait_baro && !thread_should_exit) {
 		int ret = px4_poll(&fds_init[0], 1, 1000);
@@ -421,14 +422,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (ret < 0) {
 			/* poll error */
 			mavlink_log_info(mavlink_fd, "[inav] poll error on init");
-			PX4_WARN("INAV poll error");
-
-		} else if (ret > 0) {
+		} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
+			wait_baro = false;
+			mavlink_log_info(mavlink_fd, "[inav] timed out waiting for a baro sample");
+		}
+		else if (ret > 0) {
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
 				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
 					baro_timestamp = sensor.baro_timestamp[0];
+					baro_wait_for_sample_time = hrt_absolute_time();
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 560e7d4a0b..b88aa323a3 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -530,8 +530,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
 	pthread_attr_destroy(&sender_thread_attr);
 
-	mavlink_status_t udp_status = {};
-	mavlink_status_t serial_status = {};
+
+	// set the threads name
+#ifdef __PX4_DARWIN
+	pthread_setname_np("sim_rcv");
+#else
+	pthread_setname_np(pthread_self(), "sim_rcv");
+#endif
 
 	bool sim_delay = false;
 
@@ -575,9 +580,10 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 			if (len > 0) {
 				mavlink_message_t msg;
+				mavlink_status_t status;
 
 				for (int i = 0; i < len; i++) {
-					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) {
 						// have a message, handle it
 						handle_message(&msg, publish);
 					}
@@ -591,9 +597,10 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 			if (len > 0) {
 				mavlink_message_t msg;
+				mavlink_status_t status;
 
 				for (int i = 0; i < len; ++i) {
-					if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) {
 						// have a message, handle it
 						handle_message(&msg, true);
 					}
diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt
index 59485fe494..1f99d617fd 100644
--- a/src/modules/systemlib/CMakeLists.txt
+++ b/src/modules/systemlib/CMakeLists.txt
@@ -36,7 +36,6 @@ include_directories(${CMAKE_BINARY_DIR}/src/modules/param)
 
 set(SRCS
 	perf_counter.c
-	param/param.c
 	conversions.c
 	cpuload.c
 	pid/pid.c
@@ -57,8 +56,14 @@ if(${OS} STREQUAL "nuttx")
 		printload.c
 		up_cxxinitialize.c 
 		)
+elseif ("${CONFIG_SHMEM}" STREQUAL "1")
+	list(APPEND SRCS
+		param/param_shmem.c
+		print_load_posix.c
+		)
 else()
 	list(APPEND SRCS
+		param/param.c
 		print_load_posix.c
 		)	
 endif()
diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c
index e83fd67999..41ffbcf822 100644
--- a/src/modules/systemlib/bson/tinybson.c
+++ b/src/modules/systemlib/bson/tinybson.c
@@ -54,15 +54,9 @@
 #define CODER_CHECK(_c)		do { if (_c->dead) { debug("coder dead"); return -1; }} while(0)
 #define CODER_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0)
 
-#ifdef __PX4_QURT
-#define BSON_READ  px4_read
-#define BSON_WRITE px4_write
-#define BSON_FSYNC px4_fsync
-#else
 #define BSON_READ  read
 #define BSON_WRITE write
-#define BSON_FSYNC fsync
-#endif
+#define BSON_FSYNC px4_fsync
 
 static int
 read_x(bson_decoder_t decoder, void *p, size_t s)
diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c
index a3eaebbb33..e078bb4820 100644
--- a/src/modules/systemlib/circuit_breaker_params.c
+++ b/src/modules/systemlib/circuit_breaker_params.c
@@ -42,6 +42,8 @@
  * parameter needs to set to the key (magic).
  */
 
+#ifdef __PX4_QURT
+
 /**
  * Circuit breaker for power supply check
  *
diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c
new file mode 100644
index 0000000000..2fd59f5f3d
--- /dev/null
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -0,0 +1,1199 @@
+
+
+/****************************************************************************
+ *
+ * Copyright (c) 2015 Vijay Venkatraman. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file param.c
+ *
+ * Global parameter store.
+ *
+ * Note that it might make sense to convert this into a driver.  That would
+ * offer some interesting options regarding state for e.g. ORB advertisements
+ * and background parameter saving.
+ */
+
+//#include <debug.h>
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <errno.h>
+#include <semaphore.h>
+
+#include <sys/stat.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "systemlib/param/param.h"
+#include "systemlib/uthash/utarray.h"
+#include "systemlib/bson/tinybson.h"
+
+#include "uORB/uORB.h"
+#include "uORB/topics/parameter_update.h"
+#include "px4_parameters.h"
+
+#include <crc32.h>
+
+#include "shmem.h"
+
+#if 0
+# define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
+#else
+# define debug(fmt, args...)		do { } while(0)
+#endif
+
+#define PARAM_OPEN	open
+#define PARAM_CLOSE	close
+
+/**
+ * Array of static parameter info.
+ */
+#ifdef _UNIT_TEST
+extern struct param_info_s	param_array[];
+extern struct param_info_s	*param_info_base;
+extern struct param_info_s	*param_info_limit;
+#else
+// FIXME - start and end are reversed
+static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters;
+#endif
+
+#define	param_info_count		px4_parameters.param_count
+
+/**
+ * Storage for modified parameters.
+ */
+struct param_wbuf_s {
+	param_t			param;
+	union param_value_u	val;
+	bool			unsaved;
+};
+
+
+uint8_t  *param_changed_storage = 0;
+int size_param_changed_storage_bytes = 0;
+const int bits_per_allocation_unit  = (sizeof(*param_changed_storage) * 8);
+
+#define ENABLE_SHMEM_DEBUG
+
+extern int get_shmem_lock(void);
+extern void release_shmem_lock(void);
+
+struct param_wbuf_s * param_find_changed(param_t param);
+
+void init_params(void);
+extern void init_shared_memory(void);
+
+extern void copy_params_to_shmem(struct param_info_s *param_info_base);
+
+extern struct shmem_info* shmem_info_p;
+uint64_t sync_other_prev_time=0, sync_other_current_time=0;
+
+extern void update_to_shmem(param_t param, union param_value_u value);
+extern int update_from_shmem(param_t param, union param_value_u *value);
+static int param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes);
+unsigned char set_called_from_get=0;
+
+static int param_import_done=0; /*at startup, params are loaded from file, if present. we dont want to send notifications that time since muorb is not ready*/
+
+static int param_load_default_no_notify(void);
+
+static unsigned
+get_param_info_count(void)
+{
+	/* Singleton creation of and array of bits to track changed values */
+	if (!param_changed_storage) {
+		size_param_changed_storage_bytes  = (param_info_count / bits_per_allocation_unit) + 1;
+		param_changed_storage = calloc(size_param_changed_storage_bytes, 1);
+
+		/* If the allocation fails we need to indicate failure in the
+		 * API by returning PARAM_INVALID
+		 */
+		if (param_changed_storage == NULL) {
+			return 0;
+		}
+	}
+
+	return param_info_count;
+}
+
+/** flexible array holding modified parameter values */
+UT_array	*param_values;
+
+/** array info for the modified parameters array */
+const UT_icd	param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
+
+/** parameter update topic */
+ORB_DEFINE(parameter_update, struct parameter_update_s);
+
+/** parameter update topic handle */
+static orb_advert_t param_topic = NULL;
+
+static void param_set_used_internal(param_t param);
+
+static param_t param_find_internal(const char *name, bool notification);
+
+/** lock the parameter store */
+static void
+param_lock(void)
+{
+	//do {} while (px4_sem_wait(&param_sem) != 0);
+}
+
+/** unlock the parameter store */
+static void
+param_unlock(void)
+{
+	//px4_sem_post(&param_sem);
+}
+
+/** assert that the parameter store is locked */
+static void
+param_assert_locked(void)
+{
+	/* XXX */
+}
+
+/**
+ * Test whether a param_t is value.
+ *
+ * @param param			The parameter handle to test.
+ * @return			True if the handle is valid.
+ */
+//static bool
+bool
+handle_in_range(param_t param)
+{
+	int count = get_param_info_count();
+	return (count && param < count);
+}
+
+/**
+ * Compare two modifid parameter structures to determine ordering.
+ *
+ * This function is suitable for passing to qsort or bsearch.
+ */
+static int
+param_compare_values(const void *a, const void *b)
+{
+	struct param_wbuf_s *pa = (struct param_wbuf_s *)a;
+	struct param_wbuf_s *pb = (struct param_wbuf_s *)b;
+
+	if (pa->param < pb->param) {
+		return -1;
+	}
+
+	if (pa->param > pb->param) {
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * Locate the modified parameter structure for a parameter, if it exists.
+ *
+ * @param param			The parameter being searched.
+ * @return			The structure holding the modified value, or
+ *				NULL if the parameter has not been modified.
+ */
+struct param_wbuf_s *
+param_find_changed(param_t param)
+{
+	struct param_wbuf_s	*s = NULL;
+
+	param_assert_locked();
+
+	if (param_values != NULL) {
+#if 0	/* utarray_find requires bsearch, not available */
+		struct param_wbuf_s key;
+		key.param = param;
+		s = utarray_find(param_values, &key, param_compare_values);
+#else
+
+		while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+			if (s->param == param) {
+				break;
+			}
+		}
+
+#endif
+	}
+
+	return s;
+}
+
+static void
+param_notify_changes(void)
+{
+	struct parameter_update_s pup = { .timestamp = hrt_absolute_time() };
+
+	/*
+	 * If we don't have a handle to our topic, create one now; otherwise
+	 * just publish.
+	 */
+	if (param_topic == NULL) {
+		param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
+
+	} else {
+		orb_publish(ORB_ID(parameter_update), param_topic, &pup);
+	}
+}
+
+param_t
+param_find_internal(const char *name, bool notification)
+{
+	param_t param;
+
+	/* perform a linear search of the known parameters */
+
+	for (param = 0; handle_in_range(param); param++) {
+		if (!strcmp(param_info_base[param].name, name)) {
+			if (notification) {
+				param_set_used_internal(param);
+			}
+
+			return param;
+		}
+	}
+
+	/* not found */
+	return PARAM_INVALID;
+}
+
+param_t
+param_find(const char *name)
+{
+	return param_find_internal(name, true);
+}
+
+param_t
+param_find_no_notification(const char *name)
+{
+	return param_find_internal(name, false);
+}
+
+unsigned
+param_count(void)
+{
+	return get_param_info_count();
+}
+
+unsigned
+param_count_used(void)
+{
+	unsigned count = 0;
+
+	// ensure the allocation has been done
+	if (get_param_info_count()) {
+
+		for (unsigned i = 0; i < size_param_changed_storage_bytes; i++) {
+			for (unsigned j = 0; j < bits_per_allocation_unit; j++) {
+				if (param_changed_storage[i] & (1 << j)) {
+					count++;
+				}
+			}
+		}
+	}
+
+	return count;
+}
+
+param_t
+param_for_index(unsigned index)
+{
+	unsigned count = get_param_info_count();
+
+	if (count && index < count) {
+		return (param_t)index;
+	}
+
+	return PARAM_INVALID;
+}
+
+param_t
+param_for_used_index(unsigned index)
+{
+	int count = get_param_info_count();
+
+	if (count && index < count) {
+		/* walk all params and count used params */
+		unsigned used_count = 0;
+
+		for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) {
+			for (unsigned j = 0; j < bits_per_allocation_unit; j++) {
+				if (param_changed_storage[i] & (1 << j)) {
+
+					/* we found the right used count,
+					 * return the param value
+					 */
+					if (index == used_count) {
+						return (param_t)(i * bits_per_allocation_unit + j);
+					}
+
+					used_count++;
+				}
+			}
+		}
+	}
+
+	return PARAM_INVALID;
+}
+
+int
+param_get_index(param_t param)
+{
+	if (handle_in_range(param)) {
+		return (unsigned)param;
+	}
+
+	return -1;
+}
+
+int
+param_get_used_index(param_t param)
+{
+	/* this tests for out of bounds and does a constant time lookup */
+	if (!param_used(param)) {
+		return -1;
+	}
+
+	/* walk all params and count, now knowing that it has a valid index */
+	int used_count = 0;
+
+	for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) {
+		for (unsigned j = 0; j < bits_per_allocation_unit; j++) {
+			if (param_changed_storage[i] & (1 << j)) {
+
+				if ((unsigned)param == i * bits_per_allocation_unit + j) {
+					return used_count;
+				}
+
+				used_count++;
+			}
+		}
+	}
+
+	return -1;
+}
+
+const char *
+param_name(param_t param)
+{
+	return handle_in_range(param) ? param_info_base[param].name : NULL;
+}
+
+bool
+param_value_is_default(param_t param)
+{
+	return param_find_changed(param) ? false : true;
+}
+
+bool
+param_value_unsaved(param_t param)
+{
+	static struct param_wbuf_s *s;
+	s = param_find_changed(param);
+	return (s && s->unsaved) ? true : false;
+}
+
+enum param_type_e
+param_type(param_t param) {
+	return handle_in_range(param) ? param_info_base[param].type : PARAM_TYPE_UNKNOWN;
+}
+
+size_t
+param_size(param_t param)
+{
+	if (handle_in_range(param)) {
+
+		switch (param_type(param)) {
+
+		case PARAM_TYPE_INT32:
+		case PARAM_TYPE_FLOAT:
+			return 4;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			/* decode structure size from type value */
+			return param_type(param) - PARAM_TYPE_STRUCT;
+
+		default:
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Obtain a pointer to the storage allocated for a parameter.
+ *
+ * @param param			The parameter whose storage is sought.
+ * @return			A pointer to the parameter value, or NULL
+ *				if the parameter does not exist.
+ */
+static const void *
+param_get_value_ptr(param_t param)
+{
+	const void *result = NULL;
+
+	param_assert_locked();
+
+	if (handle_in_range(param)) {
+
+		const union param_value_u *v;
+
+		/* work out whether we're fetching the default or a written value */
+		struct param_wbuf_s *s = param_find_changed(param);
+
+		if (s != NULL) {
+			v = &s->val;
+
+		} else {
+			v = &param_info_base[param].val;
+		}
+
+		if (param_type(param) >= PARAM_TYPE_STRUCT &&
+		    param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
+
+			result = v->p;
+
+		} else {
+			result = v;
+		}
+	}
+
+	return result;
+}
+
+int
+param_get(param_t param, void *val)
+{
+	int result = -1;
+
+	param_lock();
+
+	if(!handle_in_range(param))
+		return result;
+
+	union param_value_u value;
+
+	if(update_from_shmem(param, &value))
+	{
+		set_called_from_get=1;
+		param_set_internal(param, &value, true, false);
+		set_called_from_get=0;
+	}
+
+
+	const void *v = param_get_value_ptr(param);
+
+	if (val != NULL) {
+		memcpy(val, v, param_size(param));
+		result = 0;
+	}
+
+#ifdef ENABLE_SHMEM_DEBUG
+	if(param_type(param)==PARAM_TYPE_INT32)	{ PX4_INFO("param_get for %s : %d\n", param_name(param), *(int*)val); }
+	else if(param_type(param)==PARAM_TYPE_FLOAT) { PX4_INFO("param_get for %s : %f\n", param_name(param), *(double*)val); }
+	else { PX4_INFO("Unknown param type for %s\n", param_name(param)); }
+#endif
+
+	param_unlock();
+
+	return result;
+}
+
+static int
+param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes)
+{
+	int result = -1;
+	bool params_changed = false;
+
+	param_lock();
+
+	if(!handle_in_range(param))
+		return result;
+
+	mark_saved = true; //mark all params as saved
+
+	if (param_values == NULL) {
+		utarray_new(param_values, &param_icd);
+	}
+
+	if (param_values == NULL) {
+		debug("failed to allocate modified values array");
+		goto out;
+	}
+
+	if (handle_in_range(param)) {
+
+		struct param_wbuf_s *s = param_find_changed(param);
+
+		if (s == NULL) {
+
+			/* construct a new parameter */
+			struct param_wbuf_s buf = {
+				.param = param,
+				.val.p = NULL,
+				.unsaved = false
+			};
+
+			/* add it to the array and sort */
+			utarray_push_back(param_values, &buf);
+			utarray_sort(param_values, param_compare_values);
+
+			/* find it after sorting */
+			s = param_find_changed(param);
+		}
+
+		/* update the changed value */
+		switch (param_type(param)) {
+
+		case PARAM_TYPE_INT32:
+			s->val.i = *(int32_t *)val;
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			s->val.f = *(float *)val;
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			if (s->val.p == NULL) {
+				s->val.p = malloc(param_size(param));
+
+				if (s->val.p == NULL) {
+					debug("failed to allocate parameter storage");
+					goto out;
+				}
+			}
+
+			memcpy(s->val.p, val, param_size(param));
+			break;
+
+		default:
+			goto out;
+		}
+
+		s->unsaved = !mark_saved;
+		params_changed = true;
+		result = 0;
+	}
+
+out:
+	param_unlock();
+
+	/*
+	 * If we set something, now that we have unlocked, go ahead and advertise that
+	 * a thing has been set.
+	 */
+
+	if(!param_import_done) notify_changes=0;
+
+	if (params_changed && notify_changes) {
+		param_notify_changes();
+	}
+
+	if(result==0 && !set_called_from_get)
+		update_to_shmem(param, *(union param_value_u*)val);
+
+#ifdef ENABLE_SHMEM_DEBUG
+	if(param_type(param)==PARAM_TYPE_INT32) {PX4_INFO("param_set for %s : %d\n", param_name(param), *(int*)val);}
+	else if(param_type(param)==PARAM_TYPE_FLOAT) {PX4_INFO("param_set for %s : %f\n", param_name(param), *(double*)val);}
+	else {PX4_INFO("Unknown param type for %s\n", param_name(param));}
+#endif
+
+	return result;
+}
+
+int
+param_set(param_t param, const void *val)
+{
+	return param_set_internal(param, val, false, true);
+}
+
+int
+param_set_no_notification(param_t param, const void *val)
+{
+	return param_set_internal(param, val, false, false);
+}
+
+bool
+param_used(param_t param)
+{
+	int param_index = param_get_index(param);
+
+	if (param_index < 0) {
+		return false;
+	}
+
+	return param_changed_storage[param_index / bits_per_allocation_unit] &
+	       (1 << param_index % bits_per_allocation_unit);
+}
+
+void param_set_used_internal(param_t param)
+{
+	int param_index = param_get_index(param);
+
+	if (param_index < 0) {
+		return;
+	}
+
+	param_changed_storage[param_index / bits_per_allocation_unit] |=
+		(1 << param_index % bits_per_allocation_unit);
+}
+
+int
+param_reset(param_t param)
+{
+	struct param_wbuf_s *s = NULL;
+	bool param_found = false;
+
+	param_lock();
+
+	if (handle_in_range(param)) {
+
+		/* look for a saved value */
+		s = param_find_changed(param);
+
+		/* if we found one, erase it */
+		if (s != NULL) {
+			int pos = utarray_eltidx(param_values, s);
+			utarray_erase(param_values, pos, 1);
+		}
+
+		param_found = true;
+	}
+
+	param_unlock();
+
+	if (s != NULL) {
+		param_notify_changes();
+	}
+
+	return (!param_found);
+}
+
+void
+param_reset_all(void)
+{
+	param_lock();
+
+	if (param_values != NULL) {
+		utarray_free(param_values);
+	}
+
+	/* mark as reset / deleted */
+	param_values = NULL;
+
+	param_unlock();
+
+	param_notify_changes();
+}
+
+void
+param_reset_excludes(const char *excludes[], int num_excludes)
+{
+	param_lock();
+
+	param_t	param;
+
+	for (param = 0; handle_in_range(param); param++) {
+		const char *name = param_name(param);
+		bool exclude = false;
+
+		for (int index = 0; index < num_excludes; index ++) {
+			int len = strlen(excludes[index]);
+
+			if ((excludes[index][len - 1] == '*'
+			     && strncmp(name, excludes[index], len - 1) == 0)
+			    || strcmp(name, excludes[index]) == 0) {
+				exclude = true;
+				break;
+			}
+		}
+
+		if (!exclude) {
+			param_reset(param);
+		}
+	}
+
+	param_unlock();
+
+	param_notify_changes();
+}
+
+#ifdef __PX4_QURT
+static const char *param_default_file = "/dev/fs/params";
+#else
+static const char *param_default_file = "/usr/share/data/adsp/params";
+#endif
+static char *param_user_file = NULL;
+
+int
+param_set_default_file(const char *filename)
+{
+	if (param_user_file != NULL) {
+		free(param_user_file);
+		param_user_file = NULL;
+	}
+
+	if (filename) {
+		param_user_file = strdup(filename);
+	}
+
+	return 0;
+}
+
+const char *
+param_get_default_file(void)
+{
+	return (param_user_file != NULL) ? param_user_file : param_default_file;
+}
+
+int
+param_save_default(void)
+{
+	int res;
+	int fd;
+
+	const char *filename = param_get_default_file();
+
+	if(get_shmem_lock()!=0) {
+		PX4_ERR("Could not get shmem lock\n");
+		return 0;
+	}			
+
+	fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
+
+	if (fd < 0) {
+		warn("failed to open param file: %s", filename);
+		return ERROR;
+	}
+
+	res = param_export(fd, false);
+
+	if (res != OK) {
+		warnx("failed to write parameters to file: %s", filename);
+	}
+
+	PARAM_CLOSE(fd);
+
+	release_shmem_lock();
+
+	PX4_INFO("saving params done\n");
+
+	return res;
+}
+
+/**
+ * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
+ */
+int
+param_load_default(void)
+{
+	warnx("param_load_default\n");
+	int fd_load = PARAM_OPEN(param_get_default_file(), O_RDONLY);
+
+	if (fd_load < 0) {
+		/* no parameter file is OK, otherwise this is an error */
+		if (errno != ENOENT) {
+			warn("open '%s' for reading failed", param_get_default_file());
+			return -1;
+		}
+
+		return 1;
+	}
+
+	int result = param_load(fd_load);
+
+	PARAM_CLOSE(fd_load);
+
+	if (result != 0) {
+		warn("error reading parameters from '%s'", param_get_default_file());
+		return -2;
+	}
+
+	return 0;
+}
+
+/**
+ * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
+ */
+static int
+param_load_default_no_notify(void)
+{
+	if(get_shmem_lock()!=0) {
+		PX4_ERR("Could not get shmem lock\n");
+		return 0;
+	}			
+
+	int fd_load = open(param_get_default_file(), O_RDONLY);
+
+	if (fd_load < 0) {
+		release_shmem_lock();
+		/* no parameter file is OK, otherwise this is an error */
+		if (errno != ENOENT) {
+			debug("open '%s' for reading failed", param_get_default_file());
+			return -1;
+		}
+
+		return 1;
+	}
+
+	int result = param_import(fd_load);
+
+	close(fd_load);
+
+	PX4_INFO("param loading done\n");
+
+	release_shmem_lock();
+
+	if (result != 0) {
+		warn("error reading parameters from '%s'", param_get_default_file());
+		return -2;
+	}
+
+	return 0;
+}
+
+int
+param_export(int fd, bool only_unsaved)
+{
+	struct param_wbuf_s *s = NULL;
+	struct bson_encoder_s encoder;
+	int	result = -1;
+
+	param_lock();
+
+	bson_encoder_init_file(&encoder, fd);
+
+	/* no modified parameters -> we are done */
+	if (param_values == NULL) {
+		result = 0;
+		goto out;
+	}
+
+	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+
+		int32_t	i;
+		float	f;
+
+		/*
+		 * If we are only saving values changed since last save, and this
+		 * one hasn't, then skip it
+		 */
+		if (only_unsaved && !s->unsaved) {
+			continue;
+		}
+
+		s->unsaved = false;
+
+		/* append the appropriate BSON type object */
+
+		switch (param_type(s->param)) {
+
+		case PARAM_TYPE_INT32:
+			i = s->val.i;
+
+			if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			f = s->val.f;
+
+			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			if (bson_encoder_append_binary(&encoder,
+						       param_name(s->param),
+						       BSON_BIN_BINARY,
+						       param_size(s->param),
+						       param_get_value_ptr(s->param))) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		default:
+			debug("unrecognized parameter type");
+			goto out;
+		}
+	}
+
+	result = 0;
+
+out:
+	param_unlock();
+
+	if (result == 0) {
+		result = bson_encoder_fini(&encoder);
+	}
+
+	return result;
+}
+
+struct param_import_state {
+	bool mark_saved;
+};
+
+static int
+param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	float f;
+	int32_t i;
+	void *v, *tmp = NULL;
+	int result = -1;
+	struct param_import_state *state = (struct param_import_state *)private;
+
+	/*
+	 * EOO means the end of the parameter object. (Currently not supporting
+	 * nested BSON objects).
+	 */
+	if (node->type == BSON_EOO) {
+		debug("end of parameters");
+		return 0;
+	}
+
+	/*
+	 * Find the parameter this node represents.  If we don't know it,
+	 * ignore the node.
+	 */
+	param_t param = param_find_no_notification(node->name);
+
+	if (param == PARAM_INVALID) {
+		debug("ignoring unrecognised parameter '%s'", node->name);
+		return 1;
+	}
+
+	/*
+	 * Handle setting the parameter from the node
+	 */
+
+	switch (node->type) {
+	case BSON_INT32:
+		if (param_type(param) != PARAM_TYPE_INT32) {
+			debug("unexpected type for '%s", node->name);
+			goto out;
+		}
+
+		i = node->i;
+		v = &i;
+		PX4_INFO("Imported %s with value %d\n", param_name(param), i);
+		break;
+
+	case BSON_DOUBLE:
+		if (param_type(param) != PARAM_TYPE_FLOAT) {
+			debug("unexpected type for '%s", node->name);
+			goto out;
+		}
+
+		f = node->d;
+		v = &f;
+		PX4_INFO("Imported %s with value %f\n", param_name(param), (double)f);
+		break;
+
+	case BSON_BINDATA:
+		if (node->subtype != BSON_BIN_BINARY) {
+			debug("unexpected subtype for '%s", node->name);
+			goto out;
+		}
+
+		if (bson_decoder_data_pending(decoder) != param_size(param)) {
+			debug("bad size for '%s'", node->name);
+			goto out;
+		}
+
+		/* XXX check actual file data size? */
+		tmp = malloc(param_size(param));
+
+		if (tmp == NULL) {
+			debug("failed allocating for '%s'", node->name);
+			goto out;
+		}
+
+		if (bson_decoder_copy_data(decoder, tmp)) {
+			debug("failed copying data for '%s'", node->name);
+			goto out;
+		}
+
+		v = tmp;
+		break;
+
+	default:
+		debug("unrecognised node type");
+		goto out;
+	}
+
+	if (param_set_internal(param, v, state->mark_saved, true)) {
+		debug("error setting value for '%s'", node->name);
+		goto out;
+	}
+
+	if (tmp != NULL) {
+		free(tmp);
+		tmp = NULL;
+	}
+
+	/* don't return zero, that means EOF */
+	result = 1;
+
+out:
+
+	if (tmp != NULL) {
+		free(tmp);
+	}
+
+	return result;
+}
+
+static int
+param_import_internal(int fd, bool mark_saved)
+{
+	struct bson_decoder_s decoder;
+	int result = -1;
+	struct param_import_state state;
+
+	if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) {
+		debug("decoder init failed");
+		goto out;
+	}
+
+	state.mark_saved = mark_saved;
+
+	do {
+		result = bson_decoder_next(&decoder);
+
+	} while (result > 0);
+
+out:
+
+	if (result < 0) {
+		debug("BSON error decoding parameters");
+	}
+
+	return result;
+}
+
+int
+param_import(int fd)
+{
+	return param_import_internal(fd, false);
+}
+
+int
+param_load(int fd)
+{
+	param_reset_all();
+	return param_import_internal(fd, true);
+}
+
+void
+param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used)
+{
+	param_t	param;
+
+	for (param = 0; handle_in_range(param); param++) {
+
+		/* if requested, skip unchanged values */
+		if (only_changed && (param_find_changed(param) == NULL)) {
+			continue;
+		}
+
+		if (only_used && !param_used(param)) {
+			continue;
+		}
+
+		func(arg, param);
+	}
+}
+
+uint32_t param_hash_check(void)
+{
+	uint32_t param_hash = 0;
+
+	param_lock();
+
+	/* compute the CRC32 over all string param names and 4 byte values */
+	for (param_t param = 0; handle_in_range(param); param++) {
+		if (!param_used(param)) {
+			continue;
+		}
+
+		const char *name = param_name(param);
+		const void *val = param_get_value_ptr(param);
+		param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash);
+		param_hash = crc32part(val, sizeof(union param_value_u), param_hash);
+	}
+
+	param_unlock();
+
+	return param_hash;
+}
+
+void init_params(void)
+{
+	//copy params to shared memory
+	init_shared_memory();
+
+	/*load params automatically*/
+#ifdef __PX4_POSIX
+	param_load_default_no_notify();
+#endif
+	param_import_done=1;
+
+	copy_params_to_shmem(param_info_base);
+
+
+#ifdef ENABLE_SHMEM_DEBUG
+	PX4_INFO("Offsets: \n");
+	PX4_INFO("params_val %lu, krait_changed %lu, adsp_changed %lu\n", (unsigned char*)shmem_info_p->params_val - (unsigned char*)shmem_info_p, (unsigned char*)&shmem_info_p->krait_changed_index - (unsigned char*)shmem_info_p, (unsigned char*)&shmem_info_p->adsp_changed_index - (unsigned char*)shmem_info_p);
+#endif
+
+}
+
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 692b36e1a7..bf084da4b9 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -37,6 +37,8 @@
  * System wide parameters
  */
 
+#ifdef __PX4_QURT
+
 /**
  * Auto-start script index.
  *
@@ -111,3 +113,5 @@ PARAM_DEFINE_INT32(SYS_COMPANION, 157600);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);
+
+#endif
diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp
index 748e31df4e..151b6287e8 100644
--- a/src/modules/uORB/uORBDevices_posix.cpp
+++ b/src/modules/uORB/uORBDevices_posix.cpp
@@ -287,6 +287,7 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	case ORBIOCSETINTERVAL:
 		sd->update_interval = arg;
+		sd->last_update = hrt_absolute_time();
 		return PX4_OK;
 
 	case ORBIOCGADVERTISER:
@@ -405,7 +406,6 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 	 * don't match then we might have a visible update.
 	 */
 	while (sd->generation != _generation) {
-
 		/*
 		 * Handle non-rate-limited subscribers.
 		 */
@@ -426,31 +426,17 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 			break;
 		}
 
-		/*
-		 * If the interval timer is still running, the topic should not
-		 * appear updated, even though at this point we know that it has.
-		 * We have previously been through here, so the subscriber
-		 * must have collected the update we reported, otherwise
-		 * update_reported would still be true.
-		 */
-		if (!hrt_called(&sd->update_call)) {
+		// If we have not yet reached the deadline, then assume that we can ignore any
+		// newly received data.
+		if (sd->last_update + sd->update_interval > hrt_absolute_time()) {
 			break;
 		}
 
-		/*
-		 * Make sure that we don't consider the topic to be updated again
-		 * until the interval has passed once more by restarting the interval
-		 * timer and thereby re-scheduling a poll notification at that time.
-		 */
-		hrt_call_after(&sd->update_call,
-			       sd->update_interval,
-			       &uORB::DeviceNode::update_deferred_trampoline,
-			       (void *)this);
-
 		/*
 		 * Remember that we have told the subscriber that there is data.
 		 */
 		sd->update_reported = true;
+		sd->last_update = hrt_absolute_time();
 		ret = true;
 
 		break;
diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp
index 4d45ea8431..ac2f36fcb4 100644
--- a/src/modules/uORB/uORBDevices_posix.hpp
+++ b/src/modules/uORB/uORBDevices_posix.hpp
@@ -111,6 +111,7 @@ private:
 	struct SubscriberData {
 		unsigned  generation; /**< last generation the subscriber has seen */
 		unsigned  update_interval; /**< if nonzero minimum interval between updates */
+		uint64_t last_update; /**< time at which the last update was provided, used when update_interval is nonzero */
 		struct hrt_call update_call;  /**< deferred wakeup call if update_period is nonzero */
 		void    *poll_priv; /**< saved copy of fds->f_priv while poll is active */
 		bool    update_reported; /**< true if we have reported the update via poll/check */
diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp
index 2485189487..75880b3163 100644
--- a/src/platforms/posix/drivers/barosim/baro.cpp
+++ b/src/platforms/posix/drivers/barosim/baro.cpp
@@ -36,6 +36,8 @@
  * Driver for the simulated barometric pressure sensor
  */
 
+#define __STDC_FORMAT_MACROS
+#include <inttypes.h>
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_time.h>
@@ -389,7 +391,7 @@ BAROSIM::devRead(void *buffer, size_t buflen)
 int
 BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
 {
-	PX4_WARN("baro IOCTL %llu", hrt_absolute_time());
+	PX4_WARN("baro IOCTL %" PRIu64 , hrt_absolute_time());
 
 	switch (cmd) {
 
diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt
index 402e66e097..a4f533d7bd 100644
--- a/src/platforms/posix/px4_layer/CMakeLists.txt
+++ b/src/platforms/posix/px4_layer/CMakeLists.txt
@@ -30,6 +30,13 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
+if("${CONFIG_SHMEM}" STREQUAL "1")
+	list(APPEND SHMEM_SRCS
+		shmem_posix.c
+	)
+endif()
+
 px4_add_module(
 	MODULE platforms__posix__px4_layer
 	COMPILE_FLAGS
@@ -41,6 +48,7 @@ px4_add_module(
 		lib_crc32.c
 		drv_hrt.c
 		px4_log.c
+		${SHMEM_SRCS}
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk
index 846ce9469c..efa942c626 100644
--- a/src/platforms/posix/px4_layer/module.mk
+++ b/src/platforms/posix/px4_layer/module.mk
@@ -41,5 +41,8 @@ SRCS		 = 	\
 			lib_crc32.c \
 			drv_hrt.c \
 			px4_log.c
+ifeq ($(CONFIG_SHMEM), 1)
+SRCS		+=	shmem_posix.c
+endif
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
index c77fe760b8..514cf260c0 100644
--- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
@@ -58,6 +58,16 @@ __BEGIN_DECLS
 
 long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
 
+#ifdef ENABLE_SHMEM
+ extern void init_params(void);
+#endif
+ 
+#ifdef ENABLE_SHMEM
+extern void init_own_params(void);
+extern unsigned int init_other_params(void);
+extern unsigned int param_sync_done;
+#endif
+
 __END_DECLS
 
 namespace px4
@@ -72,6 +82,12 @@ void init_once(void)
 	work_queues_init();
 	hrt_work_queue_init();
 	hrt_init();
+
+#ifdef ENABLE_SHMEM
+	PX4_INFO("Starting shared memory param sync\n");
+	init_own_params();
+	param_sync_done=init_other_params();
+#endif
 }
 
 void init(int argc, char *argv[], const char *app_name)
diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c
new file mode 100644
index 0000000000..cd8c460548
--- /dev/null
+++ b/src/platforms/posix/px4_layer/shmem_posix.c
@@ -0,0 +1,270 @@
+
+/****************************************************************************
+ *
+ * Copyright (c) 2015 Ramakrishna Kintada. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <errno.h>
+#include <semaphore.h>
+
+#include <sys/stat.h>
+
+#include <drivers/drv_hrt.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+#include "systemlib/param/param.h"
+
+#include <shmem.h>
+
+#define SHMEM_DEBUG
+
+int mem_fd;
+unsigned char *map_base, *virt_addr;
+struct shmem_info* shmem_info_p;
+static void* map_memory(off_t target);
+
+int get_shmem_lock(void);
+void release_shmem_lock(void);
+void init_shared_memory(void);
+void copy_params_to_shmem(struct param_info_s *);
+void update_to_shmem(param_t param, union param_value_u value);
+int update_from_shmem(param_t param, union param_value_u *value);
+uint64_t update_from_shmem_prev_time=0, update_from_shmem_current_time=0;
+static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS/8+1];
+
+struct param_wbuf_s {
+	param_t			param;
+	union param_value_u	val;
+	bool			unsaved;
+};
+extern struct param_wbuf_s * param_find_changed(param_t param);
+
+#define MEMDEVICE	"/dev/mem"
+
+static void* map_memory(off_t target)
+{
+
+    if((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) 
+    {
+	PX4_ERR("Cannot open %s\n", MEMDEVICE);
+	exit(1);
+    }
+
+    /* Map one page */
+    map_base = (unsigned char*) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, target & ~MAP_MASK);
+    if(map_base == (void *) -1) 
+    {
+	PX4_ERR("Cannot mmap /dev/atl_mem\n");
+	exit(1);
+    }
+
+    return (map_base + (target & MAP_MASK) + LOCK_SIZE);
+
+}
+
+int get_shmem_lock(void)
+{
+	int i=0;
+	
+	/*ioctl calls cmpxchg*/
+	while(ioctl(mem_fd, LOCK_MEM)!=0)
+	{
+		PX4_INFO("Could not get lock, spinning\n");
+		usleep(100000); //sleep for 100 msec
+		i++;
+		if(i>100) break;
+	}
+	if(i>100) return -1;
+	return 0; //got the lock
+}
+
+void release_shmem_lock(void)
+{
+	*(virt_addr-LOCK_SIZE)=1;
+}
+
+void init_shared_memory(void)
+{
+
+	virt_addr = map_memory(MAP_ADDRESS); //16K space
+	shmem_info_p = (struct shmem_info*)virt_addr;
+
+	//PX4_INFO("linux memory mapped\n");
+}
+
+void copy_params_to_shmem(struct param_info_s *param_info_base)
+{
+	param_t	param;
+	unsigned int i;
+
+	if(get_shmem_lock()!=0) {
+		PX4_ERR("Could not get shmem lock\n");
+		return;
+	}			
+
+	//PX4_INFO("%d krait params allocated\n", param_count());
+	for (param = 0; param<param_count(); param++) {
+		struct param_wbuf_s *s = param_find_changed(param);
+		if(s==NULL) shmem_info_p->params_val[param] = param_info_base[param].val; 
+		else shmem_info_p->params_val[param] = s->val; 
+#ifdef SHMEM_DEBUG
+		if(param_type(param)==PARAM_TYPE_INT32){
+		{PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));}
+		}
+		else if(param_type(param)==PARAM_TYPE_FLOAT){
+		{PX4_INFO("%d: written %f for param %s to shared mem", param, (double)shmem_info_p->params_val[param].f, param_name(param));}
+		}
+#endif
+	}
+	
+	//PX4_INFO("written %u params to shmem offset %lu\n", param_count(), (unsigned char*)&shmem_info_p->params_count-(unsigned char*)shmem_info_p);
+
+	for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
+	{
+		shmem_info_p->krait_changed_index[i]=0;
+		adsp_changed_index[i]=0;
+	}
+
+	release_shmem_lock();
+}
+
+/*update value and param's change bit in shared memory*/
+void update_to_shmem(param_t param, union param_value_u value)
+{
+	unsigned int byte_changed, bit_changed;
+
+	if(get_shmem_lock()!=0) {
+		fprintf(stderr, "Could not get shmem lock\n");
+		return;
+	}			
+
+	shmem_info_p->params_val[param] = value;
+
+	byte_changed = param/8;
+	bit_changed = 1 << param%8;
+	shmem_info_p->krait_changed_index[byte_changed]|=bit_changed;
+
+	//PX4_INFO("set %d bit on krait changed index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->krait_changed_index[byte_changed]);
+
+#ifdef SHMEM_DEBUG
+	if(param_type(param)==PARAM_TYPE_INT32)
+	{PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);}
+	else if(param_type(param)==PARAM_TYPE_FLOAT)
+	{PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n", (double)value.f, param_name(param), byte_changed, bit_changed);}
+#endif
+
+	release_shmem_lock();
+	
+} 
+
+static void update_index_from_shmem(void)
+{
+	unsigned int i;
+
+	if(get_shmem_lock()!=0) {
+		fprintf(stderr, "Could not get shmem lock\n");
+		return;
+	}			
+
+	//PX4_INFO("Updating index from shmem\n");
+
+	for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
+		adsp_changed_index[i]=shmem_info_p->adsp_changed_index[i];
+
+	release_shmem_lock();
+}
+
+
+static void update_value_from_shmem(param_t param, union param_value_u *value)
+{
+	unsigned int byte_changed, bit_changed;
+
+	if(get_shmem_lock()!=0) {
+		fprintf(stderr, "Could not get shmem lock\n");
+		return;
+	}			
+
+	*value = shmem_info_p->params_val[param];
+
+	/*also clear the index since we are holding the lock*/
+	byte_changed = param/8;
+	bit_changed = 1 << param%8;
+	shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed; 
+
+	release_shmem_lock();
+
+#ifdef SHMEM_DEBUG
+	if(param_type(param)==PARAM_TYPE_INT32)
+	{PX4_INFO("Got value %d for param %s from shmem, cleared adsp index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);}
+	else if(param_type(param)==PARAM_TYPE_FLOAT)
+	{PX4_INFO("Got value %f for param %s from shmem, cleared adsp index %d:%d\n", (double)value->f, param_name(param), byte_changed, bit_changed);}
+#endif
+}
+
+int update_from_shmem(param_t param, union param_value_u *value)
+{
+	unsigned int byte_changed, bit_changed;
+	unsigned int retval = 0;
+
+        update_from_shmem_current_time = hrt_absolute_time();
+        if((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) //update every 1 second
+	{
+		update_from_shmem_prev_time = update_from_shmem_current_time;
+		update_index_from_shmem();
+	}
+
+	byte_changed = param/8;
+	bit_changed = 1 << param%8;
+
+	if(adsp_changed_index[byte_changed] & bit_changed)
+	{
+		update_value_from_shmem(param, value);
+		adsp_changed_index[byte_changed] &= ~bit_changed; //clear the bit
+		retval = 1;
+	}
+	//else {PX4_INFO("no change to param %s\n", param_name(param));}
+
+	//PX4_INFO("%s %d bit on adsp index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed);
+
+	return retval;
+}
+
+
diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp
index d9e9d1c6a9..2d83e8e201 100644
--- a/src/platforms/posix/tests/muorb/muorb_test_main.cpp
+++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp
@@ -43,7 +43,7 @@
 #include "muorb_test_example.h"
 #include <stdio.h>
 #include "uORB/uORBManager.hpp"
-#include "uORBKraitFastRpcChannel.hpp"
+#include "../../../modules/muorb/krait/uORBKraitFastRpcChannel.hpp"
 
 int PX4_MAIN(int argc, char **argv)
 {
diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
index 1276b740a2..6ae89ebc80 100644
--- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
+++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
@@ -61,10 +61,12 @@ static int writer_main(int argc, char *argv[])
 
 	int fd = px4_open(TESTDEV, PX4_F_WRONLY);
 
+	int fd = px4_open(TESTDEV, PX4_F_WRONLY);
 	if (fd < 0) {
 		PX4_INFO("Writer: Open failed %d %d", fd, px4_errno);
 		return -px4_errno;
 	}
+src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
 
 	int ret;
 	int i = 0;
@@ -92,6 +94,12 @@ static int writer_main(int argc, char *argv[])
 
 class PrivData
 {
+	PrivData() : _read_offset(0) {}
+	~PrivData() {}
+	
+	size_t _read_offset;
+};
+	
 public:
 	PrivData() : _read_offset(0) {}
 	~PrivData() {}
@@ -321,12 +329,16 @@ int VCDevExample::main()
 				 (char *const *)NULL);
 
 	ret = 0;
-
 	PX4_INFO("TEST: BLOCKING POLL ---------------");
 
 	if (do_poll(fd, -1, 3, 0)) {
 		ret = 1;
 		goto fail2;
+	PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
+	if(do_poll(fd, 0, 3, 0)) {
+		ret = 1;
+		goto fail2;
+		goto fail2;
 	}
 
 	PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
@@ -348,8 +360,7 @@ int VCDevExample::main()
 	if (do_poll(fd, 1000, 3, 0)) {
 		ret = 1;
 		goto fail2;
-	}
-
+	PX4_INFO("TEST: waiting for writer to stop");
 fail2:
 	g_exit = true;
 	px4_close(fd);
diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h
index 96db1d78cb..81ab25ad4a 100644
--- a/src/platforms/px4_posix.h
+++ b/src/platforms/px4_posix.h
@@ -77,10 +77,20 @@ typedef sem_t px4_sem_t;
 
 #define px4_sem_init	 sem_init
 #define px4_sem_wait	 sem_wait
+
 #define px4_sem_post	 sem_post
 #define px4_sem_getvalue sem_getvalue
 #define px4_sem_destroy	 sem_destroy
 
+#if 0
+// TODO: Implement this function or remove it from the implementation.
+// #define px4_sem_timedwait sem_timedwait
+inline int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime)
+{
+	return -1;
+}
+#endif
+
 #ifdef __PX4_QURT
 __EXPORT int		px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime);
 #else
diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt
index b1b99b63f2..349ac26743 100644
--- a/src/platforms/qurt/px4_layer/CMakeLists.txt
+++ b/src/platforms/qurt/px4_layer/CMakeLists.txt
@@ -31,6 +31,8 @@
 #
 ############################################################################
 
+include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
+
 set(QURT_LAYER_SRCS 
 	px4_qurt_impl.cpp
 	px4_qurt_tasks.cpp
@@ -39,6 +41,7 @@ set(QURT_LAYER_SRCS
 	qurt_stubs.c
 	main.cpp
 	params.c
+	shmem_qurt.c
 	)
 if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
 	list(APPEND QURT_LAYER_SRCS
@@ -47,18 +50,6 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
 		)
 endif()
 
-# For Eagle, the commands are specific to the build config label
-# e.g. config_qurt_eagle_hil uses commands_hil.c
-if ("${BOARD}" STREQUAL "eagle")
-
-	# The CI test target can use the hil commands
-	if ("${LABEL}" STREQUAL "travis")
-		set(CONFIG_SRC commands_hil.c)
-	else()
-		set(CONFIG_SRC commands_${LABEL}.c)
-	endif()
-
-endif()
 
 px4_add_module(
 	MODULE platforms__qurt__px4_layer
diff --git a/src/platforms/qurt/px4_layer/commands_hello.c b/src/platforms/qurt/px4_layer/commands_hello.c
deleted file mode 100644
index b2ef09b457..0000000000
--- a/src/platforms/qurt/px4_layer/commands_hello.c
+++ /dev/null
@@ -1,45 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * @file commands_hello.c
- * Commands to run for the "qurt_hello" config
- *
- * @author Mark Charlebois <charlebm@gmail.com>
- */
-
-const char *get_commands()
-{
-	static const char *commands = "hello start";
-
-	return commands;
-}
diff --git a/src/platforms/qurt/px4_layer/commands_muorb.c b/src/platforms/qurt/px4_layer/commands_muorb.c
deleted file mode 100644
index de6381a35a..0000000000
--- a/src/platforms/qurt/px4_layer/commands_muorb.c
+++ /dev/null
@@ -1,80 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * @file commands_muorb_test.c
- * Commands to run for the "qurt_muorb_test" config
- *
- * @author Mark Charlebois <charlebm@gmail.com>
- */
-
-const char *get_commands()
-{
-	static const char *commands =
-		"uorb start\n"
-		"muorb_test start\n";
-
-	/*
-	                  "hil mode_pwm\n"
-	                  "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n";
-	*/
-	/*
-	                  "param show\n"
-	                  "param set CAL_GYRO_ID 2293760\n"
-	                  "param set CAL_ACC0_ID 1310720\n"
-	                  "param set CAL_ACC1_ID 1376256\n"
-	                  "param set CAL_MAG0_ID 196608\n"
-	                  "gyrosim start\n"
-	                  "accelsim start\n"
-	                  "rgbled start\n"
-	                  "tone_alarm start\n"
-	                  "simulator start -s\n"
-	                  "commander start\n"
-	                  "sensors start\n"
-	                  "ekf_att_pos_estimator start\n"
-	                  "mc_pos_control start\n"
-	                  "mc_att_control start\n"
-	                  "param set MAV_TYPE 2\n"
-	                  "param set RC1_MAX 2015\n"
-	                  "param set RC1_MIN 996\n"
-	                  "param set RC_TRIM 1502\n"
-	*/
-
-	return commands;
-	/*====================================== Working set
-	======================================*/
-
-	//"muorb_test start\n"
-	//"gyrosim start\n"
-	//"adcsim start\n"
-
-}
diff --git a/src/platforms/qurt/px4_layer/commands_release.c b/src/platforms/qurt/px4_layer/commands_release.c
deleted file mode 100644
index 0e6ee8402c..0000000000
--- a/src/platforms/qurt/px4_layer/commands_release.c
+++ /dev/null
@@ -1,175 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * @file commands_muorb_test.c
- * Commands to run for the "qurt_muorb_test" config
- *
- * @author Mark Charlebois <charlebm@gmail.com>
- */
-
-const char *get_commands()
-{
-
-	static const char *commands =
-		"uorb start\n"
-		"param set CAL_GYRO0_ID 2293760\n"
-		"param set CAL_ACC0_ID 1310720\n"
-		"param set CAL_ACC1_ID 1376256\n"
-		"param set CAL_MAG0_ID 196608\n"
-		"commander start\n"
-		"param set RC_RECEIVER_TYPE 1\n"
-		"rc_receiver start -D /dev/tty-1\n"
-		"attitude_estimator_q start\n"
-		"position_estimator_inav start\n"
-		//"ekf_att_pos_estimator start\n"
-		"mc_pos_control start\n"
-		"mc_att_control start\n"
-		"sleep 1\n"
-
-		// Channel mapping for Spektrum DX8
-		"param set RC_MAP_THROTTLE 1\n"
-		"param set RC_MAP_ROLL 2\n"
-		"param set RC_MAP_PITCH 3\n"
-		"param set RC_MAP_YAW 4\n"
-		"param set RC_MAP_MODE_SW 5\n"
-		"param set RC_MAP_POSCTL_SW 6\n"
-
-		// RC calibration for Spektrum DX8
-		"param set RC1_MAX 852\n"
-		"param set RC1_MIN 171\n"
-		"param set RC1_TRIM 171\n"
-		"param set RC1_REV 1\n"
-		"param set RC2_MAX 852\n"
-		"param set RC2_MIN 171\n"
-		"param set RC2_TRIM 512\n"
-		"param set RC2_REV -1\n"
-		"param set RC3_MAX 852\n"
-		"param set RC3_MIN 171\n"
-		"param set RC3_TRIM 512\n"
-		"param set RC3_REV 1\n"
-		"param set RC4_MAX 852\n"
-		"param set RC4_MIN 171\n"
-		"param set RC4_TRIM 514\n"
-		"param set RC4_REV -1\n"
-		"param set RC5_MAX 852\n"
-		"param set RC5_MIN 171\n"
-		"param set RC5_TRIM 512\n"
-		"param set RC5_REV 1\n"
-		"param set RC6_MAX 852\n"
-		"param set RC6_MIN 171\n"
-		"param set RC6_TRIM 171\n"
-		"param set RC6_REV 1\n"
-
-		// // RC calibration for DX6i
-		// "param set RC1_MAX 2015\n"
-		// "param set RC1_MIN 996\n"
-		// "param set RC1_TRIM 1502\n"
-		// "param set RC1_REV -1\n"
-		// "param set RC2_MAX 2016 \n"
-		// "param set RC2_MIN 995\n"
-		// "param set RC2_TRIM 1500\n"
-		// "param set RC3_MAX 2003\n"
-		// "param set RC3_MIN 992\n"
-		// "param set RC3_TRIM 992\n"
-		// "param set RC4_MAX 2011\n"
-		// "param set RC4_MIN 997\n"
-		// "param set RC4_TRIM 1504\n"
-		// "param set RC4_REV -1\n"
-		// "param set RC6_MAX 2016\n"
-		// "param set RC6_MIN 992\n"
-		// "param set RC6_TRIM 1504\n"
-		// "param set RC_CHAN_CNT 8\n"
-		// "param set RC_MAP_MODE_SW 5\n"
-		// "param set RC_MAP_POSCTL_SW 7\n"
-		// "param set RC_MAP_RETURN_SW 8\n"
-
-		"sensors start\n"
-		"param set MC_YAW_P 7.0\n"
-		"param set MC_YAWRATE_P 0.1125\n"
-		"param set MC_YAWRATE_I 0.0\n"
-		"param set MC_YAWRATE_D 0\n"
-		"param set MC_PITCH_P 6.0\n"
-		"param set MC_PITCHRATE_P 0.125\n"
-		"param set MC_PITCHRATE_I 0.0\n"
-		"param set MC_PITCHRATE_D 0.0\n"
-		"param set MC_ROLL_P 6.0\n"
-		"param set MC_ROLLRATE_P 0.1125\n"
-		"param set MC_ROLLRATE_I 0.0\n"
-		"param set MC_ROLLRATE_D 0.0\n"
-		"param set ATT_W_MAG 0.00\n"
-		"param set PE_MAG_NOISE 1.0f\n"  // ekf_att_pos only
-		"param set SENS_BOARD_ROT 6\n"
-
-		"param set CAL_GYRO0_XOFF 0.0\n"
-		"param set CAL_GYRO0_YOFF 0.0\n"
-		"param set CAL_GYRO0_ZOFF 0.0\n"
-		"param set CAL_GYRO0_XSCALE 1.000000\n"
-		"param set CAL_GYRO0_YSCALE 1.000000\n"
-		"param set CAL_GYRO0_ZSCALE 1.000000\n"
-		"param set CAL_ACC0_XOFF 0.0\n"
-		"param set CAL_ACC0_YOFF 0.0\n"
-		"param set CAL_ACC0_ZOFF 0.0\n"
-		"param set CAL_ACC0_XSCALE 1.0\n"
-		"param set CAL_ACC0_YSCALE 1.0\n"
-		"param set CAL_ACC0_ZSCALE 1.0\n"
-		// "param set CAL_ACC0_XOFF 0.064189\n"
-		// "param set CAL_ACC0_YOFF 0.153990\n"
-		// "param set CAL_ACC0_ZOFF -0.000567\n"
-		"param set MPU_GYRO_LPF_ENUM 4\n"
-		"param set MPU_ACC_LPF_ENUM 4\n"
-		"param set MPU_SAMPLE_RATE_ENUM 2\n"
-		"sleep 1\n"
-		"mpu9x50 start -D /dev/spi-1\n"
-		"uart_esc start -D /dev/tty-2\n"
-		"csr_gps start -D /dev/tty-3\n"
-		"param set MAV_TYPE 2\n"
-		"list_devices\n"
-		"list_files\n"
-		"list_tasks\n"
-		"list_topics\n"
-
-		;
-
-	return commands;
-
-}
-
-/*
-simulator numbers
-		"param set MC_YAW_P 1.5\n"
-		"param set MC_PITCH_P 3.0\n"
-		"param set MC_ROLL_P 3.0\n"
-		"param set MC_YAWRATE_P 0.2\n"
-		"param set MC_PITCHRATE_P 0.03\n"
-		"param set MC_ROLLRATE_P 0.03\n"
-*/
diff --git a/src/platforms/qurt/px4_layer/commands_test.c b/src/platforms/qurt/px4_layer/commands_test.c
deleted file mode 100644
index 1c4c50f022..0000000000
--- a/src/platforms/qurt/px4_layer/commands_test.c
+++ /dev/null
@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * @file commands_default.c
- * Commands to run for the "qurt_default" config
- *
- * @author Mark Charlebois <charlebm@gmail.com>
- */
-
-const char *get_commands()
-{
-	static const char *commands =
-		"hello start\n"
-		"uorb start\n"
-		"simulator start -s\n"
-		"barosim start\n"
-		"adcsim start\n"
-		"accelsim start\n"
-		"gyrosim start\n"
-		"list_devices\n"
-		"list_topics\n"
-		"list_tasks\n"
-		"param show *\n"
-		"rgbled start\n"
-#if 0
-		"sensors start\n"
-		"param set CAL_GYRO0_ID 2293760\n"
-		"param set CAL_ACC0_ID 1310720\n"
-		"hil mode_pwm"
-		"param set CAL_ACC1_ID 1376256\n"
-		"param set CAL_MAG0_ID 196608\n"
-		"mavlink start -d /tmp/ttyS0\n"
-		"commander start\n"
-#endif
-		;
-
-	return commands;
-}
diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp
index e9c7ff9cdf..d5169793b3 100644
--- a/src/platforms/qurt/px4_layer/main.cpp
+++ b/src/platforms/qurt/px4_layer/main.cpp
@@ -46,7 +46,7 @@
 #include <string>
 #include <map>
 #include <stdio.h>
-#include <apps.h>
+#include <stdlib.h>
 #include "get_commands.h"
 
 using namespace std;
@@ -67,24 +67,28 @@ static void run_cmd(map<string, px4_main_t> &apps, const vector<string> &appargs
 	// command is appargs[0]
 	string command = appargs[0];
 
-	if (apps.find(command) != apps.end()) {
+	//replaces app.find with iterator code to avoid null pointer exception
+	for (map<string,px4_main_t>::iterator it=apps.begin(); it!=apps.end(); ++it)
+	if (it->first == command) {
 		const char *arg[2 + 1];
 
 		unsigned int i = 0;
 
 		while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
 			arg[i] = (char *)appargs[i].c_str();
-			PX4_WARN("  arg = '%s'\n", arg[i]);
+			PX4_WARN("  arg%d = '%s'\n", i, arg[i]);
 			++i;
 		}
 
 		arg[i] = (char *)0;
 		//PX4_DEBUG_PRINTF(i);
-		apps[command](i, (char **)arg);
+		if(apps[command]==NULL)
+			PX4_ERR("Null function !!\n");
+		else {
+			apps[command](i, (char **)arg);
+			break;
+		}
 
-	} else {
-		PX4_WARN("NOT FOUND.");
-		list_builtins(apps);
 	}
 }
 
@@ -104,6 +108,7 @@ static void process_commands(map<string, px4_main_t> &apps, const char *cmds)
 	const char *b = cmds;
 	char arg[256];
 
+	
 	// Eat leading whitespace
 	eat_whitespace(b, i);
 
@@ -159,6 +164,42 @@ int dspal_main(int argc, char *argv[]);
 __END_DECLS
 
 
+#define COMMANDS_ADSP_FILE	"/dev/fs/px4.config"
+
+const char *get_commands()
+{
+	int fd = open(COMMANDS_ADSP_FILE, O_RDONLY);
+
+	if(fd>0)
+	{
+		static char *commands;
+		char buf[4096];
+		int bytes_read, total_bytes=0;
+		PX4_INFO("reading commands from %s\n", COMMANDS_ADSP_FILE);
+		do{
+			bytes_read = read(fd, (void*)buf, sizeof(buf));
+			if(bytes_read>0)
+			{
+				commands=(char*)realloc(commands, total_bytes+bytes_read);
+				memcpy(commands+total_bytes, buf, bytes_read);
+				total_bytes += bytes_read;
+			}
+		}while((unsigned int)bytes_read>0);
+		close(fd);
+
+		return (const char*)commands;
+	}
+	
+	PX4_ERR("Could not open %s\n", COMMANDS_ADSP_FILE);
+
+	static const char *commands =
+		"uorb start\n"
+		;
+
+	return commands;
+}
+
+
 int dspal_entry(int argc, char *argv[])
 {
 	PX4_INFO("In main\n");
diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk
index 47902057cc..23d10df6b2 100644
--- a/src/platforms/qurt/px4_layer/module.mk
+++ b/src/platforms/qurt/px4_layer/module.mk
@@ -44,23 +44,8 @@ SRCS		 = 	\
 			px4_qurt_tasks.cpp  \
 			lib_crc32.c \
 			drv_hrt.c \
-                        qurt_stubs.c \
-                        main.cpp
-ifeq ($(CONFIG),qurt_hello)
-SRCS +=			commands_hello.c
-endif
-ifeq ($(CONFIG),qurt_default)
-SRCS +=			commands_default.c
-endif
-ifeq ($(CONFIG),qurt_muorb_test)
-SRCS +=			commands_muorb_test.c
-endif
-ifeq ($(CONFIG),qurt_hil)
-SRCS +=			commands_hil.c
-endif
-ifeq ($(CONFIG),qurt_adsp)
-SRCS +=			commands_adsp.c
-endif
-
+            		qurt_stubs.c \
+            		main.cpp \
+			shmem_qurt.c
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
index be419d4069..071e6d88ce 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
@@ -71,6 +71,9 @@ unsigned int sleep(unsigned int sec)
 }
 
 extern void hrt_init(void);
+extern void init_own_params();
+extern unsigned int init_other_params();
+extern unsigned int param_sync_done;
 
 #if 0
 void qurt_log(const char *fmt, ...)
@@ -108,6 +111,10 @@ void init_once(void)
 	hrt_work_queue_init();
 	hrt_init();
 	PX4_WARN("after calling hrt_init");
+
+	/*Shared memory param sync*/
+	init_own_params();
+	param_sync_done=init_other_params();
 }
 
 void init(int argc, char *argv[], const char *app_name)
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
index aa89d25606..1a13558d6f 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
@@ -106,7 +106,7 @@ static void *entry_adapter(void *ptr)
 void
 px4_systemreset(bool to_bootloader)
 {
-	PX4_WARN("Called px4_system_reset");
+	PX4_WARN("Called px4_system_reset but NOT yet implemented.");
 }
 
 px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c
new file mode 100644
index 0000000000..0fc24c0945
--- /dev/null
+++ b/src/platforms/qurt/px4_layer/shmem_qurt.c
@@ -0,0 +1,260 @@
+
+/****************************************************************************
+ *
+ * Copyright (c) 2015 Vijay Venkatraman. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_defines.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <atomic_ops.h>
+
+#include "systemlib/param/param.h"
+
+#include <shmem.h>
+#include <drivers/drv_hrt.h>
+
+#define SHMEM_DEBUG
+
+int mem_fd;
+unsigned char *map_base, *virt_addr;
+struct shmem_info* shmem_info_p;
+static void* map_memory(off_t target);
+int get_shmem_lock(void);
+void release_shmem_lock(void);
+void init_shared_memory(void);
+void copy_params_to_shmem(struct param_info_s *);
+void update_to_shmem(param_t param, union param_value_u value);
+int update_from_shmem(param_t param, union param_value_u *value);
+uint64_t update_from_shmem_prev_time=0, update_from_shmem_current_time=0;
+static unsigned char krait_changed_index[MAX_SHMEM_PARAMS/8+1];
+
+struct param_wbuf_s {
+	param_t			param;
+	union param_value_u	val;
+	bool			unsaved;
+};
+extern struct param_wbuf_s * param_find_changed(param_t param);
+
+static void* map_memory(off_t target)
+{
+
+    return (void*)(target+LOCK_SIZE);
+
+}
+
+int get_shmem_lock(void)
+{
+	unsigned char *lock = (unsigned char*)(MAP_ADDRESS+LOCK_OFFSET);
+	unsigned int i=0;
+
+	while(!atomic_compare_and_set(lock, 1, 0))
+	{
+		PX4_INFO("Could not get lock. spinning\n");
+		i++;
+		usleep(1000);
+		if(i>100) break;
+	}
+
+	if(i>100) return -1;
+
+	return 0; //got the lock
+
+}
+
+void release_shmem_lock(void)
+{
+	unsigned char *lock = (unsigned char*)(MAP_ADDRESS+LOCK_OFFSET);
+
+	*lock=1;
+	return;
+}
+
+void init_shared_memory(void)
+{
+	//PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000);
+
+	virt_addr = map_memory(MAP_ADDRESS);
+	shmem_info_p = (struct shmem_info*)virt_addr;
+
+	//PX4_INFO("adsp memory mapped\n");
+}
+
+void copy_params_to_shmem(struct param_info_s *param_info_base)
+{
+	param_t	param;
+	unsigned int i;
+
+	if(get_shmem_lock()!=0) {
+		PX4_INFO("Could not get shmem lock\n");
+		return;
+	}			
+	//else PX4_INFO("Got lock\n");
+
+	for (param = 0; param<param_count(); param++) {
+		//{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);}
+		struct param_wbuf_s *s = param_find_changed(param);
+		if(s==NULL) shmem_info_p->params_val[param] = param_info_base[param].val; 
+		else shmem_info_p->params_val[param] = s->val; 
+
+#ifdef SHMEM_DEBUG
+		if(param_type(param)==PARAM_TYPE_INT32){
+		PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));}
+		else if(param_type(param)==PARAM_TYPE_FLOAT){
+		PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param));}
+#endif
+	}
+	
+	for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
+	{
+		shmem_info_p->adsp_changed_index[i]=0;
+		krait_changed_index[i]=0;
+	}
+
+	release_shmem_lock();
+	//PX4_INFO("Released lock\n");
+
+}
+
+
+/*update value and param's change bit in shared memory*/
+void update_to_shmem(param_t param, union param_value_u value)
+{
+	unsigned int byte_changed, bit_changed;
+
+	if(!handle_in_range(param))
+		return;
+
+	if(get_shmem_lock()!=0) {
+		PX4_ERR("Could not get shmem lock\n");
+		return;
+	}			
+
+	shmem_info_p->params_val[param] = value;
+
+	byte_changed = param/8;
+	bit_changed = 1 << param%8;
+	shmem_info_p->adsp_changed_index[byte_changed]|=bit_changed;
+
+	//PX4_INFO("set %d bit on adsp index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->adsp_changed_index[byte_changed]);
+
+#ifdef SHMEM_DEBUG
+	if(param_type(param)==PARAM_TYPE_INT32)
+	{PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);}
+	else if(param_type(param)==PARAM_TYPE_FLOAT)
+	{PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed, bit_changed);}
+#endif
+
+	release_shmem_lock();
+	
+} 
+
+static void update_index_from_shmem(void)
+{
+	unsigned int i;
+
+	if(get_shmem_lock()!=0) {
+		PX4_ERR("Could not get shmem lock\n");
+		return;
+	}			
+
+	//PX4_INFO("Updating index from shmem\n");
+
+	for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
+		krait_changed_index[i]=shmem_info_p->krait_changed_index[i];
+
+	release_shmem_lock();
+}
+
+
+static void update_value_from_shmem(param_t param, union param_value_u *value)
+{
+	unsigned int byte_changed, bit_changed;
+
+	if(get_shmem_lock()!=0) {
+		PX4_ERR("Could not get shmem lock\n");
+		return;
+	}			
+
+	*value = shmem_info_p->params_val[param];
+
+	/*also clear the index since we are holding the lock*/
+	byte_changed = param/8;
+	bit_changed = 1 << param%8;
+	shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed; 
+
+	release_shmem_lock();
+
+#ifdef SHMEM_DEBUG
+	if(param_type(param)==PARAM_TYPE_INT32)
+	{PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);}
+	else if(param_type(param)==PARAM_TYPE_FLOAT)
+	{PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed, bit_changed);}
+#endif
+}
+
+int update_from_shmem(param_t param, union param_value_u *value)
+{
+	unsigned int byte_changed, bit_changed;
+	unsigned int retval = 0;
+
+	if(!handle_in_range(param) || value==NULL)
+		return retval;
+
+        update_from_shmem_current_time = hrt_absolute_time();
+        if((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) //update every 1 second
+	{
+		update_from_shmem_prev_time = update_from_shmem_current_time;
+		update_index_from_shmem();
+	}
+
+	byte_changed = param/8;
+	bit_changed = 1 << param%8;
+
+	if(krait_changed_index[byte_changed] & bit_changed)
+	{
+		update_value_from_shmem(param, value);
+		krait_changed_index[byte_changed] &= ~bit_changed; 
+		retval = 1;
+	}
+	//else {PX4_INFO("no change to param %s\n", param_name(param));}
+
+	//PX4_INFO("%s %d bit on krait changed index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed);
+
+	return retval;
+}
+
+
diff --git a/src/platforms/qurt/stubs/stubs_qurt.c b/src/platforms/qurt/stubs/stubs_qurt.c
index f114791c4d..03da646cb0 100644
--- a/src/platforms/qurt/stubs/stubs_qurt.c
+++ b/src/platforms/qurt/stubs/stubs_qurt.c
@@ -2,6 +2,8 @@
 #include <qurt_log.h>
 #include <dspal_platform.h>
 
+#ifdef CONFIG_ARCH_BOARD_EAGLE
+
 void HAP_debug(const char *msg, int level, const char *filename, int line)
 {
 }
@@ -14,3 +16,4 @@ int dlinit(int a, char **b)
 {
 	return 1;
 }
+#endif
diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp
index 3b932b5f13..eeb53e5c7d 100644
--- a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp
+++ b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp
@@ -88,7 +88,7 @@ int MuorbTestExample::DefaultTest()
 
 	int i = 0;
 	pwm.error_count++;
-	sc.gyro_errcount++;
+	sc.gyro_errcount[i]++;
 
 	while (!appState.exitRequested() && i < 10) {
 
diff --git a/src/platforms/qurt/px4_layer/commands_default.c b/src/platforms/shmem.h
similarity index 69%
rename from src/platforms/qurt/px4_layer/commands_default.c
rename to src/platforms/shmem.h
index ce7ac90768..2704836587 100644
--- a/src/platforms/qurt/px4_layer/commands_default.c
+++ b/src/platforms/shmem.h
@@ -1,6 +1,7 @@
+
 /****************************************************************************
  *
- *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ * Copyright (c) 2015 Ramakrishna Kintada. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -30,28 +31,30 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-/**
- * @file commands_default.c
- * Commands to run for the "qurt_eagle_default" config
- *
- * @author Mark Charlebois <charlebm@gmail.com>
- */
 
-#include "get_commands.h"
+#define MAX_SHMEM_PARAMS 3850 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info))
 
-const char *get_commands()
+struct shmem_info
 {
+	union param_value_u params_val[MAX_SHMEM_PARAMS];
+	unsigned char krait_changed_index[MAX_SHMEM_PARAMS/8+1]; /*bit map of all params changed by krait*/
+	unsigned char adsp_changed_index[MAX_SHMEM_PARAMS/8+1]; /*bit map of all params changed by adsp*/
 
-	static const char *commands =
-		"uorb start\n"
-		"param set CAL_GYRO0_ID 2293760\n"
-		"param set CAL_ACC0_ID 1310720\n"
-		"param set CAL_ACC1_ID 1376256\n"
-		"param set CAL_MAG0_ID 196608\n"
-		"commander start\n"
+#ifdef __PX4_NUTTX
+};
+#else
+}__attribute__((packed));
+#endif
 
-		;
+#define MAP_ADDRESS	0xfbfc000
+#define MAP_SIZE 	16384
+#define MAP_MASK 	(MAP_SIZE - 1)
 
-	return commands;
+#define LOCK_OFFSET	0
+#define LOCK_SIZE	4
+#define LOCK_MEM        1
+#define UNLOCK_MEM      2
 
-}
+#define TYPE_MASK 	0x1
+
+extern bool handle_in_range(param_t);
diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp
index c83eabc9a7..ca77594e91 100644
--- a/src/systemcmds/mixer/mixer.cpp
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -108,7 +108,6 @@ load(const char *devname, const char *fname)
 	usleep(20000);
 
 	int		dev;
-	char		buf[2048];
 
 	/* open the device */
 	if ((dev = px4_open(devname, 0)) < 0) {
@@ -122,6 +121,9 @@ load(const char *devname, const char *fname)
 		return 1;
 	}
 
+#ifndef __PX4_QURT
+
+	char		buf[2048];
 	if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) {
 		warnx("can't load mixer: %s", fname);
 		return 1;
@@ -129,6 +131,24 @@ load(const char *devname, const char *fname)
 
 	/* XXX pass the buffer to the device */
 	int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
+#else
+        char newbuf[] =
+          "R: 4x 10000 10000 10000 0\n"
+          "M: 1\n"
+          "O: 10000 10000 0 -10000 10000\n"
+          "S: 0 4 10000 10000 0 -10000 10000\n"
+          "M: 1\n"
+          "O: 10000 10000 0 -10000 10000\n"
+          "S: 0 5 10000 10000 0 -10000 10000\n"
+          "M: 1\n"
+          "O: 10000 10000 0 -10000 10000\n"
+          "S: 0 6 10000 10000 0 -10000 10000\n"
+          "M: 1\n"
+          "O: 10000 10000 0 -10000 10000\n"
+          "S: 0 7 10000 10000 0 -10000 10000\n";
+	/* XXX pass the buffer to the device */
+	int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf);
+#endif
 
 	if (ret < 0) {
 		warnx("error loading mixers from %s", fname);