Fixes for qurt HIL build

Workaround required Eigen downgrade to 3.2. Hexagon toolchain does
not support C++11 features of newest version of Eigen.

Running make qurt_fixup will downgrade and patch Eigen for qurt.
Running make restore will revert the patch and do a git submodule update
to restore the expected Eigen version.

Added a "restore" target to undo qurt_fixup

Before doing a qurt build run:

    make qurt_fixup

That will downgrade Eigen to 3.2 and apply the require patch.
To build another target after downgrading:

    make restore

Them make the desired target (other than qurt).

Fixed type used in orb_priority to be consistent with the code
(int* was used in declaration but int32_t* used in code)

Removed unused class member variable in sensors.cpp

Added cmake fix for unit tests. The location of px4_log.c changed.

Fixed the qurt drv_hrt.c implementation to use us instead of ms for time resolution

Added px4_led.c to nuttx platform layer
Use the posix version of px4_led.c for nuttx so we don't end up with
duplicate files. It was moved out of common because it is not used by qurt.

Changed PX4_DEBUG to PX4_WARN when checking for the error condition for store_poll_waiter in vdev.cpp

Updated the px4_log.h file to make calls to the qurt_log functions.
The qurt_log function is defined in the platforms/qurt layer.

Added an option to control starting the commander module in HIL mode.

Moved the flight specific drivers to the configuration file instead of adding them
to the common tool chain file because HIL mode does not need them.

Added the uorb Subscriber and Publisher classes

Call PX4_ISFINITE macro instead of isfinite().

Added px4_led.c to nuttx platform layer
Use the posix version of px4_led.c for nuttx so we don't end up with duplicate files.
It was moved out of common because it is not used by qurt.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Makefile

@@ -118,6 +118,13 @@ ifeq ($(PX4_TARGET_OS),qurt)
 include $(PX4_BASE)makefiles/qurt/firmware_qurt.mk
 endif
 
+qurt_fixup:
+	makefiles/qurt/setup.sh $(PX4_BASE)
+
+restore:
+	cd src/lib/eigen && git checkout .
+	git submodule update
+	
 #
 # Versioning
 #
@@ -155,7 +162,7 @@ size:
 
 .PHONY: checksubmodules
 checksubmodules:
-	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
+	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh $(PX4_TARGET_OS))
 
 .PHONY: updatesubmodules
 updatesubmodules:

Tools/check_submodules.sh

@@ -73,20 +73,27 @@ fi
 
 if [ -d src/lib/eigen ]
 then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<")
-	if [ -z "$STATUSRETVAL" ]
+	echo "ARG = $1"
+	if [ $1 = "qurt" ]
 	then
-		echo "Checked Eigen submodule, correct version found"
+		# QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11
+		STATUSRETVAL=$(true)
 	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "eigen sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
+		STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<")
+		if [ -z "$STATUSRETVAL" ]
+		then
+			echo "Checked Eigen submodule, correct version found"
+		else
+			echo ""
+			echo ""
+			echo "New commits required:"
+			echo "$(git submodule summary)"
+			echo ""
+			echo ""
+			echo "eigen sub repo not at correct version. Try 'git submodule update'"
+			echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+			exit 1
+		fi
 	fi
 else
 	git submodule update --init --recursive

makefiles/qurt/config_qurt_adsp.mk

@@ -1,3 +1,9 @@
+#Added configuration specific flags here. 
+
+# For Actual flight we need to link against the driver dynamic libraries
+LDFLAGS	+= -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50
+LDFLAGS	+= -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc
+
 #
 # Makefile for the EAGLE QuRT *default* configuration
 #

makefiles/qurt/config_qurt_hil.mk

@@ -57,6 +57,7 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
+MODULES 	+= modules/controllib
 
 #
 # QuRT port

makefiles/qurt/setup.sh

@@ -0,0 +1,48 @@
+############################################################################
+#
+# 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.
+#
+# ###########################################################################
+
+#
+# This file is required to modify the PX4 project so it can be build with
+# the Hexagon toolchain.
+#
+# 1. Downgrade Eigen to 3.2 because the C++11 features of latest version
+#    are not supported
+# 2. Patch Eigen because the Hexagon toolchain complex.h defines "I"
+#
+
+cd $1
+cd src/lib/eigen/
+git checkout .
+git checkout e7850ed81f9c469e02df496ef09ae32ec0379b71
+patch -p1 < ../../../makefiles/qurt/qurt_eigen.patch 
+

