From 014f15d8b01b2231b2d7717b299b0225eed6b327 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Mon, 18 Jan 2016 23:16:31 -0800
Subject: [PATCH] Rebase changes on upstream master

This brings in many of the changes from the PX4 fork on ATLFLight.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 CMakeLists.txt                                |  4 +-
 Makefile                                      |  5 +-
 cmake/configs/nuttx_px4fmu-v2_default.cmake   |  1 +
 cmake/configs/qurt_eagle_release.cmake        | 27 ++++++--
 cmake/posix/px4_impl_posix.cmake              |  9 +--
 .../Toolchain-arm-linux-gnueabihf.cmake       | 21 ++++--
 src/drivers/device/device.h                   | 41 ------------
 src/drivers/device/vfile.cpp                  | 65 -------------------
 src/include/mavlink/mavlink_log.h             |  4 +-
 .../attitude_estimator_q_main.cpp             | 32 +++++++++
 src/modules/commander/commander.cpp           |  7 +-
 .../mc_pos_control/mc_pos_control_main.cpp    | 11 ++++
 src/modules/muorb/adsp/module.mk              |  4 ++
 src/modules/muorb/krait/CMakeLists.txt        |  2 +-
 src/modules/navigator/mission_block.cpp       | 26 ++++++++
 src/modules/simulator/simulator_mavlink.cpp   | 15 +++--
 .../systemlib/circuit_breaker_params.c        |  2 +
 src/modules/systemlib/param/param_shmem.c     | 27 +++++---
 src/modules/systemlib/system_params.c         |  4 ++
 src/modules/uORB/uORBDevices_posix.cpp        |  1 -
 .../posix/px4_layer/px4_posix_impl.cpp        | 10 ++-
 src/platforms/posix/px4_layer/shmem_posix.c   |  9 ++-
 .../tests/vcdev_test/vcdevtest_example.cpp    | 14 +++-
 src/platforms/px4_posix.h                     | 10 +++
 src/platforms/qurt/px4_layer/main.cpp         | 37 ++++++-----
 .../qurt/px4_layer/px4_qurt_impl.cpp          |  9 ++-
 src/platforms/shmem.h                         |  1 -
 src/systemcmds/mixer/mixer.cpp                | 22 ++++++-
 28 files changed, 251 insertions(+), 169 deletions(-)
 delete mode 100644 src/drivers/device/device.h
 delete mode 100644 src/drivers/device/vfile.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index de40abfd08..f4b2d4fde8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -85,7 +85,7 @@
 # * If a target from add_custom_* is set in a function, explicitly pass it
 #	as an output argument so that the target name is clear to the user.
 #
-# * Avoid use of global variables in functions. Functions in a nested 
+# * Avoid use of global variables in functions. Functions in a nested
 #	scope may use global variables, but this makes it difficult to
 #	resuse functions.
 #
@@ -335,7 +335,7 @@ foreach(module ${config_module_list})
 		STRING(REGEX REPLACE "//" "/" EXT_MODULE ${module})
 		STRING(REGEX REPLACE "/" "__" EXT_MODULE_PREFIX ${EXT_MODULE})
 		add_subdirectory(${module} ${CMAKE_BINARY_DIR}/${EXT_MODULE_PREFIX})
-	else() 
+	else()
 		add_subdirectory(src/${module})
 	endif()
 	px4_mangle_name(${module} mangled_name)
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/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/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake
index d617d60fd5..df6f54c413 100644
--- a/cmake/configs/qurt_eagle_release.cmake
+++ b/cmake/configs/qurt_eagle_release.cmake
@@ -1,5 +1,11 @@
 include(qurt/px4_impl_qurt)
 
+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 ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
 	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
 else()
@@ -15,11 +21,22 @@ 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}/inc
-	${HEXAGON_SDK_ROOT}/inc/stddef
-	${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include
+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${EAGLE_ADDON_ROOT}/flight_controller/hexagon/libs
+	mpu9x50
+	uart_esc
+	csr_gps
+	rc_receiver
 	)
 
 message("hexagon_sdk_root is ${HEXAGON_SDK_ROOT}")
diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake
index 00532f635d..312510f0ed 100644
--- a/cmake/posix/px4_impl_posix.cmake
+++ b/cmake/posix/px4_impl_posix.cmake
@@ -87,7 +87,7 @@ function(px4_posix_generate_builtin_commands)
 		set(MAIN_DEFAULT MAIN-NOTFOUND)
 		set(STACK_DEFAULT 1024)
 		set(PRIORITY_DEFAULT SCHED_PRIORITY_DEFAULT)
-		foreach(property MAIN STACK PRIORITY) 
+		foreach(property MAIN STACK PRIORITY)
 			get_target_property(${property} ${module} ${property})
 			if(NOT ${property})
 				set(${property} ${${property}_DEFAULT})
