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>

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.