makefiles/qurt/toolchain_hexagon.mk

@@ -246,10 +246,6 @@ LDFLAGS			+=  -g -mv5 -mG0lib -G0 -fpic -shared \
 			   $(EXTRALDFLAGS) \
 			   $(addprefix -L,$(LIB_DIRS))
 
-# driver dynamic libraries
-LDFLAGS	+= -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50
-LDFLAGS	+= -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc
-
 # Compiler support library
 #
 LIBGCC			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name)

src/drivers/device/vdev.cpp

@@ -367,6 +367,8 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
 			/* yes? post the notification */
 			if (fds->revents != 0)
 				sem_post(fds->sem);
+		} else {
+			PX4_WARN("Store Poll Waiter error.");
 		}
 
 	} else {
@@ -524,5 +526,4 @@ const char *VDev::devList(unsigned int *next)
 	return NULL;
 }
 
-} // namespace device
-
+} // namespace device

src/drivers/device/vdev.h

@@ -324,6 +324,13 @@ public:
 	static const char *devList(unsigned int *next);
 	static const char *topicList(unsigned int *next);
 
+	/**
+	 * Get the device name.
+	 *
+	 * @return the file system string of the device handle
+	 */
+	const char*	get_devname() { return _devname; }
+
 protected:
 
 	int register_driver(const char *name, void *data);
@@ -406,13 +413,6 @@ protected:
 	 */
 	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
 
-	/**
-	 * Get the device name.
-	 *
-	 * @return the file system string of the device handle
-	 */
-	const char*	get_devname() { return _devname; }
-
 	bool		_pub_blocked;		/**< true if publishing should be blocked */
 
 private:

src/drivers/pwm_out_sim/pwm_out_sim.cpp

@@ -406,7 +406,7 @@ PWMSim::task_main()
 				     ORB_ID(actuator_controls_1), _t_actuators, &_controls);
 
 			/* can we mix? */