@@ -182,10 +182,11 @@ if(UNIX AND APPLE)
 
 else()
 
+
         set(added_definitions
 		-D__PX4_POSIX
-		-D__PX4_LINUX 
-		-D__DF_LINUX 
+		-D__PX4_LINUX
+		-D__DF_LINUX
 		-DCLOCK_MONOTONIC=1
 		-Dnoreturn_function=__attribute__\(\(noreturn\)\)
 		-include ${PX4_INCLUDE_DIR}visibility.h
@@ -204,7 +205,7 @@ if ("${BOARD}" STREQUAL "eagle")
 	else()
 		set(HEXAGON_ARM_SYSROOT $ENV{HEXAGON_ARM_SYSROOT})
 	endif()
-	
+
 	# Add the toolchain specific flags
         set(added_cflags ${POSIX_CMAKE_C_FLAGS} --sysroot=${HEXAGON_ARM_SYSROOT})
         set(added_cxx_flags ${POSIX_CMAKE_CXX_FLAGS} --sysroot=${HEXAGON_ARM_SYSROOT})
diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
index 2e96beecba..0f1f26d76a 100644
--- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
+++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
@@ -36,13 +36,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()
@@ -51,7 +57,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()
@@ -66,13 +75,17 @@ foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
 	endif()
 endforeach()
 
+<<<<<<< ce0aecb062077c0c99423a12eea9ccb36adbd065
 set(C_FLAGS "--sysroot=${HEXAGON_ARM_SYSROOT}")
+=======
+set(C_FLAGS "--sysroot=${HEXAGON_SDK_ROOT}/sysroot")
+>>>>>>> Rebase changes on upstream master
 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 
+# where is the target environment
 set(CMAKE_FIND_ROOT_PATH  get_file_component(${C_COMPILER} PATH))
 
 # search for programs in the build host directories
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
deleted file mode 100644
index e145635b37..0000000000
--- a/src/drivers/device/device.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 PX4 Development Team. 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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#ifdef __PX4_NUTTX
-#include "device_nuttx.h"
-#elif defined (__PX4_POSIX)
-#include "vdev.h"
-#endif
-
diff --git a/src/drivers/device/vfile.cpp b/src/drivers/device/vfile.cpp
deleted file mode 100644
index e142f626d1..0000000000
--- a/src/drivers/device/vfile.cpp
+++ /dev/null
@@ -1,65 +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 vdev_file.cpp
- * Virtual file
- *
- * @author Mark Charlebois <charlebm@gmail.com>
- */
-
-#include "vfile.h"
-#include <stdio.h>
-
-using namespace device;
-
-VFile::VFile(const char *fname, mode_t mode) :
-	VDev("vfile", fname)
-{
-}
-
-VFile *VFile::createFile(const char *fname, mode_t mode)
-{
-	VFile *me = new VFile(fname, mode);
-	me->register_driver(fname, me);
-	return me;
-}
-
-ssize_t VFile::write(device::file_t *handlep, const char *buffer, size_t buflen)
-{
-	// ignore what was written, but let pollers know something was written
-	poll_notify(POLLIN);
-
-	return buflen;
-}
-
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/commander.cpp b/src/modules/commander/commander.cpp
index ca546d2149..a6dfd46456 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1610,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;
 				}
@@ -2977,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 a4770a0ce6..762f00713e 100644
--- a/src/modules/muorb/adsp/module.mk
+++ b/src/modules/muorb/adsp/module.mk
@@ -42,6 +42,10 @@ SRCS		= \
 		  px4muorb.cpp \
 		  uORBFastRpcChannel.cpp
 endif
+=======
+SRCS		=  param/param.c
+
+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 aa28b2d7b0..fe4c509303 100644
--- a/src/modules/muorb/krait/CMakeLists.txt
+++ b/src/modules/muorb/krait/CMakeLists.txt
@@ -46,4 +46,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
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/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/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
index fe52effbe3..97e47e3cc4 100644
--- a/src/modules/systemlib/param/param_shmem.c
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -72,7 +72,11 @@
 
 #include "shmem.h"
 
-#define debug(fmt, args...)		do { } while(0)
+#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
@@ -85,7 +89,7 @@ extern struct param_info_s	param_array[];
 extern struct param_info_s	*param_info_base;
 extern struct param_info_s	*param_info_limit;
 #else
-// TODO: start and end are reversed
+// FIXME - start and end are reversed
 static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters;
 #endif
 
@@ -110,7 +114,7 @@ const int bits_per_allocation_unit  = (sizeof(*param_changed_storage) * 8);
 extern int get_shmem_lock(void);
 extern void release_shmem_lock(void);
 
-struct param_wbuf_s *param_find_changed(param_t param);
+struct param_wbuf_s * param_find_changed(param_t param);
 
 void init_params(void);
 extern void init_shared_memory(void);