-			if (_mixers != nullptr) {
+			if (_armed && _mixers != nullptr) {
 				/* do mixing */
 				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
 				outputs.timestamp = hrt_absolute_time();

src/modules/commander/commander.cpp

@@ -877,6 +877,17 @@ int commander_thread_main(int argc, char *argv[])
 	bool arm_tune_played = false;
 	bool was_armed = false;
 
+	bool startup_in_hil = false;
+	if(argc>0) {
+		if ( strcmp(argv[0],"-hil")==0) {
+			startup_in_hil = true;
+		} else {
+			PX4_ERR("Argument %s not supported.",argv[0]);
+			PX4_ERR("COMMANDER NOT STARTED");
+			thread_should_exit = true;
+		}
+	}
+
 	/* set parameters */
 	param_t _param_sys_type = param_find("MAV_TYPE");
 	param_t _param_system_id = param_find("MAV_SYS_ID");
@@ -958,7 +969,12 @@ int commander_thread_main(int argc, char *argv[])
 	status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
 	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
-	status.hil_state = vehicle_status_s::HIL_STATE_OFF;
+
+	if(startup_in_hil) {
+		status.hil_state = vehicle_status_s::HIL_STATE_ON;
+	} else {
+		status.hil_state = vehicle_status_s::HIL_STATE_OFF;
+	}
 	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -116,10 +116,10 @@ static inline int max(int val1, int val2)
 static void usage(const char *reason)
 {
 	if (reason) {
-		fprintf(stderr, "%s\n", reason);
+		PX4_INFO("%s\n", reason);
 	}
 
-	fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
 	return;
 }
 
@@ -404,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						if (isfinite(sensor.baro_alt_meter)) {
+						if (PX4_ISFINITE(sensor.baro_alt_meter)) {
 							baro_offset += sensor.baro_alt_meter;
 							baro_init_cnt++;
 						}

src/modules/sensors/sensors.cpp

@@ -239,7 +239,6 @@ private:
 	int32_t	_mag2_prio;			/**< mag2 sensor priority */
 	int32_t	_baro_prio;			/**< baro0 sensor priority */
 	int32_t	_baro1_prio;			/**< baro1 sensor priority */
-	int32_t	_diff_pres_prio;		/**< baro1 sensor priority */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -1526,6 +1525,12 @@ Sensors::parameter_update_poll(bool forced)
 		/* run through all mag sensors */
 		for (unsigned s = 0; s < 3; s++) {
 
+			/* set a valid default rotation (same as board).
+			 * if the mag is configured, this might be replaced
+			 * in the section below.
+			 */
+			_mag_rotation[s] = _board_rotation;
+
 			res = ERROR;
 			(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
 
@@ -1536,12 +1541,6 @@ Sensors::parameter_update_poll(bool forced)
 				continue;
 			}
 
-			/* set a valid default rotation (same as board).
-			 * if the mag is configured, this might be replaced
-			 * in the section below.
-			 */
-			_mag_rotation[s] = _board_rotation;
-
 			bool config_ok = false;
 
 			/* run through all stored calibrations */ 
@@ -2124,7 +2123,7 @@ Sensors::task_main()
 		warnx("Sensor initialization failed");
 		_sensors_task = -1;
 		if (_fd_adc >=0) {
-			close(_fd_adc);
+			px4_close(_fd_adc);
 			_fd_adc = -1;
 		}
 		return;
@@ -2169,9 +2168,6 @@ Sensors::task_main()
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
 
-	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
-	orb_set_interval(_gyro_sub, 4);
-
 	/*
 	 * do advertisements
 	 */

src/modules/uORB/module.mk

@@ -56,14 +56,11 @@ ifeq ($(PX4_TARGET_OS),posix-arm)
 SRCS 			+= uORBTest_UnitTest.cpp
 endif
 
-ifneq ($(PX4_TARGET_OS),qurt)
-SRCS 			+= Publication.cpp \
-			   Subscription.cpp
-endif
-
 SRCS	+= 		  objects_common.cpp \
 			  uORBUtils.cpp \
 			  uORB.cpp \
-			  uORBMain.cpp
+			  uORBMain.cpp \
+			  Publication.cpp \
+			  Subscription.cpp
 
 MAXOPTIMIZATION	 = -Os

src/modules/uORB/uORB.h

@@ -338,7 +338,7 @@ extern int	orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
  *			priority, independent of the startup order of the associated publishers.
  * @return		OK on success, ERROR otherwise with errno set accordingly.
  */
-extern int	orb_priority(int handle, int *priority) __EXPORT;
+extern int	orb_priority(int handle, int32_t *priority) __EXPORT;
 
 /**
  * Set the minimum interval between which updates are seen for a subscription.

src/platforms/common/module.mk

@@ -2,6 +2,5 @@
 # Common OS porting APIs
 #
 
-SRCS		 = px4_getopt.c \
-		   px4_log.c
+SRCS		 = px4_getopt.c
 

src/platforms/common/px4_log.c

@@ -1,5 +0,0 @@
-#include <px4_log.h>
-
-__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
-
-__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" };

src/platforms/nuttx/px4_layer/module.mk

@@ -35,6 +35,7 @@
 # NuttX porting layer
 #
 
-SRCS		 = 	px4_nuttx_tasks.c
+SRCS		 = 	px4_nuttx_tasks.c \
+			../../posix/px4_layer/px4_log.c
 
 MAXOPTIMIZATION	 = -Os

src/platforms/posix/px4_layer/module.mk

@@ -40,5 +40,6 @@ SRCS		 = 	\
 			px4_posix_tasks.cpp  \
 			lib_crc32.c \
 			drv_hrt.c \
+			px4_log.c
 
 MAXOPTIMIZATION	 = -Os

src/platforms/posix/px4_layer/px4_log.c

@@ -0,0 +1,5 @@
+#include <px4_log.h>
+
+__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
+
+__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" };

src/platforms/px4_log.h

@@ -38,6 +38,24 @@
 
 #pragma once
 
+#define _PX4_LOG_LEVEL_ALWAYS		0
+#define _PX4_LOG_LEVEL_DEBUG		1
+#define _PX4_LOG_LEVEL_WARN		2
+#define _PX4_LOG_LEVEL_ERROR		3
+#define _PX4_LOG_LEVEL_PANIC		4
+
+// Used to silence unused variable warning
+static inline void do_nothing(int level, ...)
+{
+	(void)level;
+}
+
+/****************************************************************************
+ * __px4_log_omit:
+ * Compile out the message
+ ****************************************************************************/
+#define __px4_log_omit(level, FMT, ...)   do_nothing(level, ##__VA_ARGS__)
+
 #if defined(__PX4_ROS)
 
 #include <ros/console.h>
@@ -47,6 +65,54 @@
 #define PX4_INFO(...) 	ROS_WARN(__VA_ARGS__)
 #define PX4_DEBUG(...) 	
 
+#elif defined(__PX4_QURT)
+#include "qurt_log.h"
+/****************************************************************************
+ * Messages that should never be filtered or compiled out
+ ****************************************************************************/
+#define PX4_LOG(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_INFO(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+
+#if defined(TRACE_BUILD)
+/****************************************************************************
+ * Extremely Verbose settings for a Trace build
+ ****************************************************************************/
+#define PX4_PANIC(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_WARN(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_WARN,  __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_DEBUG(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+
+#elif defined(DEBUG_BUILD)
+/****************************************************************************
+ * Verbose settings for a Debug build
+ ****************************************************************************/
+#define PX4_PANIC(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_WARN(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__,  FMT, ##__VA_ARGS__)
+#define PX4_DEBUG(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+
+#elif defined(RELEASE_BUILD)
+/****************************************************************************
+ * Non-verbose settings for a Release build to minimize strings in build
+ ****************************************************************************/
+#define PX4_PANIC(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_WARN(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__)
+#define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
+
+#else
+/****************************************************************************
+ * Medium verbose settings for a default build
+ ****************************************************************************/
+#define PX4_PANIC(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_WARN(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_WARN,  __FILENAME__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
+
+#endif
+#define PX4_LOG_NAMED(name, FMT, ...) 	qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__)
+#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name,  ##__VA_ARGS__)
+
 #else
 
 #define __STDC_FORMAT_MACROS
@@ -59,18 +125,6 @@
 __BEGIN_DECLS
 __EXPORT extern uint64_t hrt_absolute_time(void);
 
-// Used to silence unused variable warning
-static inline void do_nothing(int level, ...)
-{
-	(void)level;
-}
-
-#define _PX4_LOG_LEVEL_ALWAYS		0
-#define _PX4_LOG_LEVEL_PANIC		1
-#define _PX4_LOG_LEVEL_ERROR		2
-#define _PX4_LOG_LEVEL_WARN		3
-#define _PX4_LOG_LEVEL_DEBUG		4
-
 __EXPORT extern const char *__px4_log_level_str[5];
 __EXPORT extern int __px4_log_level_current;
 
@@ -107,11 +161,22 @@ __EXPORT extern int __px4_log_level_current;
  * Use these to implement the code level macros below
  ****************************************************************************/
 
+
 /****************************************************************************
- * __px4_log_omit:
- * Compile out the message
+ * __px4_log_named_cond:
+ * Convert a message in the form:
+ * 	PX4_LOG_COND(__dbg_enabled, "val is %d", val);
+ * to
+ * 	printf("%-5s val is %d\n", "LOG", val);
+ * if the first arg/condition is true.
  ****************************************************************************/
-#define __px4_log_omit(level, FMT, ...)   do_nothing(level, ##__VA_ARGS__)
+#define __px4_log_named_cond(name, cond, FMT, ...) \
+	__px4__log_startcond(cond)\
+	"%s " \
+	FMT\
+	__px4__log_end_fmt \
+	,name, ##__VA_ARGS__\
+	__px4__log_endline
 
 /****************************************************************************
  * __px4_log_named_cond:

src/platforms/px4_workqueue.h

@@ -47,6 +47,10 @@
 #include <queue.h>
 #include <px4_platform_types.h>
 
+#ifdef __PX4_QURT
+   #include <dspal_types.h>
+#endif
+
 __BEGIN_DECLS
 
 #define HPWORK 0

src/platforms/qurt/eagle_px4_firmware_patch.patch

@@ -0,0 +1,190 @@
+diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
+index 4ba2f98..a7b664e 100644
+--- a/src/include/mavlink/mavlink_log.h
++++ b/src/include/mavlink/mavlink_log.h
+@@ -106,10 +106,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
+  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+  * @param _text		The text to log;
+  */
+-#define mavlink_and_console_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
+-									fprintf(stderr, "telem> "); \
++#define mavlink_and_console_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
++/*									fprintf(stderr, "telem> "); \
+ 									fprintf(stderr, _text, ##__VA_ARGS__); \
+ 									fprintf(stderr, "\n");
++*/
+ 
+ /**
+  * Send a mavlink critical message and print to console.
+@@ -117,10 +118,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
+  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+  * @param _text		The text to log;
+  */
+-#define mavlink_and_console_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
+-									fprintf(stderr, "telem> "); \
++#define mavlink_and_console_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__)
++/*									fprintf(stderr, "telem> "); \
+ 									fprintf(stderr, _text, ##__VA_ARGS__); \
+ 									fprintf(stderr, "\n");
++*/
+ 
+ /**
+  * Send a mavlink emergency message and print to console.
+@@ -128,11 +130,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
+  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+  * @param _text		The text to log;
+  */
+-#define mavlink_and_console_log_info(_fd, _text, ...)			mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
+-									fprintf(stderr, "telem> "); \
++#define mavlink_and_console_log_info(_fd, _text, ...)			mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
++/*									fprintf(stderr, "telem> "); \
+ 									fprintf(stderr, _text, ##__VA_ARGS__); \
+ 									fprintf(stderr, "\n");
+-
++*/
+ struct mavlink_logmessage {
+ 	char text[MAVLINK_LOG_MAXLEN + 1];
+ 	unsigned char severity;
+diff --git a/src/lib/eigen b/src/lib/eigen
+--- a/src/lib/eigen
++++ b/src/lib/eigen
+@@ -1 +1 @@
+-Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71
++Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71-dirty
+diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
+index bbf5f8e..50c7d8b 100644
+--- a/src/modules/commander/PreflightCheck.cpp
++++ b/src/modules/commander/PreflightCheck.cpp
+@@ -378,6 +378,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 971e086..c165322 100644
+--- a/src/modules/commander/commander.cpp
++++ b/src/modules/commander/commander.cpp
+@@ -1836,8 +1836,13 @@ int commander_thread_main(int argc, char *argv[])
+ 		}
+ 
+ 		/* RC input check */
++#ifndef __PX4_QURT
+ 		if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
+-		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
++		   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_mode == vehicle_status_s::RC_IN_MODE_OFF) && !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;
+@@ -2526,7 +2531,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/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
+index 892e50f..7ac6776 100644
+--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
++++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
+@@ -384,7 +384,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
+ 	};
+ 
+ 	/* wait for initial baro value */
++#ifdef __PX4_QURT
++	// WARNING skipping
++	PX4_WARN("Hacked to skip baro initialization for testing.");
++	bool wait_baro = false;
++#else
+ 	bool wait_baro = true;
++#endif
+ 
+ 	thread_running = true;
+ 
+diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
+index 3fa4458..0ef1883 100644
+--- a/src/modules/sensors/sensors.cpp
++++ b/src/modules/sensors/sensors.cpp
+@@ -1099,7 +1099,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
+ 		raw.gyro_raw[1] = gyro_report.y_raw;
+ 		raw.gyro_raw[2] = gyro_report.z_raw;
+ 
++#ifdef __PX4_QURT
++// Temporary fix required due to lack of time sync between apps and dsp cores
++		raw.timestamp = hrt_absolute_time();
++#else
+ 		raw.timestamp = gyro_report.timestamp;
++#endif
+ 		raw.gyro_errcount = gyro_report.error_count;
+ 		raw.gyro_temp = gyro_report.temperature;
+ 	}
+@@ -2068,6 +2073,7 @@ Sensors::task_main()
+ 
+ 	/* start individual sensors */
+ 	int ret = 0;
++#if 0
+ 	do { /* create a scope to handle exit with break */
+ 		ret = accel_init();
+ 		if (ret) break;
+@@ -2091,7 +2097,7 @@ Sensors::task_main()
+ 		}
+ 		return;
+ 	}
+-
++#endif
+ 	/*
+ 	 * do subscriptions
+ 	 */
+diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp
+index 83a9378..ec86faf 100644
+--- a/src/systemcmds/mixer/mixer.cpp
++++ b/src/systemcmds/mixer/mixer.cpp
+@@ -118,6 +118,8 @@ load(const char *devname, const char *fname)
+ 		return 1;
+ 	}
+ 
++#ifndef __PX4_QURT
++
+ 	if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) {
+ 		warnx("can't load mixer: %s", fname);
+ 		return 1;
+@@ -125,6 +127,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);

src/platforms/qurt/include/qurt_log.h

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+#ifndef QURT_LOG_H
+#define QURT_LOG_H
+
+#include <stdarg.h>
+#include <stdio.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//void qurt_log(int level, const char *file, int line, const char *format, ...);
+
+// declaration to make the compiler happy.  This symbol is part of the adsp static image.
+void HAP_debug(const char *msg, int level, const char *filename, int line);
+
+static __inline void qurt_log(int level, const char *file, int line,
+			      const char *format, ...)
+{
+	char buf[256];
+	va_list args;
+	va_start(args, format);
+	vsnprintf(buf, sizeof(buf), format, args);
+	va_end(args);
+	HAP_debug(buf, level, file, line);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // QURT_LOG_H
+

src/platforms/qurt/px4_layer/commands_hil.c

@@ -48,7 +48,7 @@ const char *get_commands()
 		"param set CAL_MAG0_ID 196608\n"
 //		"rgbled start\n"
 //		"tone_alarm start\n"
-		"commander start\n"
+		"commander start -hil\n"
 		"sensors start\n"
 		//"ekf_att_pos_estimator start\n"
 		"attitude_estimator_q start\n"

src/platforms/qurt/px4_layer/drv_hrt.c

@@ -46,8 +46,6 @@
 
 static struct sq_queue_s	callout_queue;
 
-extern uint64_t get_abs_time_in_us();
-
 /* latency histogram */
 #define LATENCY_BUCKET_COUNT 8
 __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
@@ -83,15 +81,9 @@ static void hrt_unlock(void)
  */
 hrt_abstime hrt_absolute_time(void)
 {
-
-   return get_abs_time_in_us();
-/*
 	struct timespec ts;
-
-	// FIXME - clock_gettime unsupported in QuRT
-	//clock_gettime(CLOCK_MONOTONIC, &ts);
+	clock_gettime(CLOCK_MONOTONIC, &ts);
 	return ts_to_abstime(&ts);
-*/
 }
 
 /*
@@ -252,7 +244,7 @@ hrt_call_reschedule()
 	hrt_abstime	now = hrt_absolute_time();
 	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
 	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
-	uint32_t	ticks = USEC2TICK(HRT_INTERVAL_MAX*1000);
+	uint32_t	ticks = USEC2TICK(HRT_INTERVAL_MAX);
 
 	//printf("hrt_call_reschedule\n");
 	
@@ -273,11 +265,11 @@ hrt_call_reschedule()
 		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
 			//lldbg("pre-expired\n");
 			/* set a minimal deadline so that we call ASAP */
-			ticks = USEC2TICK(HRT_INTERVAL_MIN*1000);
+			ticks = USEC2TICK(HRT_INTERVAL_MIN);
 
 		} else if (next->deadline < deadline) {
 			//lldbg("due soon\n");
-			ticks = USEC2TICK((next->deadline - now)*1000);
+			ticks = USEC2TICK((next->deadline - now));
 		}
 	}
 