@@ -123,10 +127,9 @@ 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;
+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_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);
 
@@ -236,6 +239,11 @@ param_find_changed(param_t param)
 	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;
@@ -501,10 +509,11 @@ param_get(param_t param, void *val)
 
 	union param_value_u value;
 
-	if (update_from_shmem(param, &value)) {
-		set_called_from_get = 1;
+	if(update_from_shmem(param, &value))
+	{
+		set_called_from_get=1;
 		param_set_internal(param, &value, true, false);
-		set_called_from_get = 0;
+		set_called_from_get=0;
 	}
 
 
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 f67e2d4159..151b6287e8 100644
--- a/src/modules/uORB/uORBDevices_posix.cpp
+++ b/src/modules/uORB/uORBDevices_posix.cpp
@@ -406,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.
 		 */
diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
index 074afc1f41..1651a7ef7f 100644
--- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
@@ -60,6 +60,11 @@ long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
 
 #ifdef ENABLE_SHMEM
 extern void init_params(void);
+
+#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
@@ -78,8 +83,9 @@ void init_once(void)
 	hrt_init();
 
 #ifdef ENABLE_SHMEM
-	PX4_INFO("Syncing params to shared memory\n");
-	init_params();
+	PX4_INFO("Starting shared memory param sync\n");
+	init_own_params();
+	param_sync_done=init_other_params();
 #endif
 }
 
diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c
index 319d4287b6..d30caf5b40 100644
--- a/src/platforms/posix/px4_layer/shmem_posix.c
+++ b/src/platforms/posix/px4_layer/shmem_posix.c
@@ -1,6 +1,7 @@
+
 /****************************************************************************
  *
- * Copyright (c) 2015 Vijay Venkatraman. 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
@@ -31,6 +32,7 @@
  *
  ****************************************************************************/
 
+
 #include <px4_defines.h>
 #include <px4_posix.h>
 #include <string.h>
@@ -56,8 +58,8 @@
 
 int mem_fd;
 unsigned char *map_base, *virt_addr;
-struct shmem_info *shmem_info_p;
-static void *map_memory(off_t target);
+struct shmem_info* shmem_info_p;
+static void* map_memory(off_t target);
 
 int get_shmem_lock(void);
 void release_shmem_lock(void);
@@ -236,6 +238,7 @@ static void update_index_from_shmem(void)
 	release_shmem_lock();
 }
 
+
 static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
 	unsigned int byte_changed, bit_changed;
diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
index 3009908b2b..0cc376ac31 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 -----------");
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/main.cpp b/src/platforms/qurt/px4_layer/main.cpp
index 5ae5ac7eab..10cc2bfd5f 100644
--- a/src/platforms/qurt/px4_layer/main.cpp
+++ b/src/platforms/qurt/px4_layer/main.cpp
@@ -73,30 +73,30 @@ static void run_cmd(map<string, px4_main_t> &apps, const vector<string> &appargs
 	string command = appargs[0];
 
 	//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];
+	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;
+		unsigned int i = 0;
 
-			while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
-				arg[i] = (char *)appargs[i].c_str();
-				PX4_WARN("  arg%d = '%s'\n", i, arg[i]);
-				++i;
-			}
-
-			arg[i] = (char *)0;
+		while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
+			arg[i] = (char *)appargs[i].c_str();
+			PX4_WARN("  arg%d = '%s'\n", i, arg[i]);
+			++i;
+		}
 
-			//PX4_DEBUG_PRINTF(i);
-			if (apps[command] == NULL) {
-				PX4_ERR("Null function !!\n");
+		arg[i] = (char *)0;
 
-			} else {
-				apps[command](i, (char **)arg);
-				break;
-			}
+		//PX4_DEBUG_PRINTF(i);
+		if (apps[command] == NULL) {
+			PX4_ERR("Null function !!\n");
 
+		} else {
+			apps[command](i, (char **)arg);
+			break;
 		}
+
+	}
 }
 
 void eat_whitespace(const char *&b, int &i)
@@ -115,6 +115,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);
 
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
index 1ffc685182..f3bed14778 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
@@ -71,7 +71,9 @@ unsigned int sleep(unsigned int sec)
 }
 
 extern void hrt_init(void);
-extern void init_params();
+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, ...)
@@ -110,8 +112,9 @@ void init_once(void)
 	hrt_init();
 	PX4_WARN("after calling hrt_init");
 
-	/* Shared memory param sync*/
-	init_params();
+	/*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/shmem.h b/src/platforms/shmem.h
index 17e582654c..b46d895afe 100644
--- a/src/platforms/shmem.h
+++ b/src/platforms/shmem.h
@@ -1,4 +1,3 @@
-
 /****************************************************************************
  *
  * Copyright (c) 2015 Ramakrishna Kintada. All rights reserved.
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);