src/platforms/qurt/px4_layer/main.cpp

@@ -165,11 +165,9 @@ int dspal_entry( int argc, char* argv[] )
 	px4::init_once();
 	px4::init(argc, (char **)argv, "mainapp");
 	process_commands(apps, get_commands());
-        for( ;; )
-        {
-           volatile int x = 0;
-           ++x;
-        }
+	for( ;; ){
+		usleep( 1000000 );
+	}
         return 0;
 }
 

src/platforms/qurt/px4_layer/px4_qurt_impl.cpp

@@ -59,9 +59,7 @@
 __BEGIN_DECLS
 extern uint64_t get_ticks_per_us();
 
-// FIXME - sysconf(_SC_CLK_TCK) not supported
-//long PX4_TICKS_PER_SEC = get_ticks_per_us();
-long PX4_TICKS_PER_SEC = 800000000;
+long PX4_TICKS_PER_SEC = 1000;
 
 unsigned int sleep(unsigned int sec) 
 { 

src/systemcmds/mixer/mixer.cpp

@@ -91,10 +91,10 @@ static void
 usage(const char *reason)
 {
 	if (reason)
-		fprintf(stderr, "%s\n", reason);
+		PX4_INFO("%s\n", reason);
 
-	fprintf(stderr, "usage:\n");
-	fprintf(stderr, "  mixer load <device> <filename>\n");
+	PX4_INFO("usage:\n");
+	PX4_INFO("  mixer load <device> <filename>\n");
 }
 
 static int

unittests/CMakeLists.txt

@@ -75,7 +75,7 @@ endfunction()
 
 add_library( px4_platform 
 #                           ${PX_SRC}/platforms/common/px4_getopt.c
-                           ${PX_SRC}/platforms/common/px4_log.c
+                           ${PX_SRC}/platforms/posix/px4_layer/px4_log.c
                            ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp
                            ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp
                            ${PX_SRC}/platforms/posix/work_queue/work_lock.c