Merge branch 'master' of https://github.com/PX4/Firmware into distance_sensor

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -41,7 +41,9 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_posix.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>
@@ -49,9 +51,6 @@
 #include <poll.h>
 #include <fcntl.h>
 #include <float.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
 #include <errno.h>
 #include <limits.h>
 #include <math.h>
@@ -101,11 +100,11 @@ static void usage(const char *reason);
 static void
 usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}
 
 	fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
 }
 
 /**
@@ -114,12 +113,13 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_spawn_cmd().
+ * to px4_task_spawn_cmd().
  */
 int attitude_estimator_ekf_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		usage("missing command");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
@@ -127,39 +127,39 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		if (thread_running) {
 			warnx("already running\n");
 			/* this is not an error */
-			exit(0);
+			return 0;
 		}
 
 		thread_should_exit = false;
-		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
+		attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
 					      7700,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		thread_should_exit = true;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			warnx("running");
-			exit(0);
+			return 0;
 
 		} else {
 			warnx("not started");
-			exit(1);
+			return 1;
 		}
 
-		exit(0);
+		return 0;
 	}
 
 	usage("unrecognized command");
-	exit(1);
+	return 1;
 }
 
 /*
@@ -298,12 +298,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* Main loop*/
 	while (!thread_should_exit) {
 
-		struct pollfd fds[2];
+		px4_pollfd_struct_t fds[2];
 		fds[0].fd = sub_raw;
 		fds[0].events = POLLIN;
 		fds[1].fd = sub_params;
 		fds[1].events = POLLIN;
-		int ret = poll(fds, 2, 1000);
+		int ret = px4_poll(fds, 2, 1000);
 
 		if (ret < 0) {
 			/* XXX this is seriously bad - should be an emergency */
@@ -525,7 +525,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 							debugOutput);
 
 					/* swap values for next iteration, check for fatal inputs */
-					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
+					if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) {
 						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
 						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
 
@@ -535,7 +535,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					}
 
 					if (last_data > 0 && raw.timestamp - last_data > 30000) {
-						warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data);
+						warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data));
 					}
 
 					last_data = raw.timestamp;
@@ -572,8 +572,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					memcpy(&att.q[0],&q.data[0],sizeof(att.q));
 					att.R_valid = true;
 
-					if (isfinite(att.q[0]) && isfinite(att.q[1])
-						&& isfinite(att.q[2]) && isfinite(att.q[3])) {
+					if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
+						&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
 						// Broadcast
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -41,6 +41,7 @@
 
 #include "attitude_estimator_ekf_params.h"
 #include <math.h>
+#include <px4_defines.h>
 
 /* Extended Kalman Filter covariances */
 

src/modules/attitude_estimator_ekf/module.mk

@@ -45,4 +45,6 @@ MODULE_STACKSIZE = 1200
 
 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
 
+ifeq ($(PX4_TARGET_OS),nuttx)
 EXTRACXXFLAGS = -Wframe-larger-than=2400
+endif

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -202,7 +202,7 @@ int AttitudeEstimatorQ::start() {
 	ASSERT(_control_task == -1);
 
 	/* start the task */
-	_control_task = task_spawn_cmd("attitude_estimator_q",
+	_control_task = px4_task_spawn_cmd("attitude_estimator_q",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       2500,

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -50,7 +50,7 @@
  *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <string.h>
@@ -132,7 +132,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_spawn_cmd().
+ * to px4_task_spawn_cmd().
  */
 int attitude_estimator_so3_main(int argc, char *argv[])
 {
@@ -149,7 +149,7 @@ int attitude_estimator_so3_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
+		attitude_estimator_so3_task = px4_task_spawn_cmd("attitude_estimator_so3",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
 					      14000,

src/modules/bottle_drop/bottle_drop.cpp

@@ -40,7 +40,7 @@
  * @author Julian Oes <joes@student.ethz.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -225,7 +225,7 @@ BottleDrop::start()
 	ASSERT(_main_task == -1);
 
 	/* start the task */
-	_main_task = task_spawn_cmd("bottle_drop",
+	_main_task = px4_task_spawn_cmd("bottle_drop",
 				    SCHED_DEFAULT,
 				    SCHED_PRIORITY_DEFAULT + 15,
 				    1500,

src/modules/bottle_drop/bottle_drop_params.c

@@ -38,7 +38,7 @@
  * @author Dominik Juchli <juchlid@ethz.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <systemlib/param/param.h>
 
 /**

src/modules/commander/PreflightCheck.cpp

@@ -40,7 +40,8 @@
 * @author Johan Jansen <jnsn.johan@gmail.com>
 */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_posix.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>
@@ -48,6 +49,7 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <math.h>
+#include <poll.h>
 
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
@@ -61,6 +63,7 @@
 #include <drivers/drv_airspeed.h>
 
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_gps_position.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -74,7 +77,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 
 	char s[30];
 	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
-	int fd = open(s, 0);
+	int fd = px4_open(s, 0);
 
 	if (fd < 0) {
 		if (!optional) {
@@ -87,7 +90,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 
 	int calibration_devid;
 	int ret;
-	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
 	sprintf(s, "CAL_MAG%u_ID", instance);
 	param_get(param_find(s), &(calibration_devid));
 
@@ -98,7 +101,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 		goto out;
 	}
 
-	ret = ioctl(fd, MAGIOCSELFTEST, 0);
+	ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
 
 	if (ret != OK) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -108,7 +111,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 	}
 
 out:
-	close(fd);
+	px4_close(fd);
 	return success;
 }
 
@@ -118,7 +121,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 
 	char s[30];
 	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
-	int fd = open(s, O_RDONLY);
+	int fd = px4_open(s, O_RDONLY);
 
 	if (fd < 0) {
 		if (!optional) {
@@ -131,7 +134,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 
 	int calibration_devid;
 	int ret;
-	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
 	sprintf(s, "CAL_ACC%u_ID", instance);
 	param_get(param_find(s), &(calibration_devid));
 
@@ -142,7 +145,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 		goto out;
 	}
 
-	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+	ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
 
 	if (ret != OK) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -154,7 +157,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 	if (dynamic) {
 		/* check measurement result range */
 		struct accel_report acc;
-		ret = read(fd, &acc, sizeof(acc));
+		ret = px4_read(fd, &acc, sizeof(acc));
 
 		if (ret == sizeof(acc)) {
 			/* evaluate values */
@@ -175,7 +178,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 	}
 
 out:
-	close(fd);
+	px4_close(fd);
 	return success;
 }
 
@@ -185,7 +188,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 
 	char s[30];
 	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
-	int fd = open(s, 0);
+	int fd = px4_open(s, 0);
 
 	if (fd < 0) {
 		if (!optional) {
@@ -198,7 +201,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 
 	int calibration_devid;
 	int ret;
-	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
 	sprintf(s, "CAL_GYRO%u_ID", instance);
 	param_get(param_find(s), &(calibration_devid));
 
@@ -209,7 +212,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 		goto out;
 	}
 
-	ret = ioctl(fd, GYROIOCSELFTEST, 0);
+	ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
 
 	if (ret != OK) {
 		mavlink_and_console_log_critical(mavlink_fd,
@@ -219,7 +222,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 	}
 
 out:
-	close(fd);
+	px4_close(fd);
 	return success;
 }
 
@@ -229,7 +232,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
 
 	char s[30];
 	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
-	int fd = open(s, 0);
+	int fd = px4_open(s, 0);
 
 	if (fd < 0) {
 		if (!optional) {
@@ -240,7 +243,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	close(fd);
+	px4_close(fd);
 	return success;
 }
 
@@ -259,7 +262,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
 		goto out;
 	}
 
-	if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+	if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
 		mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
 		// XXX do not make this fatal yet
 	}
@@ -269,8 +272,38 @@ out:
 	return success;
 }
 
+static bool gnssCheck(int mavlink_fd)
+{
+	bool success = true;
+
+	int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	//Wait up to 2000ms to allow the driver to detect a GNSS receiver module
+	struct pollfd fds[1];
+	fds[0].fd = gpsSub;
+	fds[0].events = POLLIN;
+	if(poll(fds, 1, 2000) <= 0) {
+		success = false;
+	}
+	else {
+		struct vehicle_gps_position_s gps;
+		if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
+		    (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {
+			success = false;
+		}
+	}
+
+	//Report failure to detect module
+	if(!success) {
+		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+	}
+
+	close(gpsSub);
+	return success;
+}
+
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
-		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
 {
 	bool failed = false;
 
@@ -336,6 +369,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		}
 	}
 
+	/* ---- Global Navigation Satellite System receiver ---- */
+	if(checkGNSS) {
+		if(!gnssCheck(mavlink_fd)) {
+			failed = true;
+		}
+	}
+
 	/* Report status */
 	return !failed;
 }

src/modules/commander/PreflightCheck.h

@@ -59,11 +59,15 @@ namespace Commander
 *   true if the gyroscopes should be checked
 * @param checkBaro
 *   true if the barometer should be checked
+* @param checkAirspeed
+*   true if the airspeed sensor should be checked
 * @param checkRC
 *   true if the Remote Controller should be checked
+* @param checkGNSS
+*   true if the GNSS receiver should be checked
 **/
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
-	bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false);
+    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
 
 const unsigned max_mandatory_gyro_count = 1;
 const unsigned max_optional_gyro_count = 3;

src/modules/commander/accelerometer_calibration.cpp

@@ -127,12 +127,15 @@
 #include "calibration_routines.h"
 #include "commander_helper.h"
 
+#include <px4_posix.h>
+#include <px4_time.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <poll.h>
 #include <fcntl.h>
-#include <sys/prctl.h>
+//#include <sys/prctl.h>
 #include <math.h>
+#include <poll.h>
 #include <float.h>
 #include <mathlib/mathlib.h>
 #include <string.h>
@@ -143,6 +146,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -189,16 +193,16 @@ int do_accel_calibration(int mavlink_fd)
 	for (unsigned s = 0; s < max_accel_sens; s++) {
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
-		fd = open(str, 0);
+		fd = px4_open(str, 0);
 
 		if (fd < 0) {
 			continue;
 		}
 
-		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
+		device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
 
-		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
-		close(fd);
+		res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+		px4_close(fd);
 
 		if (res != OK) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
@@ -277,14 +281,14 @@ int do_accel_calibration(int mavlink_fd)
 		}
 
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
-		fd = open(str, 0);
+		fd = px4_open(str, 0);
 
 		if (fd < 0) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
 			res = ERROR;
 		} else {
-			res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
-			close(fd);
+			res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+			px4_close(fd);
 		}
 
 		if (res != OK) {
@@ -383,7 +387,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 			if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
 				(*active_sensors)++;
 			}
-			close(worker_data.subs[i]);
+			px4_close(worker_data.subs[i]);
 		}
 	}
 
@@ -432,7 +436,7 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
 	/* combine board rotation with offset rotation */
 	board_rotation = board_rotation_offset * board_rotation;
 
-	struct pollfd fds[max_accel_sens];
+	px4_pollfd_struct_t fds[max_accel_sens];
 
 	for (unsigned i = 0; i < max_accel_sens; i++) {
 		fds[i].fd = subs[i];
@@ -447,7 +451,7 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
 
 	/* use the first sensor to pace the readout, but do per-sensor counts */
 	while (counts[0] < samples_num) {
-		int poll_ret = poll(&fds[0], max_accel_sens, 1000);
+		int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000);
 
 		if (poll_ret > 0) {
 
@@ -552,3 +556,74 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
 
 	return calibrate_return_ok;
 }
+
+int do_level_calibration(int mavlink_fd) {
+	const unsigned cal_time = 5;
+	const unsigned cal_hz = 250;
+	const unsigned settle_time = 30;
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level");
+
+	param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF");
+	param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF");
+
+	float zero = 0.0f;
+	param_set(roll_offset_handler, &zero);
+	param_set(pitch_offset_handler, &zero);
+
+	px4_pollfd_struct_t fds[1];
+
+	fds[0].fd = att_sub;
+	fds[0].events = POLLIN;
+
+	float roll_mean = 0.0f;
+	float pitch_mean = 0.0f;
+	unsigned counter = 0;
+
+	// sleep for some time
+	hrt_abstime start = hrt_absolute_time();
+	while(hrt_elapsed_time(&start) < settle_time * 1000000) {
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
+		sleep(settle_time / 10);
+	}
+
+	start = hrt_absolute_time();
+	// average attitude for 5 seconds
+	while(hrt_elapsed_time(&start) < cal_time * 1000000) {
+		px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+		roll_mean += att.roll;
+		pitch_mean += att.pitch;
+		counter++;
+	}
+
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
+
+	if (counter > (cal_time * cal_hz / 2 )) {
+		roll_mean /= counter;
+		pitch_mean /= counter;
+	} else {
+		mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
+		return 1;
+	}
+
+	if (fabsf(roll_mean) > 0.8f ) {
+		mavlink_and_console_log_critical(mavlink_fd, "excess roll angle");
+		return 1;
+	} else if (fabsf(pitch_mean) > 0.8f ) {
+		mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle");
+		return 1;
+	}
+	else {
+		roll_mean *= (float)M_RAD_TO_DEG;
+		pitch_mean *= (float)M_RAD_TO_DEG;
+		param_set(roll_offset_handler, &roll_mean);
+		param_set(pitch_offset_handler, &pitch_mean);
+	}
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
+	return 0;
+
+}

src/modules/commander/accelerometer_calibration.h

@@ -45,5 +45,6 @@
 #include <stdint.h>
 
 int do_accel_calibration(int mavlink_fd);
+int do_level_calibration(int mavlink_fd);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */

src/modules/commander/airspeed_calibration.cpp

@@ -41,10 +41,13 @@
 #include "calibration_routines.h"
 #include "commander_helper.h"
 
+#include <px4_posix.h>
+#include <px4_time.h>
 #include <stdio.h>
+#include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
-#include <math.h>
+#include <cmath>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_airspeed.h>
 #include <uORB/topics/sensor_combined.h>
@@ -91,17 +94,17 @@ int do_airspeed_calibration(int mavlink_fd)
 	};
 
 	bool paramreset_successful = false;
-	int  fd = open(AIRSPEED0_DEVICE_PATH, 0);
+	int  fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
 
 	if (fd > 0) {
-		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+		if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
 
 		} else {
 			mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
 		}
 
-		close(fd);
+		px4_close(fd);
 	}
     
 	int cancel_sub = calibrate_cancel_subscribe();
@@ -133,11 +136,11 @@ int do_airspeed_calibration(int mavlink_fd)
 		}
         
 		/* wait blocking for new data */
-		struct pollfd fds[1];
+		px4_pollfd_struct_t fds[1];
 		fds[0].fd = diff_pres_sub;
 		fds[0].events = POLLIN;
 
-		int poll_ret = poll(fds, 1, 1000);
+		int poll_ret = px4_poll(fds, 1, 1000);
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
@@ -158,16 +161,16 @@ int do_airspeed_calibration(int mavlink_fd)
 
 	diff_pres_offset = diff_pres_offset / calibration_count;
 
-	if (isfinite(diff_pres_offset)) {
+	if (PX4_ISFINITE(diff_pres_offset)) {
 
-		int  fd_scale = open(AIRSPEED0_DEVICE_PATH, 0);
+		int  fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
 		airscale.offset_pa = diff_pres_offset;
 		if (fd_scale > 0) {
-			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+			if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 				mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
 			}
 
-			close(fd_scale);
+			px4_close(fd_scale);
 		}
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
@@ -206,11 +209,11 @@ int do_airspeed_calibration(int mavlink_fd)
         }
         
 		/* wait blocking for new data */
-		struct pollfd fds[1];
+		px4_pollfd_struct_t fds[1];
 		fds[0].fd = diff_pres_sub;
 		fds[0].events = POLLIN;
 
-		int poll_ret = poll(fds, 1, 1000);
+		int poll_ret = px4_poll(fds, 1, 1000);
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
@@ -270,7 +273,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 normal_return:
 	calibrate_cancel_unsubscribe(cancel_sub);
-	close(diff_pres_sub);
+	px4_close(diff_pres_sub);
 	
 	// This give a chance for the log messages to go out of the queue before someone else stomps on then
 	sleep(1);

src/modules/commander/calibration_routines.cpp

@@ -38,7 +38,11 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <px4_time.h>
 #include <stdio.h>
+#include <unistd.h>
 #include <math.h>
 #include <float.h>
 #include <poll.h>
@@ -54,9 +58,6 @@
 #include "calibration_messages.h"
 #include "commander_helper.h"
 
-// FIXME: Fix return codes
-static const int ERROR = -1;
-
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
 			     float *sphere_radius)
@@ -245,7 +246,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 	float		accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
 	hrt_abstime	still_time = lenient_still_position ? 1000000 : 1500000;	// still time required in us
     
-	struct pollfd fds[1];
+	px4_pollfd_struct_t fds[1];
 	fds[0].fd = accel_sub;
 	fds[0].events = POLLIN;
 	
@@ -261,7 +262,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 	
 	while (true) {
 		/* wait blocking for new data */
-		int poll_ret = poll(fds, 1, 1000);
+		int poll_ret = px4_poll(fds, 1, 1000);
 		
 		if (poll_ret) {
 			orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
@@ -527,11 +528,11 @@ static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &c
 
 bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
 {
-	struct pollfd fds[1];
+	px4_pollfd_struct_t fds[1];
 	fds[0].fd = cancel_sub;
 	fds[0].events = POLLIN;
 
-	if (poll(&fds[0], 1, 0) > 0) {
+	if (px4_poll(&fds[0], 1, 0) > 0) {
 		struct vehicle_command_s cmd;
 		memset(&cmd, 0, sizeof(cmd));
 		
@@ -553,4 +554,4 @@ bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
 	}
 	
 	return false;
-}
+}

src/modules/commander/commander.cpp

@@ -42,9 +42,12 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_posix.h>
+#include <px4_time.h>
 #include <pthread.h>
 #include <stdio.h>
+#include <sys/stat.h>
 #include <stdlib.h>
 #include <stdbool.h>
 #include <string.h>
@@ -53,8 +56,10 @@
 #include <errno.h>
 #include <systemlib/err.h>
 #include <systemlib/circuit_breaker.h>
-#include <debug.h>
+//#include <debug.h>
+#ifndef __PX4_QURT
 #include <sys/prctl.h>
+#endif
 #include <sys/stat.h>
 #include <string.h>
 #include <math.h>
@@ -125,6 +130,8 @@ static const int ERROR = -1;
 
 extern struct system_load_s system_load;
 
+static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */
+
 /* Decouple update interval and hysteris counters, all depends on intervals */
 #define COMMANDER_MONITORING_INTERVAL 50000
 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
@@ -252,6 +259,7 @@ int commander_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		usage("missing command");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
@@ -259,11 +267,11 @@ int commander_main(int argc, char *argv[])
 		if (thread_running) {
 			warnx("commander already running");
 			/* this is not an error */
-			exit(0);
+			return 0;
 		}
 
 		thread_should_exit = false;
-		daemon_task = task_spawn_cmd("commander",
+		daemon_task = px4_task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
 					     3400,
@@ -274,13 +282,14 @@ int commander_main(int argc, char *argv[])
 			usleep(200);
 		}
 
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 
 		if (!thread_running) {
-			errx(0, "commander already stopped");
+			warnx("commander already stopped");
+			return 0;
 		}
 
 		thread_should_exit = true;
@@ -292,18 +301,18 @@ int commander_main(int argc, char *argv[])
 
 		warnx("terminated.");
 
-		exit(0);
+		return 0;
 	}
 
 	/* commands needing the app to run below */
 	if (!thread_running) {
 		warnx("\tcommander not started");
-		exit(1);
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		print_status();
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "calibrate")) {
@@ -315,14 +324,17 @@ int commander_main(int argc, char *argv[])
 				calib_ret = do_accel_calibration(mavlink_fd);
 			} else if (!strcmp(argv[2], "gyro")) {
 				calib_ret = do_gyro_calibration(mavlink_fd);
+			} else if (!strcmp(argv[2], "level")) {
+				calib_ret = do_level_calibration(mavlink_fd);
 			} else {
 				warnx("argument %s unsupported.", argv[2]);
 			}
 
 			if (calib_ret) {
-				errx(1, "calibration failed, exiting.");
+				warnx("calibration failed, exiting.");
+				return 0;
 			} else {
-				exit(0);
+				return 0;
 			}
 		} else {
 			warnx("missing argument");
@@ -334,25 +346,25 @@ int commander_main(int argc, char *argv[])
 		int checkres = prearm_check(&status, mavlink_fd_local);
 		close(mavlink_fd_local);
 		warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "arm")) {
 		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
 		arm_disarm(true, mavlink_fd_local, "command line");
 		close(mavlink_fd_local);
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
 		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
 		arm_disarm(false, mavlink_fd_local, "command line");
 		close(mavlink_fd_local);
-		exit(0);
+                return 0;
 	}
 
 	usage("unrecognized command");
-	exit(1);
+	return 1;
 }
 
 void usage(const char *reason)
@@ -362,7 +374,6 @@ void usage(const char *reason)
 	}
 
 	fprintf(stderr, "usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n");
-	exit(1);
 }
 
 void print_status()
@@ -541,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				else {
 
 					// Refuse to arm if preflight checks have failed
-					if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
+					if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
 						mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
 						cmd_result = VEHICLE_CMD_RESULT_DENIED;			
 						break;
@@ -916,6 +927,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.circuit_breaker_engaged_airspd_check = false;
 	status.circuit_breaker_engaged_enginefailure_check = false;
 	status.circuit_breaker_engaged_gpsfailure_check = false;
+	get_circuit_breaker_params();
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -923,7 +935,7 @@ int commander_thread_main(int argc, char *argv[])
 	if (status_pub < 0) {
 		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
 		warnx("exiting.");
-		exit(ERROR);
+		px4_task_exit(ERROR);
 	}
 
 	/* armed topic */
@@ -1117,8 +1129,6 @@ int commander_thread_main(int argc, char *argv[])
 	param_get(_param_sys_type, &(status.system_type)); // get system type
 	status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
 
-	get_circuit_breaker_params();
-
 	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
@@ -1127,7 +1137,7 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	// Run preflight check
-	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
+	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 	if (!status.condition_system_sensors_initialized) {
 		set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
 	}
@@ -1300,7 +1310,7 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 					/* provide RC and sensor status feedback to the user */
-					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true);
+					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 				}
 
 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@@ -1637,19 +1647,31 @@ int commander_thread_main(int argc, char *argv[])
 						  (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
 		}
 
-		/* check if GPS fix is ok */
-		if (status.circuit_breaker_engaged_gpsfailure_check ||
-		    (gps_position.fix_type >= 3 &&
-		     hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
-			/* handle the case where gps was regained */
-			if (status.gps_failure) {
-				status.gps_failure = false;
-				status_changed = true;
-				mavlink_log_critical(mavlink_fd, "gps regained");
+		/* check if GPS is ok */
+		if (!status.circuit_breaker_engaged_gpsfailure_check) {
+			bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE;
+
+			//Check if GPS receiver is too noisy while we are disarmed
+			if (!armed.armed && gpsIsNoisy) {
+				if (!status.gps_failure) {
+					mavlink_log_critical(mavlink_fd, "GPS signal noisy");
+					set_tune_override(TONE_GPS_WARNING_TUNE);
+
+					//GPS suffers from signal jamming or excessive noise, disable GPS-aided flight
+					status.gps_failure = true;
+					status_changed = true;
+				}
 			}
 
-		} else {
-			if (!status.gps_failure) {
+			if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
+				/* handle the case where gps was regained */
+				if (status.gps_failure && !gpsIsNoisy) {
+					status.gps_failure = false;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "gps regained");
+				}
+
+			} else if (!status.gps_failure) {
 				status.gps_failure = true;
 				status_changed = true;
 				mavlink_log_critical(mavlink_fd, "gps fix lost");
@@ -2578,8 +2600,10 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 
 void *commander_low_prio_loop(void *arg)
 {
+#ifndef __PX4_QURT
 	/* Set thread name */
 	prctl(PR_SET_NAME, "commander_low_prio", getpid());
+#endif
 
 	/* Subscribe to command topic */
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -2590,7 +2614,7 @@ void *commander_low_prio_loop(void *arg)
 	hrt_abstime need_param_autosave_timeout = 0;
 
 	/* wakeup source(s) */
-	struct pollfd fds[1];
+	px4_pollfd_struct_t fds[1];
 
 	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
 	fds[0].fd = cmd_sub;
@@ -2598,7 +2622,7 @@ void *commander_low_prio_loop(void *arg)
 
 	while (!thread_should_exit) {
 		/* wait for up to 1000ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
 
 		/* timed out - periodic check for thread_should_exit, etc. */
 		if (pret == 0) {
@@ -2647,13 +2671,13 @@ void *commander_low_prio_loop(void *arg)
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 						usleep(100000);
 						/* reboot */
-						systemreset(false);
+						px4_systemreset(false);
 
 					} else if (((int)(cmd.param1)) == 3) {
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 						usleep(100000);
 						/* reboot to bootloader */
-						systemreset(true);
+						px4_systemreset(true);
 
 					} else {
 						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
@@ -2707,7 +2731,10 @@ void *commander_low_prio_loop(void *arg)
 						/* accelerometer calibration */
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 						calib_ret = do_accel_calibration(mavlink_fd);
-
+					} else if ((int)(cmd.param5) == 2) {
+						// board offset calibration
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						calib_ret = do_level_calibration(mavlink_fd);
 					} else if ((int)(cmd.param6) == 1) {
 						/* airspeed calibration */
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -2751,7 +2778,7 @@ void *commander_low_prio_loop(void *arg)
 							checkAirspeed = true;
 						}
 
-						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
+						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 
 						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 

src/modules/commander/commander_helper.cpp

@@ -41,6 +41,8 @@
  *
  */
 
+#include <px4_defines.h>
+#include <px4_posix.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <stdint.h>
@@ -117,7 +119,7 @@ int battery_init()
 	bat_capacity_h = param_find("BAT_CAPACITY");
 	bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
 
-	return OK;
+	return PX4_OK;
 }
 
 int buzzer_init()
@@ -130,24 +132,24 @@ int buzzer_init()
 	tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
 	tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
 
-	buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
+	buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
-		warnx("Buzzer: open fail\n");
+		warnx("Buzzer: px4_open fail\n");
 		return ERROR;
 	}
 
-	return OK;
+	return PX4_OK;
 }
 
 void buzzer_deinit()
 {
-	close(buzzer);
+	px4_close(buzzer);
 }
 
 void set_tune_override(int tune)
 {
-	ioctl(buzzer, TONE_SET_ALARM, tune);
+	px4_ioctl(buzzer, TONE_SET_ALARM, tune);
 }
 
 void set_tune(int tune)
@@ -158,7 +160,7 @@ void set_tune(int tune)
 	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
 		/* allow interrupting current non-repeating tune by the same tune */
 		if (tune != tune_current || new_tune_duration != 0) {
-			ioctl(buzzer, TONE_SET_ALARM, tune);
+			px4_ioctl(buzzer, TONE_SET_ALARM, tune);
 		}
 
 		tune_current = tune;
@@ -236,22 +238,22 @@ int led_init()
 	blink_msg_end = 0;
 
 	/* first open normal LEDs */
-	leds = open(LED0_DEVICE_PATH, 0);
+	leds = px4_open(LED0_DEVICE_PATH, 0);
 
 	if (leds < 0) {
-		warnx("LED: open fail\n");
+		warnx("LED: px4_open fail\n");
 		return ERROR;
 	}
 
 	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
-	(void)ioctl(leds, LED_ON, LED_BLUE);
+	(void)px4_ioctl(leds, LED_ON, LED_BLUE);
 
 	/* switch blue off */
 	led_off(LED_BLUE);
 
 	/* we consider the amber led mandatory */
-	if (ioctl(leds, LED_ON, LED_AMBER)) {
-		warnx("Amber LED: ioctl fail\n");
+	if (px4_ioctl(leds, LED_ON, LED_AMBER)) {
+		warnx("Amber LED: px4_ioctl fail\n");
 		return ERROR;
 	}
 
@@ -259,7 +261,7 @@ int led_init()
 	led_off(LED_AMBER);
 
 	/* then try RGB LEDs, this can fail on FMUv1*/
-	rgbleds = open(RGBLED0_DEVICE_PATH, 0);
+	rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0);
 
 	if (rgbleds < 0) {
 		warnx("No RGB LED found at " RGBLED0_DEVICE_PATH);
@@ -271,11 +273,11 @@ int led_init()
 void led_deinit()
 {
 	if (leds >= 0) {
-		close(leds);
+		px4_close(leds);
 	}
 
 	if (rgbleds >= 0) {
-		close(rgbleds);
+		px4_close(rgbleds);
 	}
 }
 
@@ -284,7 +286,7 @@ int led_toggle(int led)
 	if (leds < 0) {
 		return leds;
 	}
-	return ioctl(leds, LED_TOGGLE, led);
+	return px4_ioctl(leds, LED_TOGGLE, led);
 }
 
 int led_on(int led)
@@ -292,7 +294,7 @@ int led_on(int led)
 	if (leds < 0) {
 		return leds;
 	}
-	return ioctl(leds, LED_ON, led);
+	return px4_ioctl(leds, LED_ON, led);
 }
 
 int led_off(int led)
@@ -300,7 +302,7 @@ int led_off(int led)
 	if (leds < 0) {
 		return leds;
 	}
-	return ioctl(leds, LED_OFF, led);
+	return px4_ioctl(leds, LED_OFF, led);
 }
 
 void rgbled_set_color(rgbled_color_t color)
@@ -309,7 +311,7 @@ void rgbled_set_color(rgbled_color_t color)
 	if (rgbleds < 0) {
 		return;
 	}
-	ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+	px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
 }
 
 void rgbled_set_mode(rgbled_mode_t mode)
@@ -318,7 +320,7 @@ void rgbled_set_mode(rgbled_mode_t mode)
 	if (rgbleds < 0) {
 		return;
 	}
-	ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+	px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
 }
 
 void rgbled_set_pattern(rgbled_pattern_t *pattern)
@@ -327,7 +329,7 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 	if (rgbleds < 0) {
 		return;
 	}
-	ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+	px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
 }
 
 unsigned battery_get_n_cells() {

src/modules/commander/commander_params.c

@@ -42,7 +42,7 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <systemlib/param/param.h>
 
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);

src/modules/commander/esc_calibration.cpp

@@ -48,7 +48,8 @@
 #include <sys/ioctl.h>
 #include <systemlib/err.h>
 #include <fcntl.h>
-#include <poll.h>
+#include <px4_posix.h>
+#include <px4_time.h>
 #include "drivers/drv_pwm_output.h"
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_command.h>
@@ -78,7 +79,7 @@ int check_if_batt_disconnected(int mavlink_fd) {
 
 int do_esc_calibration(int mavlink_fd) {
 
-	int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0);
+	int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
 	int ret;
 
 	if(fd < 0) {
@@ -86,15 +87,15 @@ int do_esc_calibration(int mavlink_fd) {
 	}
 
 	/* tell IO/FMU that its ok to disable its safety with the switch */
-	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
 	if (ret != OK)
 		err(1, "PWM_SERVO_SET_ARM_OK");
 	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
-	ret = ioctl(fd, PWM_SERVO_ARM, 0);
+	ret = px4_ioctl(fd, PWM_SERVO_ARM, 0);
 	if (ret != OK)
 		err(1, "PWM_SERVO_ARM");
 	/* tell IO to switch off safety without using the safety switch */
-	ret = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
 	if(ret!=0) {
 		err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
 	}
@@ -143,10 +144,10 @@ int do_esc_calibration(int mavlink_fd) {
 	}
 
 	/* disarm */
-	ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+	ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0);
 	if (ret != OK)
 		err(1, "PWM_SERVO_DISARM");
 
 	mavlink_log_info(mavlink_fd,"ESC calibration finished");
 	return OK;
- }
+ }

src/modules/commander/gyro_calibration.cpp

@@ -42,10 +42,13 @@
 #include "calibration_routines.h"
 #include "commander_helper.h"
 
+#include <px4_posix.h>
+#include <px4_time.h>
 #include <stdio.h>
+#include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
-#include <math.h>
+#include <cmath>
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
@@ -82,7 +85,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 	struct gyro_report	gyro_report;
 	unsigned		poll_errcount = 0;
 	
-	struct pollfd fds[max_gyros];
+	px4_pollfd_struct_t fds[max_gyros];
 	for (unsigned s = 0; s < max_gyros; s++) {
 		fds[s].fd = worker_data->gyro_sensor_sub[s];
 		fds[s].events = POLLIN;
@@ -97,7 +100,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 			return calibrate_return_cancelled;
 		}
 		
-		int poll_ret = poll(&fds[0], max_gyros, 1000);
+		int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
 		
 		if (poll_ret > 0) {
 			
@@ -180,11 +183,11 @@ int do_gyro_calibration(int mavlink_fd)
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-		int fd = open(str, 0);
+		int fd = px4_open(str, 0);
 		if (fd >= 0) {
-			worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
-			res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
-			close(fd);
+			worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+			res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
+			px4_close(fd);
 
 			if (res != OK) {
 				mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
@@ -225,9 +228,9 @@ int do_gyro_calibration(int mavlink_fd)
 			/* maximum allowable calibration error in radians */
 			const float maxoff = 0.0055f;
 
-			if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
-			    !isfinite(worker_data.gyro_scale[0].y_offset) ||
-			    !isfinite(worker_data.gyro_scale[0].z_offset) ||
+			if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
+			    !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
+			    !PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) ||
 			    fabsf(xdiff) > maxoff ||
 			    fabsf(ydiff) > maxoff ||
 			    fabsf(zdiff) > maxoff) {
@@ -251,7 +254,7 @@ int do_gyro_calibration(int mavlink_fd)
 	calibrate_cancel_unsubscribe(cancel_sub);
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
-		close(worker_data.gyro_sensor_sub[s]);
+		px4_close(worker_data.gyro_sensor_sub[s]);
 	}
 
 	if (res == OK) {
@@ -273,15 +276,15 @@ int do_gyro_calibration(int mavlink_fd)
 
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-				int fd = open(str, 0);
+				int fd = px4_open(str, 0);
 
 				if (fd < 0) {
 					failed = true;
 					continue;
 				}
 
-				res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
-				close(fd);
+				res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
+				px4_close(fd);
 
 				if (res != OK) {
 					mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);

src/modules/commander/mag_calibration.cpp

@@ -42,11 +42,14 @@
 #include "calibration_routines.h"
 #include "calibration_messages.h"
 
+#include <px4_posix.h>
+#include <px4_time.h>
 #include <stdio.h>
+#include <unistd.h>
 #include <stdlib.h>
 #include <string.h>
 #include <poll.h>
-#include <math.h>
+#include <cmath>
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
@@ -119,16 +122,16 @@ int do_mag_calibration(int mavlink_fd)
 
 		// Attempt to open mag
 		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
-		int fd = open(str, O_RDONLY);
+		int fd = px4_open(str, O_RDONLY);
 		if (fd < 0) {
 			continue;
 		}
 
 		// Get device id for this mag
-		device_ids[cur_mag] = ioctl(fd, DEVIOCGDEVICEID, 0);
+		device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
 
 		// Reset mag scale
-		result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
+		result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
 		if (result != OK) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
@@ -136,7 +139,7 @@ int do_mag_calibration(int mavlink_fd)
 
 		/* calibrate range */
 		if (result == OK) {
-			result = ioctl(fd, MAGIOCCALIBRATE, fd);
+			result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
 
 			if (result != OK) {
 				mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
@@ -145,7 +148,7 @@ int do_mag_calibration(int mavlink_fd)
 			}
 		}
 
-		close(fd);
+		px4_close(fd);
 	}
 
 	// Calibrate all mags at the same time
@@ -274,7 +277,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		}
 		
 		// Wait clocking for new data on all mags
-		struct pollfd fds[max_mags];
+		px4_pollfd_struct_t fds[max_mags];
 		size_t fd_count = 0;
 		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (worker_data->sub_mag[cur_mag] >= 0) {
@@ -283,7 +286,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 				fd_count++;
 			}
 		}
-		int poll_ret = poll(fds, fd_count, 1000);
+		int poll_ret = px4_poll(fds, fd_count, 1000);
 		
 		if (poll_ret > 0) {
 			for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
@@ -419,7 +422,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	// Close subscriptions
 	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 		if (worker_data.sub_mag[cur_mag] >= 0) {
-			close(worker_data.sub_mag[cur_mag]);
+			px4_close(worker_data.sub_mag[cur_mag]);
 		}
 	}
 	
@@ -443,7 +446,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
 							 &sphere_radius[cur_mag]);
 				
-				if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !isfinite(sphere_z[cur_mag])) {
+				if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
 					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
 					result = calibrate_return_error;
 				}
@@ -467,14 +470,14 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 				// Set new scale
 				
 				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
-				fd_mag = open(str, 0);
+				fd_mag = px4_open(str, 0);
 				if (fd_mag < 0) {
 					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
 					result = calibrate_return_error;
 				}
 				
 				if (result == calibrate_return_ok) {
-					if (ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
+					if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
 						mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
 						result = calibrate_return_error;
 					}
@@ -485,7 +488,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					mscale.y_offset = sphere_y[cur_mag];
 					mscale.z_offset = sphere_z[cur_mag];
 
-					if (ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
+					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
 						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
 						result = calibrate_return_error;
 					}
@@ -493,7 +496,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 				
 				// Mag device no longer needed
 				if (fd_mag >= 0) {
-					close(fd_mag);
+					px4_close(fd_mag);
 				}
 
 				if (result == calibrate_return_ok) {

src/modules/commander/module.mk

@@ -54,5 +54,7 @@ MODULE_STACKSIZE = 5000
 
 MAXOPTIMIZATION	 = -Os
 
+ifeq ($(PX4_TARGET_OS),nuttx)
 EXTRACXXFLAGS = -Wframe-larger-than=2200
+endif
 

src/modules/commander/rc_calibration.cpp

@@ -36,6 +36,9 @@
  * Remote Control calibration routine
  */
 
+#include <px4_posix.h>
+#include <px4_time.h>
+
 #include "rc_calibration.h"
 #include "commander_helper.h"
 
@@ -82,11 +85,11 @@ int do_trim_calibration(int mavlink_fd)
 
 	if (save_ret != 0) {
 		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
-		close(sub_man);
+		px4_close(sub_man);
 		return ERROR;
 	}
 
 	mavlink_log_info(mavlink_fd, "trim cal done");
-	close(sub_man);
+	px4_close(sub_man);
 	return OK;
 }

src/modules/commander/state_machine_helper.cpp

@@ -39,6 +39,7 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
+#include <px4_posix.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <stdint.h>
@@ -129,10 +130,12 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
+#ifdef __PX4_NUTTX
 		/*
 		 * Perform an atomic state update
 		 */
 		irqstate_t flags = irqsave();
+#endif
 
 		/* enforce lockdown in HIL */
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
@@ -255,8 +258,10 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			status->arming_state = new_arming_state;
 		}
 
+#ifdef __PX4_NUTTX
 		/* end of atomic state update */
 		irqrestore(flags);
+#endif
 	}
 
 	if (ret == TRANSITION_DENIED) {
@@ -356,6 +361,121 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	return ret;
 }
 
+#ifdef __PX4_NUTTX
+static transition_result_t disable_publication(const int mavlink_fd)
+{
+	transition_result_t ret;
+
+	/* Disable publication of all attached sensors */
+	/* list directory */
+	DIR		*d;
+	d = opendir("/dev");
+
+	if (d) {
+		struct dirent	*direntry;
+		char devname[24];
+
+		while ((direntry = readdir(d)) != NULL) {
+
+			/* skip serial ports */
+			if (!strncmp("tty", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip mtd devices */
+			if (!strncmp("mtd", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip ram devices */
+			if (!strncmp("ram", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip MMC devices */
+			if (!strncmp("mmc", direntry->d_name, 3)) {
+				continue;
+			}
+
+			/* skip mavlink */
+			if (!strcmp("mavlink", direntry->d_name)) {
+				continue;
+			}
+
+			/* skip console */
+			if (!strcmp("console", direntry->d_name)) {
+				continue;
+			}
+
+			/* skip null */
+			if (!strcmp("null", direntry->d_name)) {
+				continue;
+			}
+
+			snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+			int sensfd = ::open(devname, 0);
+
+			if (sensfd < 0) {
+				warn("failed opening device %s", devname);
+				continue;
+			}
+
+			int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
+			close(sensfd);
+
+			printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
+		}
+		closedir(d);
+		ret = TRANSITION_CHANGED;
+		mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
+
+	} else {
+		/* failed opening dir */
+		mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
+		ret = TRANSITION_DENIED;
+	}
+	return ret;
+}
+
+#else
+
+static transition_result_t disable_publication(const int mavlink_fd)
+{
+	transition_result_t ret;
+	const char *devname;
+	unsigned int handle = 0;
+	for(;;) {
+		devname = px4_get_device_names(&handle);
+		if (devname == NULL)
+			break;
+
+		/* skip mavlink */
+		if (!strcmp("/dev/mavlink", devname)) {
+			continue;
+		}
+
+
+		int sensfd = px4_open(devname, 0);
+
+		if (sensfd < 0) {
+			warn("failed opening device %s", devname);
+			continue;
+		}
+
+		int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
+		px4_close(sensfd);
+
+			printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
+	}
+	ret = TRANSITION_CHANGED;
+	mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
+	return ret;
+}
+#endif
+
 /**
  * Transition from one hil state to another
  */
@@ -378,77 +498,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 			if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
 			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
 			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-
-				/* Disable publication of all attached sensors */
-				/* list directory */
-				DIR		*d;
-				d = opendir("/dev");
-
-				if (d) {
-					struct dirent	*direntry;
-					char devname[24];
-
-					while ((direntry = readdir(d)) != NULL) {
-
-						/* skip serial ports */
-						if (!strncmp("tty", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip mtd devices */
-						if (!strncmp("mtd", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip ram devices */
-						if (!strncmp("ram", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip MMC devices */
-						if (!strncmp("mmc", direntry->d_name, 3)) {
-							continue;
-						}
-
-						/* skip mavlink */
-						if (!strcmp("mavlink", direntry->d_name)) {
-							continue;
-						}
-
-						/* skip console */
-						if (!strcmp("console", direntry->d_name)) {
-							continue;
-						}
-
-						/* skip null */
-						if (!strcmp("null", direntry->d_name)) {
-							continue;
-						}
-
-						snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
-
-						int sensfd = ::open(devname, 0);
-
-						if (sensfd < 0) {
-							warn("failed opening device %s", devname);
-							continue;
-						}
-
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
-						close(sensfd);
-
-						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
-					}
-					closedir(d);
-					ret = TRANSITION_CHANGED;
-					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-
-
-				} else {
-					/* failed opening dir */
-					mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
-					ret = TRANSITION_DENIED;
-				}
+				ret = disable_publication(mavlink_fd);
 			} else {
 				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
 				ret = TRANSITION_DENIED;
@@ -689,5 +739,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		checkAirspeed = true;
 	}
 
-	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
+	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status->circuit_breaker_engaged_gpsfailure_check, true);
 }

src/modules/commander/state_machine_helper_posix.cpp

@@ -0,0 +1,634 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper.cpp
+ * State machine helper functions implementations
+ *
+ * @author Thomas Gubler <thomas@px4.io>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_posix.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <string.h>
+#include <math.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/differential_pressure.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_device.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+#include "commander_helper.h"
+#include "PreflightCheck.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+// This array defines the arming state transitions. The rows are the new state, and the columns
+// are the current state. Using new state and current  state you can index into the array which
+// will be true for a valid transition or false for a invalid transition. In some cases even
+// though the transition is marked as true additional checks must be made. See arming_state_transition
+// code for those checks.
+static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
+	//                                                    INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
+	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       true,          false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
+	{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,        true,          false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
+	{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
+};
+
+// You can index into the array with an arming_state_t in order to get it's textual representation
+static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
+	"ARMING_STATE_INIT",
+	"ARMING_STATE_STANDBY",
+	"ARMING_STATE_ARMED",
+	"ARMING_STATE_ARMED_ERROR",
+	"ARMING_STATE_STANDBY_ERROR",
+	"ARMING_STATE_REBOOT",
+	"ARMING_STATE_IN_AIR_RESTORE",
+};
+
+transition_result_t
+arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
+			const struct safety_s   *safety,		///< current safety settings
+			arming_state_t          new_arming_state,	///< arming state requested
+			struct actuator_armed_s *armed,			///< current armed status
+			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
+{
+	// Double check that our static arrays are still valid
+	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
+	ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
+
+	transition_result_t ret = TRANSITION_DENIED;
+	arming_state_t current_arming_state = status->arming_state;
+	bool feedback_provided = false;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_arming_state == current_arming_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		/*
+		 * Get sensing state if necessary
+		 */
+		int prearm_ret = OK;
+
+		/* only perform the check if we have to */
+		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+			prearm_ret = prearm_check(status, mavlink_fd);
+		}
+
+		/*
+		 * Perform an atomic state update
+		 */
+		//irqstate_t flags = irqsave();
+
+		/* enforce lockdown in HIL */
+		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
+			armed->lockdown = true;
+
+		} else {
+			armed->lockdown = false;
+		}
+
+		// Check that we have a valid state transition
+		bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
+
+		if (valid_transition) {
+			// We have a good transition. Now perform any secondary validation.
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+
+				//      Do not perform pre-arm checks if coming from in air restore
+				//      Allow if vehicle_status_s::HIL_STATE_ON
+				if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
+					status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+
+					// Fail transition if pre-arm check fails
+					if (prearm_ret) {
+						/* the prearm check already prints the reject reason */
+						feedback_provided = true;
+						valid_transition = false;
+
+					// Fail transition if we need safety switch press
+					} else if (safety->safety_switch_available && !safety->safety_off) {
+
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						feedback_provided = true;
+						valid_transition = false;
+					}
+
+					// Perform power checks only if circuit breaker is not
+					// engaged for these checks
+					if (!status->circuit_breaker_engaged_power_check) {
+						// Fail transition if power is not good
+						if (!status->condition_power_input_valid) {
+
+							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							feedback_provided = true;
+							valid_transition = false;
+						}
+
+						// Fail transition if power levels on the avionics rail
+						// are measured but are insufficient
+						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
+							// Check avionics rail voltages
+							if (status->avionics_power_rail_voltage < 4.75f) {
+								mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+								valid_transition = false;
+							} else if (status->avionics_power_rail_voltage < 4.9f) {
+								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+							} else if (status->avionics_power_rail_voltage > 5.4f) {
+								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+							}
+						}
+					}
+
+				}
+
+			} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
+				new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
+			}
+		}
+
+		// HIL can always go to standby
+		if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+			warnx("HIL STATE ON -> STANDBY");
+			valid_transition = true;
+		}
+
+		/* Sensors need to be initialized for STANDBY state */
+		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			warnx("NOT ARMING: Sensors not operational.");
+			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+			feedback_provided = true;
+			valid_transition = false;
+			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
+		}
+
+		/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
+		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
+			new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+			if (status->condition_system_sensors_initialized) {
+				mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+			} else {
+				mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
+			}
+			feedback_provided = true;
+		}
+
+		// Finish up the state transition
+		if (valid_transition) {
+			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
+			armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
+			ret = TRANSITION_CHANGED;
+			status->arming_state = new_arming_state;
+		}
+
+		/* end of atomic state update */
+		//irqrestore(flags);
+	}
+
+	if (ret == TRANSITION_DENIED) {
+		const char * str = "INVAL: %s - %s";
+		/* only print to console here by default as this is too technical to be useful during operation */
+		warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
+
+		/* print to MAVLink if we didn't provide any feedback yet */
+		if (!feedback_provided) {
+			mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
+		}
+	}
+
+	return ret;
+}
+
+bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
+{
+	// System is safe if:
+	// 1) Not armed
+	// 2) Armed, but in software lockdown (HIL)
+	// 3) Safety switch is present AND engaged -> actuators locked
+	if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+transition_result_t
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* transition may be denied even if the same state is requested because conditions may have changed */
+	switch (new_main_state) {
+	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_ACRO:
+		ret = TRANSITION_CHANGED;
+		break;
+
+	case vehicle_status_s::MAIN_STATE_ALTCTL:
+		/* need at minimum altitude estimate */
+		/* TODO: add this for fixedwing as well */
+		if (!status->is_rotary_wing ||
+		    (status->condition_local_altitude_valid ||
+		     status->condition_global_position_valid)) {
+			ret = TRANSITION_CHANGED;
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_POSCTL:
+		/* need at minimum local position estimate */
+		if (status->condition_local_position_valid ||
+		    status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
+		/* need global position estimate */
+		if (status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
+	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
+		/* need global position and home position */
+		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_OFFBOARD:
+
+		/* need offboard signal */
+		if (!status->offboard_control_signal_lost) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
+	case vehicle_status_s::MAIN_STATE_MAX:
+	default:
+		break;
+	}
+	if (ret == TRANSITION_CHANGED) {
+		if (status->main_state != new_main_state) {
+			status->main_state = new_main_state;
+		} else {
+			ret = TRANSITION_NOT_CHANGED;
+		}
+	}
+
+	return ret;
+}
+
+/**
+ * Transition from one hil state to another
+ */
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	if (current_status->hil_state == new_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+		switch (new_state) {
+		case vehicle_status_s::HIL_STATE_OFF:
+			/* we're in HIL and unexpected things can happen if we disable HIL now */
+			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
+			ret = TRANSITION_DENIED;
+			break;
+
+		case vehicle_status_s::HIL_STATE_ON:
+			if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
+			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
+			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+
+				const char *devname;
+				unsigned int handle = 0;
+				for(;;) {
+					devname = px4_get_device_names(&handle);
+					if (devname == NULL)
+						break;
+
+					/* skip mavlink */
+					if (!strcmp("/dev/mavlink", devname)) {
+						continue;
+					}
+
+
+					int sensfd = px4_open(devname, 0);
+
+					if (sensfd < 0) {
+						warn("failed opening device %s", devname);
+						continue;
+					}
+
+					int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
+					px4_close(sensfd);
+
+						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
+				}
+				ret = TRANSITION_CHANGED;
+				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+			} else {
+				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+				ret = TRANSITION_DENIED;
+			}
+			break;
+
+		default:
+			warnx("Unknown HIL state");
+			break;
+		}
+	}
+
+	if (ret == TRANSITION_CHANGED) {
+		current_status->hil_state = new_state;
+		current_status->timestamp = hrt_absolute_time();
+		// XXX also set lockdown here
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+	}
+	return ret;
+}
+
+/**
+ * Check failsafe and main status and set navigation status for navigator accordingly
+ */
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
+		   const bool stay_in_failsafe)
+{
+	navigation_state_t nav_state_old = status->nav_state;
+
+	bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+	status->failsafe = false;
+
+	/* evaluate main state to decide in normal (non-failsafe) mode */
+	switch (status->main_state) {
+	case vehicle_status_s::MAIN_STATE_ACRO:
+	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_ALTCTL:
+	case vehicle_status_s::MAIN_STATE_POSCTL:
+		/* require RC for all manual modes */
+		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+
+		} else {
+			switch (status->main_state) {
+			case vehicle_status_s::MAIN_STATE_ACRO:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
+				break;
+
+			case vehicle_status_s::MAIN_STATE_MANUAL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+				break;
+
+			case vehicle_status_s::MAIN_STATE_ALTCTL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+				break;
+
+			case vehicle_status_s::MAIN_STATE_POSCTL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+				break;
+
+			default:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+				break;
+			}
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
+
+		/* go into failsafe
+		 * - if commanded to do so
+		 * - if we have an engine failure
+		 * - depending on datalink, RC and if the mission is finished */
+
+		/* first look at the commands */
+		if (status->engine_failure_cmd) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if (status->data_link_lost_cmd) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+		} else if (status->gps_failure_cmd) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+		} else if (status->rc_signal_lost_cmd) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
+
+		/* finished handling commands which have priority, now handle failures */
+		} else if (status->gps_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+		} else if (status->engine_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
+		/* datalink loss enabled:
+		 * check for datalink lost: this should always trigger RTGS */
+		} else if (data_link_loss_enabled && status->data_link_lost) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* datalink loss disabled:
+		 * check if both, RC and datalink are lost during the mission
+		 * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+						       (status->rc_signal_lost && mission_finished))) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
+		} else if (!stay_in_failsafe){
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
+		/* go into failsafe on a engine failure */
+		if (status->engine_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* go into failsafe if RC is lost and datalink loss is not set up */
+		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* don't bother if RC is lost if datalink is connected */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for loitering */
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+		} else {
+			/* everything is perfect */
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
+		/* require global position and home, also go into failsafe on an engine failure */
+
+		if (status->engine_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if ((!status->condition_global_position_valid ||
+					!status->condition_home_position_valid)) {
+			status->failsafe = true;
+
+			if (status->condition_local_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+		} else {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+		}
+		break;
+
+	case vehicle_status_s::MAIN_STATE_OFFBOARD:
+		/* require offboard control, otherwise stay where you are */
+		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
+			status->failsafe = true;
+
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
+			status->failsafe = true;
+
+			if (status->condition_local_position_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+		} else {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
+		}
+	default:
+		break;
+	}
+
+	return status->nav_state != nav_state_old;
+}
+
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+{
+	/* at this point this equals the preflight check, but might add additional
+	 * quantities later.
+	 */
+	bool checkAirspeed = false;
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
+		checkAirspeed = true;
+	}
+
+	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
+}

src/modules/dataman/dataman.c

@@ -40,7 +40,7 @@
  * @author Thomas Gubler
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <fcntl.h>
@@ -48,6 +48,8 @@
 #include <systemlib/err.h>
 #include <queue.h>
 #include <string.h>
+#include <semaphore.h>
+#include <unistd.h>
 
 #include "dataman.h"
 #include <systemlib/param/param.h>
@@ -129,6 +131,7 @@ static sem_t g_sys_state_mutex;
 
 /* The data manager store file handle and file name */
 static int g_fd = -1, g_task_fd = -1;
+// FIXME - need a configurable path that is not OS specific
 static const char *k_data_manager_device_path = "/fs/microsd/dataman";
 
 /* The data manager work queues */
@@ -668,7 +671,7 @@ task_main(int argc, char *argv[])
 	}
 
 	/* Open or create the data manager file */
-	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
+	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, 0x0777);
 
 	if (g_task_fd < 0) {
 		warnx("Could not open data manager file %s", k_data_manager_device_path);
@@ -794,7 +797,7 @@ start(void)
 	sem_init(&g_init_sema, 1, 0);
 
 	/* start the worker thread */
-	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) {
+	if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}
@@ -828,31 +831,40 @@ stop(void)
 static void
 usage(void)
 {
-	errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
+	warnx("usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
 }
 
 int
 dataman_main(int argc, char *argv[])
 {
-	if (argc < 2)
+	if (argc < 2) {
 		usage();
+		return -1;
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (g_fd >= 0)
-			errx(1, "already running");
+		if (g_fd >= 0) {
+			warnx("dataman already running");
+			return -1;
+		}
 
 		start();
 
-		if (g_fd < 0)
-			errx(1, "start failed");
+		if (g_fd < 0) {
+			warnx("dataman start failed");
+			return -1;
+		}
 
-		exit(0);
+		return 0;
 	}
 
 	/* Worker thread should be running for all other commands */
-	if (g_fd < 0)
-		errx(1, "not running");
+	if (g_fd < 0) {
+		warnx("dataman worker thread not running");
+		usage();
+		return -1;
+	}
 
 	if (!strcmp(argv[1], "stop"))
 		stop();
@@ -862,8 +874,10 @@ dataman_main(int argc, char *argv[])
 		dm_restart(DM_INIT_REASON_POWER_ON);
 	else if (!strcmp(argv[1], "inflightrestart"))
 		dm_restart(DM_INIT_REASON_IN_FLIGHT);
-	else
+	else {
 		usage();
+		return -1;
+	}
 
-	exit(1);
+	return 1;
 }

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -43,7 +43,11 @@
 #include "AttitudePositionEstimatorEKF.h"
 #include "estimator_22states.h"
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_time.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -228,10 +232,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	for (unsigned s = 0; s < 3; s++) {
 		char str[30];
 		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-		fd = open(str, O_RDONLY);
+		fd = px4_open(str, O_RDONLY);
 
 		if (fd >= 0) {
-			res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]);
+			res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]);
 			close(fd);
 
 			if (res) {
@@ -240,11 +244,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 		}
 
 		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
-		fd = open(str, O_RDONLY);
+		fd = px4_open(str, O_RDONLY);
 
 		if (fd >= 0) {
-			res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]);
-			close(fd);
+			res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]);
+			px4_close(fd);
 
 			if (res) {
 				warnx("A%u SCALE FAIL", s);
@@ -252,11 +256,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 		}
 
 		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
-		fd = open(str, O_RDONLY);
+		fd = px4_open(str, O_RDONLY);
 
 		if (fd >= 0) {
-			res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]);
-			close(fd);
+			res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]);
+			px4_close(fd);
 
 			if (res) {
 				warnx("M%u SCALE FAIL", s);
@@ -281,7 +285,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_estimator_task);
+				px4_task_delete(_estimator_task);
 				break;
 			}
 		} while (_estimator_task != -1);
@@ -490,7 +494,8 @@ void AttitudePositionEstimatorEKF::task_main()
 	_filter_start_time = hrt_absolute_time();
 
 	if (!_ekf) {
-		errx(1, "OUT OF MEM!");
+		warnx("OUT OF MEM!");
+		return;
 	}
 
 	/*
@@ -517,7 +522,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	parameters_update();
 
 	/* wakeup source(s) */
-	struct pollfd fds[2];
+	px4_pollfd_struct_t fds[2];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
@@ -535,7 +540,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0) {
@@ -696,10 +701,8 @@ void AttitudePositionEstimatorEKF::task_main()
 
 	_task_running = false;
 
-	warnx("exiting.\n");
-
 	_estimator_task = -1;
-	_exit(0);
+	return;
 }
 
 void AttitudePositionEstimatorEKF::initializeGPS()
@@ -1035,11 +1038,11 @@ int AttitudePositionEstimatorEKF::start()
 	ASSERT(_estimator_task == -1);
 
 	/* start the task */
-	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
+	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
 					 7500,
-					 (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
+					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 
 	if (_estimator_task < 0) {
@@ -1155,7 +1158,7 @@ void AttitudePositionEstimatorEKF::pollData()
 	float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
 
 	/* guard against too large deltaT's */
-	if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
 		deltaT = 0.01f;
 	}
 
@@ -1167,9 +1170,9 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	int last_gyro_main = _gyro_main;
 
-	if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
-	    isfinite(_sensor_combined.gyro_rad_s[1]) &&
-	    isfinite(_sensor_combined.gyro_rad_s[2]) &&
+	if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
+	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
+	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
 	    (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
 
 		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
@@ -1178,9 +1181,9 @@ void AttitudePositionEstimatorEKF::pollData()
 		_gyro_main = 0;
 		_gyro_valid = true;
 
-	} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
-		   isfinite(_sensor_combined.gyro1_rad_s[1]) &&
-		   isfinite(_sensor_combined.gyro1_rad_s[2])) {
+	} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
+		   PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
+		   PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
 
 		_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
 		_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
@@ -1528,25 +1531,29 @@ int AttitudePositionEstimatorEKF::trip_nan()
 int ekf_att_pos_estimator_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}");
+		warnx("usage: ekf_att_pos_estimator {start|stop|status|logging}");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (estimator::g_estimator != nullptr) {
-			errx(1, "already running");
+			warnx("already running");
+			return 1;
 		}
 
 		estimator::g_estimator = new AttitudePositionEstimatorEKF();
 
 		if (estimator::g_estimator == nullptr) {
-			errx(1, "alloc failed");
+			warnx("alloc failed");
+			return 1;
 		}
 
 		if (OK != estimator::g_estimator->start()) {
 			delete estimator::g_estimator;
 			estimator::g_estimator = nullptr;
-			err(1, "start failed");
+			warnx("start failed");
+			return 1;
 		}
 
 		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
@@ -1558,64 +1565,46 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 
 		printf("\n");
 
-		exit(0);
+		return 0;
+	}
+
+	if (estimator::g_estimator == nullptr) {
+		warnx("not running");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (estimator::g_estimator == nullptr) {
-			errx(1, "not running");
-		}
 
 		delete estimator::g_estimator;
 		estimator::g_estimator = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
-		if (estimator::g_estimator) {
-			warnx("running");
+		warnx("running");
 
-			estimator::g_estimator->print_status();
+		estimator::g_estimator->print_status();
 
-			exit(0);
-
-		} else {
-			errx(1, "not running");
-		}
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "trip")) {
-		if (estimator::g_estimator) {
-			int ret = estimator::g_estimator->trip_nan();
+		int ret = estimator::g_estimator->trip_nan();
 
-			exit(ret);
-
-		} else {
-			errx(1, "not running");
-		}
+		return ret;
 	}
 
 	if (!strcmp(argv[1], "logging")) {
-		if (estimator::g_estimator) {
-			int ret = estimator::g_estimator->enable_logging(true);
+		int ret = estimator::g_estimator->enable_logging(true);
 
-			exit(ret);
-
-		} else {
-			errx(1, "not running");
-		}
+		return ret;
 	}
 
 	if (!strcmp(argv[1], "debug")) {
-		if (estimator::g_estimator) {
-			int debug = strtoul(argv[2], NULL, 10);
-			int ret = estimator::g_estimator->set_debuglevel(debug);
+		int debug = strtoul(argv[2], NULL, 10);
+		int ret = estimator::g_estimator->set_debuglevel(debug);
 
-			exit(ret);
-
-		} else {
-			errx(1, "not running");
-		}
+		return ret;
 	}
 
 	warnx("unrecognized command");

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c

@@ -39,7 +39,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 

src/modules/ekf_att_pos_estimator/estimator_22states.cpp

@@ -37,6 +37,7 @@
  * @author Lorenz Meier <lorenz@px4.io>
  */
 
+#include <px4_defines.h>
 #include "estimator_22states.h"
 #include <string.h>
 #include <stdio.h>
@@ -48,10 +49,6 @@
 #define M_PI_F static_cast<float>(M_PI)
 #endif
 
-#ifndef isfinite
-#define isfinite(__x) std::isfinite(__x)
-#endif
-
 constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
 
 AttPosEKF::AttPosEKF() :
@@ -2392,9 +2389,9 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
     if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
     {
         for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) {
-            if (isfinite(storedStates[i][bestStoreIndex])) {
+            if (PX4_ISFINITE(storedStates[i][bestStoreIndex])) {
                 statesForFusion[i] = storedStates[i][bestStoreIndex];
-            } else if (isfinite(states[i])) {
+            } else if (PX4_ISFINITE(states[i])) {
                 statesForFusion[i] = states[i];
             } else {
                 // There is not much we can do here, except reporting the error we just
@@ -2406,7 +2403,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
     else // otherwise output current state
     {
         for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
-            if (isfinite(states[i])) {
+            if (PX4_ISFINITE(states[i])) {
                 statesForFusion[i] = states[i];
             } else {
                 ret++;
@@ -2630,7 +2627,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
         ret = val;
     }
 
-    if (!isfinite(val)) {
+    if (!PX4_ISFINITE(val)) {
         ekf_debug("constrain: non-finite!");
     }
 
@@ -2710,7 +2707,7 @@ void AttPosEKF::ConstrainStates()
     // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
     // Constrain dtIMUfilt
-    if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
+    if (!PX4_ISFINITE(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
         dtIMUfilt = dtIMU;
     }
 
@@ -2922,21 +2919,21 @@ bool AttPosEKF::StatesNaN() {
     bool err = false;
 
     // check all integrators
-    if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
+    if (!PX4_ISFINITE(summedDelAng.x) || !PX4_ISFINITE(summedDelAng.y) || !PX4_ISFINITE(summedDelAng.z)) {
         current_ekf_state.angNaN = true;
         ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
         err = true;
         goto out;
     } // delta angles
 
-    if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
+    if (!PX4_ISFINITE(correctedDelAng.x) || !PX4_ISFINITE(correctedDelAng.y) || !PX4_ISFINITE(correctedDelAng.z)) {
         current_ekf_state.angNaN = true;
         ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
         err = true;
         goto out;
     } // delta angles
 
-    if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
+    if (!PX4_ISFINITE(summedDelVel.x) || !PX4_ISFINITE(summedDelVel.y) || !PX4_ISFINITE(summedDelVel.z)) {
         current_ekf_state.summedDelVelNaN = true;
         ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
         err = true;
@@ -2946,7 +2943,7 @@ bool AttPosEKF::StatesNaN() {
     // check all states and covariance matrices
     for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
         for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
-            if (!isfinite(KH[i][j])) {
+            if (!PX4_ISFINITE(KH[i][j])) {
 
                 current_ekf_state.KHNaN = true;
                 err = true;
@@ -2954,7 +2951,7 @@ bool AttPosEKF::StatesNaN() {
                 goto out;
             } //  intermediate result used for covariance updates
 
-            if (!isfinite(KHP[i][j])) {
+            if (!PX4_ISFINITE(KHP[i][j])) {
 
                 current_ekf_state.KHPNaN = true;
                 err = true;
@@ -2962,7 +2959,7 @@ bool AttPosEKF::StatesNaN() {
                 goto out;
             } // intermediate result used for covariance updates
 
-            if (!isfinite(P[i][j])) {
+            if (!PX4_ISFINITE(P[i][j])) {
 
                 current_ekf_state.covarianceNaN = true;
                 err = true;
@@ -2970,7 +2967,7 @@ bool AttPosEKF::StatesNaN() {
             } // covariance matrix
         }
 
-        if (!isfinite(Kfusion[i])) {
+        if (!PX4_ISFINITE(Kfusion[i])) {
 
             current_ekf_state.kalmanGainsNaN = true;
             ekf_debug("Kfusion NaN");
@@ -2978,7 +2975,7 @@ bool AttPosEKF::StatesNaN() {
             goto out;
         } // Kalman gains
 
-        if (!isfinite(states[i])) {
+        if (!PX4_ISFINITE(states[i])) {
 
             current_ekf_state.statesNaN = true;
             ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);

src/modules/fixedwing_backside/fixedwing_backside_main.cpp

@@ -39,7 +39,7 @@
  * Fixedwing backside controller using control library
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -111,7 +111,7 @@ int fixedwing_backside_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 
-		deamon_task = task_spawn_cmd("fixedwing_backside",
+		deamon_task = px4_task_spawn_cmd("fixedwing_backside",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -41,7 +41,7 @@
  *
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -1117,7 +1117,7 @@ FixedwingAttitudeControl::start()
 	ASSERT(_control_task == -1);
 
 	/* start the task */
-	_control_task = task_spawn_cmd("fw_att_control",
+	_control_task = px4_task_spawn_cmd("fw_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       1600,

src/modules/fw_att_control/fw_att_control_params.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
-f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2013-2015 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
@@ -37,11 +36,11 @@ f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Parameters defined by the fixed-wing attitude control task
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 
@@ -73,6 +72,8 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
  * This defines how much the elevator input will be commanded depending on the
  * current body angular rate error.
  *
+ * @min 0.005
+ * @max 1.0
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
@@ -144,6 +145,8 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f);
  * This defines how much the aileron input will be commanded depending on the
  * current body angular rate error.
  *
+ * @min 0.005
+ * @max 1.0
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
@@ -190,6 +193,8 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
  * This defines how much the rudder input will be commanded depending on the
  * current body angular rate error.
  *
+ * @min 0.005
+ * @max 1.0
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
@@ -234,7 +239,9 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
 /**
  * Roll rate feed forward
  *
- * Direct feed forward from rate setpoint to control surface output
+ * Direct feed forward from rate setpoint to control surface output. Use this
+ * to obtain a tigher response of the controller without introducing
+ * noise amplification.
  *
  * @min 0.0
  * @max 10.0

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -51,7 +51,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -1621,7 +1621,7 @@ FixedwingPositionControl::start()
 	ASSERT(_control_task == -1);
 
 	/* start the task */
-	_control_task = task_spawn_cmd("fw_pos_control_l1",
+	_control_task = px4_task_spawn_cmd("fw_pos_control_l1",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       1600,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -39,7 +39,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <systemlib/param/param.h>
 
 /*

src/modules/fw_pos_control_l1/landingslope.cpp

@@ -39,7 +39,7 @@
 
 #include "landingslope.h"
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <stdlib.h>
 #include <errno.h>
 #include <math.h>

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -39,7 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <systemlib/param/param.h>
 
 /*

src/modules/land_detector/land_detector_main.cpp

@@ -136,7 +136,7 @@ static int land_detector_start(const char *mode)
 	}
 
 	//Start new thread task
-	_landDetectorTaskID = task_spawn_cmd("land_detector",
+	_landDetectorTaskID = px4_task_spawn_cmd("land_detector",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_DEFAULT,
 					     1000,

src/modules/mavlink/mavlink.c

@@ -38,7 +38,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdbool.h>

src/modules/mavlink/mavlink_ftp.cpp

@@ -371,7 +371,11 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 
 		// Determine the directory entry type
 		switch (entry.d_type) {
+#ifdef __PX4_NUTTX
 		case DTYPE_FILE:
+#else
+		case DT_REG:
+#endif
 			// For files we get the file size as well
 			direntType = kDirentFile;
 			snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
@@ -380,9 +384,12 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 				fileSize = st.st_size;
 			}
 			break;
+#ifdef __PX4_NUTTX
 		case DTYPE_DIRECTORY:
-			// XXX @DonLakeFlyer: Remove the first condition for the test setup
-			if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
+#else
+		case DT_DIR:
+#endif
+			if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
 				// Don't bother sending these back
 				direntType = kDirentSkip;
 			} else {
@@ -791,7 +798,12 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
 		return -1;
 	}
 
-	dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
+	dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY
+// POSIX requires the permissions to be supplied if O_CREAT passed
+#ifdef __PX4_POSIX
+			, 0x0777
+#endif
+			);
 	if (dst_fd < 0) {
 		op_errno = errno;
 		::close(src_fd);

src/modules/mavlink/mavlink_main.cpp

@@ -39,7 +39,8 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_getopt.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -50,7 +51,9 @@
 #include <assert.h>
 #include <math.h>
 #include <poll.h>
+#ifndef __PX4_POSIX
 #include <termios.h>
+#endif
 #include <time.h>
 #include <math.h> /* isinf / isnan checks */
 
@@ -68,7 +71,7 @@
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
 #include <dataman/dataman.h>
-#include <mathlib/mathlib.h>
+//#include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
 
 #include <uORB/topics/parameter_update.h>
@@ -96,8 +99,10 @@ static const int ERROR = -1;
 
 static Mavlink *_mavlink_instances = nullptr;
 
+#ifdef __PX4_NUTTX
 /* TODO: if this is a class member it crashes */
 static struct file_operations fops;
+#endif
 
 static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
 static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
@@ -114,6 +119,9 @@ extern mavlink_system_t mavlink_system;
 static void usage(void);
 
 Mavlink::Mavlink() :
+#ifndef __PX4_NUTTX
+	VDev("mavlink-log", MAVLINK_LOG_DEVICE),
+#endif
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
 	next(nullptr),
@@ -141,7 +149,9 @@ Mavlink::Mavlink() :
 	_forwarding_on(false),
 	_passing_on(false),
 	_ftp_on(false),
+#ifndef __PX4_POSIX
 	_uart_fd(-1),
+#endif
 	_baudrate(57600),
 	_datarate(1000),
 	_datarate_events(500),
@@ -176,7 +186,9 @@ Mavlink::Mavlink() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
 	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
+#ifdef __PX4_NUTTX
 	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
+#endif
 
 	_instance_id = Mavlink::instance_count();
 
@@ -217,7 +229,8 @@ Mavlink::Mavlink() :
 #endif
 
 	default:
-		errx(1, "instance ID is out of range");
+		warnx("instance ID is out of range");
+		px4_task_exit(1);
 		break;
 	}
 
@@ -401,6 +414,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	}
 }
 
+#ifndef __PX4_POSIX
 int
 Mavlink::get_uart_fd(unsigned index)
 {
@@ -418,6 +432,7 @@ Mavlink::get_uart_fd()
 {
 	return _uart_fd;
 }
+#endif // __PX4_POSIX
 
 int
 Mavlink::get_instance_id()
@@ -436,7 +451,11 @@ Mavlink::get_channel()
  ****************************************************************************/
 
 int
-Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
+#ifdef __PX4_NUTTX
+Mavlink::mavlink_dev_ioctl(struct file *filp, int cmd, unsigned long arg)
+#else
+Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+#endif
 {
 	switch (cmd) {
 	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
@@ -555,6 +574,7 @@ int Mavlink::get_component_id()
 	return mavlink_system.compid;
 }
 
+#ifndef __PX4_POSIX
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
 {
 	/* process baud rate */
@@ -610,7 +630,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	}
 
 	/* open uart */
-	_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
+	_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
 
 	if (_uart_fd < 0) {
 		return _uart_fd;
@@ -625,7 +645,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
 		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
-		close(_uart_fd);
+		::close(_uart_fd);
 		return -1;
 	}
 
@@ -641,7 +661,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		/* Set baud rate */
 		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
 			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
-			close(_uart_fd);
+			::close(_uart_fd);
 			return -1;
 		}
 
@@ -649,7 +669,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
 		warnx("ERR SET CONF %s\n", uart_name);
-		close(_uart_fd);
+		::close(_uart_fd);
 		return -1;
 	}
 
@@ -659,7 +679,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		 * which is not an issue, but requires a separate call so we can fail silently.
 		 */
 		(void)tcgetattr(_uart_fd, &uart_config);
+#ifdef CRTS_IFLOW
 		uart_config.c_cflag |= CRTS_IFLOW;
+#else
+		uart_config.c_cflag |= CRTSCTS;
+#endif
 		(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
 
 		/* setup output flow control */
@@ -702,6 +726,8 @@ Mavlink::enable_flow_control(bool enabled)
 	return ret;
 }
 
+#endif
+
 int
 Mavlink::set_hil_enabled(bool hil_enabled)
 {
@@ -733,7 +759,13 @@ Mavlink::get_free_tx_buf()
 	 * flow control if it continues to be full
 	 */
 	int buf_free = 0;
+
+#ifndef __PX4_POSIX
+
+// No FIONWRITE on Linux
+#if !defined(__PX4_LINUX)
 	(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+#endif
 
 	if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
 		/* Disable hardware flow control:
@@ -747,6 +779,7 @@ Mavlink::get_free_tx_buf()
 			enable_flow_control(false);
 		}
 	}
+#endif
 
 	return buf_free;
 }
@@ -801,8 +834,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
+#ifndef __PX4_POSIX
 	/* send message to UART */
-	ssize_t ret = write(_uart_fd, buf, packet_len);
+	ssize_t ret = ::write(_uart_fd, buf, packet_len);
 
 	if (ret != (int) packet_len) {
 		count_txerr();
@@ -812,6 +846,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
+#endif
 
 	pthread_mutex_unlock(&_send_mutex);
 }
@@ -851,8 +886,9 @@ Mavlink::resend_message(mavlink_message_t *msg)
 	buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
 
+#ifndef __PX4_POSIX
 	/* send message to UART */
-	ssize_t ret = write(_uart_fd, buf, packet_len);
+	ssize_t ret = ::write(_uart_fd, buf, packet_len);
 
 	if (ret != (int) packet_len) {
 		count_txerr();
@@ -862,6 +898,7 @@ Mavlink::resend_message(mavlink_message_t *msg)
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
+#endif
 
 	pthread_mutex_unlock(&_send_mutex);
 }
@@ -1204,38 +1241,42 @@ Mavlink::task_main(int argc, char *argv[])
 	_datarate = 0;
 	_mode = MAVLINK_MODE_NORMAL;
 
+#ifdef __PX4_NUTTX
 	/* work around some stupidity in task_create's argv handling */
 	argc -= 2;
 	argv += 2;
+#endif
 
 	/* don't exit from getopt loop to leave getopt global variables in consistent state,
 	 * set error flag instead */
 	bool err_flag = false;
+	int myoptind=1;
+	const char *myoptarg = NULL;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
-			_baudrate = strtoul(optarg, NULL, 10);
+			_baudrate = strtoul(myoptarg, NULL, 10);
 
 			if (_baudrate < 9600 || _baudrate > 921600) {
-				warnx("invalid baud rate '%s'", optarg);
+				warnx("invalid baud rate '%s'", myoptarg);
 				err_flag = true;
 			}
 
 			break;
 
 		case 'r':
-			_datarate = strtoul(optarg, NULL, 10);
+			_datarate = strtoul(myoptarg, NULL, 10);
 
 			if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
-				warnx("invalid data rate '%s'", optarg);
+				warnx("invalid data rate '%s'", myoptarg);
 				err_flag = true;
 			}
 
 			break;
 
 		case 'd':
-			_device_name = optarg;
+			_device_name = myoptarg;
 			break;
 
 //		case 'e':
@@ -1243,13 +1284,13 @@ Mavlink::task_main(int argc, char *argv[])
 //			break;
 
 		case 'm':
-			if (strcmp(optarg, "custom") == 0) {
+			if (strcmp(myoptarg, "custom") == 0) {
 				_mode = MAVLINK_MODE_CUSTOM;
 
-			} else if (strcmp(optarg, "camera") == 0) {
+			} else if (strcmp(myoptarg, "camera") == 0) {
 				// left in here for compatibility
 				_mode = MAVLINK_MODE_ONBOARD;
-			} else if (strcmp(optarg, "onboard") == 0) {
+			} else if (strcmp(myoptarg, "onboard") == 0) {
 				_mode = MAVLINK_MODE_ONBOARD;
 			}
 
@@ -1305,6 +1346,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
+#ifndef __PX4_POSIX
 	struct termios uart_config_original;
 
 	/* default values for arguments */
@@ -1314,6 +1356,7 @@ Mavlink::task_main(int argc, char *argv[])
 		warn("could not open %s", _device_name);
 		return ERROR;
 	}
+#endif
 
 	/* initialize send mutex */
 	pthread_mutex_init(&_send_mutex, NULL);
@@ -1328,7 +1371,8 @@ Mavlink::task_main(int argc, char *argv[])
 		 * marker ring buffer approach.
 		 */
 		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
-			errx(1, "msg buf:");
+			warnx("msg buf:");
+			return 1;
 		}
 
 		/* initialize message buffer mutex */
@@ -1336,10 +1380,14 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	/* create the device node that's used for sending text log messages, etc. */
+#ifdef __PX4_NUTTX
 	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
+#else
+	register_driver(MAVLINK_LOG_DEVICE, NULL);
+#endif
 
 	/* initialize logging device */
-	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
 
 	/* Initialize system properties */
 	mavlink_update_system();
@@ -1570,14 +1618,16 @@ Mavlink::task_main(int argc, char *argv[])
 	/* wait for threads to complete */
 	pthread_join(_receive_thread, NULL);
 
+#ifndef __PX4_POSIX
 	/* reset the UART flags to original state */
 	tcsetattr(_uart_fd, TCSANOW, &uart_config_original);
 
 	/* close UART */
-	close(_uart_fd);
+	::close(_uart_fd);
+#endif
 
 	/* close mavlink logging device */
-	close(_mavlink_fd);
+	px4_close(_mavlink_fd);
 
 	if (_passing_on || _ftp_on) {
 		message_buffer_destroy();
@@ -1632,11 +1682,11 @@ Mavlink::start(int argc, char *argv[])
 	// between the starting task and the spawned
 	// task - start_helper() only returns
 	// when the started task exits.
-	task_spawn_cmd(buf,
+	px4_task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
 		       2400,
-		       (main_t)&Mavlink::start_helper,
+		       (px4_main_t)&Mavlink::start_helper,
 		       (char * const *)argv);
 
 	// Ensure that this shell command
@@ -1667,7 +1717,7 @@ Mavlink::display_status()
 {
 
 	if (_rstatus.heartbeat_time > 0) {
-		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
+		printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time));
 	}
 
 	printf("\tmavlink chan: #%u\n", _channel);
@@ -1757,11 +1807,13 @@ Mavlink::stream_command(int argc, char *argv[])
 
 			// If the link is not running we should complain, but not fall over
 			// because this is so easy to get wrong and not fatal. Warning is sufficient.
-			errx(0, "mavlink for device %s is not running", device_name);
+			warnx("mavlink for device %s is not running", device_name);
+			return 0;
 		}
 
 	} else {
-		errx(1, "usage: mavlink stream [-d device] -s stream -r rate");
+		warnx("usage: mavlink stream [-d device] -s stream -r rate");
+		return 1;
 	}
 
 	return OK;
@@ -1776,7 +1828,7 @@ int mavlink_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		usage();
-		exit(1);
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
@@ -1785,7 +1837,7 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stop")) {
 		warnx("mavlink stop is deprecated, use stop-all instead");
 		usage();
-		exit(1);
+		return 1;
 
 	} else if (!strcmp(argv[1], "stop-all")) {
 		return Mavlink::destroy_all_instances();
@@ -1798,7 +1850,7 @@ int mavlink_main(int argc, char *argv[])
 
 	} else {
 		usage();
-		exit(1);
+		return 1;
 	}
 
 	return 0;

src/modules/mavlink/mavlink_main.h

@@ -42,7 +42,11 @@
 #pragma once
 
 #include <stdbool.h>
+#ifdef __PX4_NUTTX
 #include <nuttx/fs/fs.h>
+#else
+#include <drivers/device/device.h>
+#endif
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 #include <pthread.h>
@@ -61,7 +65,11 @@
 #include "mavlink_parameters.h"
 #include "mavlink_ftp.h"
 
+#ifdef __PX4_NUTTX
 class Mavlink
+#else
+class Mavlink : public device::VDev
+#endif
 {
 
 public:
@@ -105,9 +113,11 @@ public:
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
 
+#ifndef __PX4_QURT
 	static int		get_uart_fd(unsigned index);
 
 	int			get_uart_fd();
+#endif
 
 	/**
 	 * Get the MAVLink system id.
@@ -183,12 +193,14 @@ public:
 
 	int			get_instance_id();
 
+#ifndef __PX4_QURT
 	/**
 	 * Enable / disable hardware flow control.
 	 *
 	 * @param enabled	True if hardware flow control should be enabled
 	 */
 	int			enable_flow_control(bool enabled);
+#endif
 
 	mavlink_channel_t	get_channel();
 
@@ -314,7 +326,9 @@ private:
 	bool			_forwarding_on;
 	bool			_passing_on;
 	bool			_ftp_on;
+#ifndef __PX4_QURT
 	int			_uart_fd;
+#endif
 	int			_baudrate;
 	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
 	int			_datarate_events;	///< data rate for params, waypoints, text messages
@@ -372,7 +386,9 @@ private:
 
 	void			mavlink_update_system();
 
+#ifndef __PX4_QURT
 	int			mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
+#endif
 
 	static unsigned int	interval_from_rate(float rate);
 
@@ -404,7 +420,11 @@ private:
 	 */
 	void update_rate_mult();
 
+#ifdef __PX4_NUTTX
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
+#else
+	virtual int	ioctl(device::file_t *filp, int cmd, unsigned long arg);
+#endif
 
 	/**
 	 * Main mavlink task.

src/modules/mavlink/mavlink_messages.cpp

@@ -39,6 +39,7 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
+#include <px4_time.h>
 #include <stdio.h>
 
 #include <commander/px4_custom_mode.h>
@@ -354,6 +355,7 @@ protected:
 		}
 	}
 
+#ifndef __PX4_QURT
 	void send(const hrt_abstime t)
 	{
 		if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
@@ -391,7 +393,7 @@ protected:
 						char log_file_path[64] = "";
 
 						timespec ts;
-						clock_gettime(CLOCK_REALTIME, &ts);
+						px4_clock_gettime(CLOCK_REALTIME, &ts);
 						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
 						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
 						struct tm tt;
@@ -410,6 +412,7 @@ protected:
 			}
 		}
 	}
+#endif
 };
 
 class MavlinkStreamCommandLong : public MavlinkStream
@@ -997,7 +1000,7 @@ protected:
 		mavlink_system_time_t msg;
 		timespec tv;
 
-		clock_gettime(CLOCK_REALTIME, &tv);
+		px4_clock_gettime(CLOCK_REALTIME, &tv);
 
 		msg.time_boot_ms = hrt_absolute_time() / 1000;
 		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -86,8 +86,10 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 	}
 
 	if (orb_copy(_topic, _fd, data)) {
-		/* error copying topic data */
-		memset(data, 0, _topic->o_size);
+		if (data) {
+			/* error copying topic data */
+			memset(data, 0, _topic->o_size);
+		}
 		return false;
 
 	} else {

src/modules/mavlink/mavlink_receiver.cpp

@@ -41,7 +41,8 @@
  */
 
 /* XXX trim includes */
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_time.h>
 #include <unistd.h>
 #include <pthread.h>
 #include <stdio.h>
@@ -59,9 +60,10 @@
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
-#include <nuttx/sched.h>
+#ifndef __PX4_POSIX
 #include <sys/prctl.h>
 #include <termios.h>
+#endif
 #include <errno.h>
 #include <stdlib.h>
 #include <poll.h>
@@ -989,16 +991,16 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
 	mavlink_msg_system_time_decode(msg, &time);
 
 	timespec tv;
-	clock_gettime(CLOCK_REALTIME, &tv);
+	px4_clock_gettime(CLOCK_REALTIME, &tv);
 
 	// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
-	bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS;
+	bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS;
 	bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
 
 	if (!onb_unix_valid && ofb_unix_valid) {
 		tv.tv_sec = time.time_unix_usec / 1000000ULL;
 		tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
-		if(clock_settime(CLOCK_REALTIME, &tv)) {
+		if(px4_clock_settime(CLOCK_REALTIME, &tv)) {
 			warn("failed setting clock");
 		}
 		else {
@@ -1484,6 +1486,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 void *
 MavlinkReceiver::receive_thread(void *arg)
 {
+#ifndef __PX4_POSIX
 	int uart_fd = _mavlink->get_uart_fd();
 
 	const int timeout = 500;
@@ -1506,7 +1509,7 @@ MavlinkReceiver::receive_thread(void *arg)
 		if (poll(fds, 1, timeout) > 0) {
 
 			/* non-blocking read. read may return negative values */
-			if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
+			if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
 				/* to avoid reading very small chunks wait for data before reading */
 				usleep(1000);
 			}
@@ -1526,6 +1529,7 @@ MavlinkReceiver::receive_thread(void *arg)
 			_mavlink->count_rxbytes(nread);
 		}
 	}
+#endif
 
 	return NULL;
 }
@@ -1572,9 +1576,11 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
+#ifndef __PX4_POSIX
 	// set to non-blocking read
 	int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
 	fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
+#endif
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);

src/modules/mavlink/mavlink_stream.cpp

@@ -79,7 +79,9 @@ MavlinkStream::update(const hrt_abstime t)
 
 	if (dt > 0 && dt >= interval) {
 		/* interval expired, send message */
+#ifndef __PX4_QURT
 		send(t);
+#endif
 
 		if (const_rate()) {
 			_last_sent = (t / _interval) * _interval;

src/modules/mavlink/mavlink_stream.h

@@ -90,7 +90,9 @@ protected:
 	Mavlink     *_mavlink;
 	unsigned int _interval;
 
+#ifndef __PX4_QURT
 	virtual void send(const hrt_abstime t) = 0;
+#endif
 
 private:
 	hrt_abstime _last_sent;

src/modules/mavlink/module.mk

@@ -35,17 +35,18 @@
 # MAVLink protocol to uORB interface process
 #
 
-MODULE_COMMAND	 = mavlink
-SRCS		 += mavlink_main.cpp \
-			mavlink.c \
-			mavlink_receiver.cpp \
+MODULE_COMMAND  = mavlink
+
+SRCS 		 +=	mavlink.c \
+		  	mavlink_main.cpp \
 			mavlink_mission.cpp \
 			mavlink_parameters.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_ftp.cpp
+			mavlink_receiver.cpp \
+			mavlink_ftp.cpp 
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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
@@ -40,7 +40,7 @@
  * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton.babushkin@me.com>
  *
  * The controller has two loops: P loop for angular error and PD loop for angular rate error.
@@ -53,7 +53,10 @@
  * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -404,7 +407,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_control_task);
+				px4_task_delete(_control_task);
 				break;
 			}
 		} while (_control_task != -1);
@@ -721,7 +724,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
 
-				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
 				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
 					_rates_int(i) = rate_i;
 				}
@@ -757,7 +760,7 @@ MulticopterAttitudeControl::task_main()
 	parameters_update();
 
 	/* wakeup source: vehicle attitude */
-	struct pollfd fds[1];
+	px4_pollfd_struct_t fds[1];
 
 	fds[0].fd = _v_att_sub;
 	fds[0].events = POLLIN;
@@ -765,7 +768,7 @@ MulticopterAttitudeControl::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit */
 		if (pret == 0)
@@ -858,10 +861,10 @@ MulticopterAttitudeControl::task_main()
 				control_attitude_rates(dt);
 
 				/* publish actuator controls */
-				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+				_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
+				_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
+				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
+				_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
 				_actuators.timestamp = hrt_absolute_time();
 				_actuators.timestamp_sample = _v_att.timestamp;
 
@@ -893,10 +896,8 @@ MulticopterAttitudeControl::task_main()
 		perf_end(_loop_perf);
 	}
 
-	warnx("exit");
-
 	_control_task = -1;
-	_exit(0);
+	return;
 }
 
 int
@@ -905,11 +906,11 @@ MulticopterAttitudeControl::start()
 	ASSERT(_control_task == -1);
 
 	/* start the task */
-	_control_task = task_spawn_cmd("mc_att_control",
+	_control_task = px4_task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       1500,
-				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
+				       (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
 				       nullptr);
 
 	if (_control_task < 0) {
@@ -923,43 +924,53 @@ MulticopterAttitudeControl::start()
 int mc_att_control_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		errx(1, "usage: mc_att_control {start|stop|status}");
+		warnx("usage: mc_att_control {start|stop|status}");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (mc_att_control::g_control != nullptr)
-			errx(1, "already running");
+		if (mc_att_control::g_control != nullptr) {
+			warnx("already running");
+			return 1;
+		}
 
 		mc_att_control::g_control = new MulticopterAttitudeControl;
 
-		if (mc_att_control::g_control == nullptr)
-			errx(1, "alloc failed");
+		if (mc_att_control::g_control == nullptr) {
+			warnx("alloc failed");
+			return 1;
+		}
 
 		if (OK != mc_att_control::g_control->start()) {
 			delete mc_att_control::g_control;
 			mc_att_control::g_control = nullptr;
-			err(1, "start failed");
+			warnx("start failed");
+			return 1;
 		}
 
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (mc_att_control::g_control == nullptr)
-			errx(1, "not running");
+		if (mc_att_control::g_control == nullptr) {
+			warnx("not running");
+			return 1;
+		}
 
 		delete mc_att_control::g_control;
 		mc_att_control::g_control = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (mc_att_control::g_control) {
-			errx(0, "running");
+			warnx("running");
+			return 0;
 
 		} else {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 	}
 

src/modules/mc_att_control/mc_att_control_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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

src/modules/mc_att_control_multiplatform/mc_att_control.cpp

@@ -71,7 +71,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_att_sp_pub(nullptr),
 	_v_rates_sp_pub(nullptr),
 	_actuators_0_pub(nullptr),
-	_n(),
+	_n(_appState),
 
 	/* parameters */
 	_params_handles({
@@ -236,10 +236,10 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
 		control_attitude_rates(dt);
 
 		/* publish actuator controls */
-		_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-		_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-		_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-		_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+		_actuators.data().control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
+		_actuators.data().control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
+		_actuators.data().control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
+		_actuators.data().control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
 		_actuators.data().timestamp = px4::get_time_micros();
 
 		if (!_actuators_0_circuit_breaker_enabled) {

src/modules/mc_att_control_multiplatform/mc_att_control.h

@@ -94,6 +94,8 @@ private:
 
 	px4::NodeHandle _n;
 
+	px4::AppState _appState;
+
 	struct {
 		px4::ParameterFloat roll_p;
 		px4::ParameterFloat roll_rate_p;

src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp

@@ -68,7 +68,7 @@ int mc_att_control_m_main(int argc, char *argv[])
 
 		task_should_exit = false;
 
-		daemon_task = task_spawn_cmd("mc_att_control_m",
+		daemon_task = px4_task_spawn_cmd("mc_att_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       1900,

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -49,9 +49,11 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <px4.h>
-#include <functional>
-#include <cstdio>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
@@ -59,6 +61,7 @@
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
+#include <functional>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 
@@ -384,7 +387,7 @@ MulticopterPositionControl::~MulticopterPositionControl()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_control_task);
+				px4_task_delete(_control_task);
 				break;
 			}
 		} while (_control_task != -1);
@@ -761,9 +764,9 @@ void MulticopterPositionControl::control_auto(float dt)
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
 
 		//Make sure that the position setpoint is valid
-		if (!isfinite(_pos_sp_triplet.current.lat) ||
-			!isfinite(_pos_sp_triplet.current.lon) ||
-			!isfinite(_pos_sp_triplet.current.alt)) {
+		if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
+			!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
+			!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
 			_pos_sp_triplet.current.valid = false;
 		}
 	}
@@ -881,7 +884,7 @@ void MulticopterPositionControl::control_auto(float dt)
 		_pos_sp = pos_sp_s.edivide(scale);
 
 		/* update yaw setpoint if needed */
-		if (isfinite(_pos_sp_triplet.current.yaw)) {
+		if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
 			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
 		}
 
@@ -933,14 +936,14 @@ MulticopterPositionControl::task_main()
 	R.identity();
 
 	/* wakeup source */
-	struct pollfd fds[1];
+	px4_pollfd_struct_t fds[1];
 
 	fds[0].fd = _local_pos_sub;
 	fds[0].events = POLLIN;
 
 	while (!_task_should_exit) {
 		/* wait for up to 500ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
 
 		/* timed out - periodic check for _task_should_exit */
 		if (pret == 0) {
@@ -1427,11 +1430,9 @@ MulticopterPositionControl::task_main()
 		reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
 	}
 
-	warnx("stopped");
 	mavlink_log_info(_mavlink_fd, "[mpc] stopped");
 
 	_control_task = -1;
-	_exit(0);
 }
 
 int
@@ -1440,11 +1441,11 @@ MulticopterPositionControl::start()
 	ASSERT(_control_task == -1);
 
 	/* start the task */
-	_control_task = task_spawn_cmd("mc_pos_control",
+	_control_task = px4_task_spawn_cmd("mc_pos_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       1500,
-				       (main_t)&MulticopterPositionControl::task_main_trampoline,
+				       (px4_main_t)&MulticopterPositionControl::task_main_trampoline,
 				       nullptr);
 
 	if (_control_task < 0) {
@@ -1458,46 +1459,53 @@ MulticopterPositionControl::start()
 int mc_pos_control_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		errx(1, "usage: mc_pos_control {start|stop|status}");
+		warnx("usage: mc_pos_control {start|stop|status}");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (pos_control::g_control != nullptr) {
-			errx(1, "already running");
+			warnx("already running");
+			return 1;
 		}
 
 		pos_control::g_control = new MulticopterPositionControl;
 
 		if (pos_control::g_control == nullptr) {
-			errx(1, "alloc failed");
+			warnx("alloc failed");
+			return 1;
 		}
 
 		if (OK != pos_control::g_control->start()) {
 			delete pos_control::g_control;
 			pos_control::g_control = nullptr;
-			err(1, "start failed");
+			warnx("start failed");
+			return 1;
 		}
 
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		if (pos_control::g_control == nullptr) {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 
 		delete pos_control::g_control;
 		pos_control::g_control = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (pos_control::g_control) {
-			errx(0, "running");
+			warnx("running");
+			return 0;
 
 		} else {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 	}
 

src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp

@@ -65,7 +65,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_local_pos_sp_msg(),
 	_global_vel_sp_msg(),
 
-	_n(),
+	_n(_appState),
 
 	/* parameters */
 	_params_handles({
@@ -537,7 +537,7 @@ MulticopterPositionControl::control_auto(float dt)
 		_pos_sp = pos_sp_s.edivide(scale);
 
 		/* update yaw setpoint if needed */
-		if (isfinite(_pos_sp_triplet->data().current.yaw)) {
+		if (PX4_ISFINITE(_pos_sp_triplet->data().current.yaw)) {
 			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
 		}
 
@@ -554,9 +554,9 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd
 void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg)
 {
 	/* Make sure that the position setpoint is valid */
-	if (!isfinite(_pos_sp_triplet->data().current.lat) ||
-			!isfinite(_pos_sp_triplet->data().current.lon) ||
-			!isfinite(_pos_sp_triplet->data().current.alt)) {
+	if (!PX4_ISFINITE(_pos_sp_triplet->data().current.lat) ||
+			!PX4_ISFINITE(_pos_sp_triplet->data().current.lon) ||
+			!PX4_ISFINITE(_pos_sp_triplet->data().current.alt)) {
 		_pos_sp_triplet->data().current.valid = false;
 	}
 }

src/modules/mc_pos_control_multiplatform/mc_pos_control.h

@@ -107,6 +107,8 @@ protected:
 
 	px4::NodeHandle _n;
 
+	px4::AppState _appState;
+
 	struct {
 		px4::ParameterFloat thr_min;
 		px4::ParameterFloat thr_max;

src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp

@@ -68,7 +68,7 @@ int mc_pos_control_m_main(int argc, char *argv[])
 
 		task_should_exit = false;
 
-		daemon_task = task_spawn_cmd("mc_pos_control_m",
+		daemon_task = px4_task_spawn_cmd("mc_pos_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       2500,

src/modules/navigator/datalinkloss_params.c

@@ -39,7 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 

src/modules/navigator/geofence.cpp

@@ -47,7 +47,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <ctype.h>
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <unistd.h>
 #include <mavlink/mavlink_log.h>
 #include <geo/geo.h>

src/modules/navigator/geofence_params.c

@@ -39,7 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 

src/modules/navigator/gpsfailure_params.c

@@ -39,7 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 

src/modules/navigator/mission_params.c

@@ -39,7 +39,7 @@
  * @author Julian Oes <joes@student.ethz.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 
@@ -110,4 +110,4 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
  * @max 3
  * @group Mission
  */
-PARAM_DEFINE_INT32(MIS_YAWMODE, 0);
+PARAM_DEFINE_INT32(MIS_YAWMODE, 1);

src/modules/navigator/navigator_main.cpp

@@ -43,7 +43,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdio.h>
 #include <stdlib.h>
@@ -515,7 +515,7 @@ Navigator::start()
 	ASSERT(_navigator_task == -1);
 
 	/* start the task */
-	_navigator_task = task_spawn_cmd("navigator",
+	_navigator_task = px4_task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT + 20,
 					 1700,

src/modules/navigator/navigator_params.c

@@ -40,7 +40,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 

src/modules/navigator/rcloss_params.c

@@ -39,7 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 

src/modules/navigator/rtl_params.c

@@ -39,7 +39,7 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <systemlib/param/param.h>
 

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -44,7 +44,7 @@
 #include <stdbool.h>
 #include <fcntl.h>
 #include <string.h>
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <nuttx/sched.h>
 #include <sys/prctl.h>
 #include <termios.h>
@@ -153,7 +153,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 			}
 
 		thread_should_exit = false;
-		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
+		position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",
 					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
 					       position_estimator_inav_thread_main,
 					       (argv) ? (char * const *) &argv[2] : (char * const *) NULL);

src/modules/px4iofirmware/adc.c

@@ -36,7 +36,7 @@
  *
  * Simple ADC support for PX4IO on STM32.
  */
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <stdint.h>
 
 #include <nuttx/arch.h>

src/modules/px4iofirmware/controls.c

@@ -37,7 +37,7 @@
  * R/C inputs and servo outputs.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <stdbool.h>
 
 #include <drivers/drv_hrt.h>

src/modules/px4iofirmware/dsm.c

@@ -39,7 +39,7 @@
  * Decodes into the global PPM buffer and updates accordingly.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <nuttx/arch.h>
 
 #include <fcntl.h>

src/modules/px4iofirmware/mixer.cpp

@@ -37,7 +37,7 @@
  * Control channel input/output mixer and failsafe.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <syslog.h>
 
 #include <sys/types.h>

src/modules/px4iofirmware/px4io.c

@@ -36,7 +36,7 @@
  * Top-level logic for the PX4IO module.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <nuttx/arch.h>
 
 #include <stdio.h>	// required for task_create

src/modules/px4iofirmware/px4io.h

@@ -37,7 +37,7 @@
  * General defines and structures for the PX4IO module firmware.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 #include <stdint.h>

src/modules/px4iofirmware/registers.c

@@ -37,7 +37,7 @@
  * Implementation of the PX4IO register space.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 #include <stdlib.h>

src/modules/px4iofirmware/safety.c

@@ -36,7 +36,7 @@
  * Safety button logic.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 

src/modules/px4iofirmware/sbus.c

@@ -37,7 +37,7 @@
  * Serial protocol decoder for the Futaba S.bus protocol.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <fcntl.h>
 #include <unistd.h>

src/modules/sdlog2/logbuffer.c

@@ -39,6 +39,7 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
+#include <px4_defines.h>
 #include <string.h>
 #include <stdlib.h>
 
@@ -50,7 +51,7 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
 	lb->write_ptr = 0;
 	lb->read_ptr = 0;
 	lb->data = malloc(lb->size);
-	return (lb->data == 0) ? ERROR : OK;
+	return (lb->data == 0) ? PX4_ERROR : PX4_OK;
 }
 
 int logbuffer_count(struct logbuffer_s *lb)

src/modules/sdlog2/module.mk

@@ -46,6 +46,8 @@ MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
 
+ifeq ($(PX4_TARGET_OS),nuttx)
 EXTRACFLAGS = -Wframe-larger-than=1400
+endif
 
 EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"'

src/modules/sdlog2/sdlog2.c

@@ -42,7 +42,8 @@
  * @author Ban Siesta <bansiesta@gmail.com>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_defines.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <sys/prctl.h>
@@ -112,7 +113,7 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
 
-#define PX4_EPOCH_SECS 1234567890ULL
+#define PX4_EPOCH_SECS 1234567890L
 
 /**
  * Logging rate.
@@ -305,7 +306,7 @@ sdlog2_usage(const char *reason)
 		fprintf(stderr, "%s\n", reason);
 	}
 
-	errx(1, "usage: sdlog2 {start|stop|status|on|off} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
+	warnx("usage: sdlog2 {start|stop|status|on|off} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 		 "\t-b\tLog buffer size in KiB, default is 8\n"
 		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
@@ -326,6 +327,7 @@ int sdlog2_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		sdlog2_usage("missing command");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
@@ -333,17 +335,17 @@ int sdlog2_main(int argc, char *argv[])
 		if (thread_running) {
 			warnx("already running");
 			/* this is not an error */
-			exit(0);
+			return 0;
 		}
 
 		main_thread_should_exit = false;
-		deamon_task = task_spawn_cmd("sdlog2",
+		deamon_task = px4_task_spawn_cmd("sdlog2",
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_DEFAULT - 30,
 						 3000,
 						 sdlog2_thread_main,
 						 (char * const *)argv);
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
@@ -352,7 +354,7 @@ int sdlog2_main(int argc, char *argv[])
 		}
 
 		main_thread_should_exit = true;
-		exit(0);
+		return 0;
 	}
 
 	if (!thread_running) {
@@ -388,7 +390,7 @@ int sdlog2_main(int argc, char *argv[])
 	}
 
 	sdlog2_usage("unrecognized command");
-	exit(1);
+	return 1;
 }
 
 bool get_log_time_utc_tt(struct tm *tt, bool boot_time) {
@@ -505,7 +507,7 @@ int open_log_file()
 		}
 	}
 
-	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
+	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, 0x0777);
 
 	if (fd < 0) {
 		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
@@ -553,7 +555,7 @@ int open_perf_file(const char* str)
 		}
 	}
 
-	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
+	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, 0x0777);
 
 	if (fd < 0) {
 		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
@@ -634,7 +636,8 @@ static void *logwriter_thread(void *arg)
 
 			if (n < 0) {
 				main_thread_should_exit = true;
-				err(1, "error writing log file");
+				warn("error writing log file");
+				break;
 			}
 
 			if (n > 0) {
@@ -683,7 +686,7 @@ void sdlog2_start_log()
 	/* create log dir if needed */
 	if (create_log_dir() != 0) {
 		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
-		exit(1);
+		return;
 	}
 
 	/* initialize statistics counter */
@@ -709,7 +712,7 @@ void sdlog2_start_log()
 
 	/* start log buffer emptying thread */
 	if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
-		errx(1, "error creating logwriter thread");
+		warnx("error creating logwriter thread");
 	}
 
 	/* write all performance counters */
@@ -1016,7 +1019,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 
 	if (check_free_space() != OK) {
-		errx(1, "ERR: MicroSD almost full");
+		warnx("ERR: MicroSD almost full");
+		return 1;
 	}
 
 
@@ -1024,7 +1028,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
 	if (mkdir_ret != 0 && errno != EEXIST) {
-		err(1, "ERR: failed creating log dir: %s", log_root);
+		warn("ERR: failed creating log dir: %s", log_root);
+		return 1;
 	}
 
 	/* copy conversion scripts */
@@ -1042,7 +1047,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	warnx("log buffer size: %i bytes", log_buffer_size);
 
 	if (OK != logbuffer_init(&lb, log_buffer_size)) {
-		errx(1, "can't allocate log buffer, exiting");
+		warnx("can't allocate log buffer, exiting");
+		return 1;
 	}
 
 	struct vehicle_status_s buf_status;
@@ -1665,8 +1671,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (buf.triplet.current.valid) {
 				log_msg.msg_type = LOG_GPSP_MSG;
 				log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
-				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
-				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
+				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * (double)1e7);
+				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * (double)1e7);
 				log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
 				log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
 				log_msg.body.log_GPSP.type = buf.triplet.current.type;
@@ -1955,7 +1961,7 @@ int file_copy(const char *file_old, const char *file_new)
 
 	if (source == NULL) {
 		warnx("ERR: open in");
-		return 1;
+		return PX4_ERROR;
 	}
 
 	target = fopen(file_new, "w");
@@ -1963,7 +1969,7 @@ int file_copy(const char *file_old, const char *file_new)
 	if (target == NULL) {
 		fclose(source);
 		warnx("ERR: open out");
-		return 1;
+		return PX4_ERROR;
 	}
 
 	char buf[128];
@@ -1974,7 +1980,7 @@ int file_copy(const char *file_old, const char *file_new)
 
 		if (ret <= 0) {
 			warnx("error writing file");
-			ret = 1;
+			ret = PX4_ERROR;
 			break;
 		}
 	}
@@ -1984,7 +1990,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return OK;
+	return PX4_OK;
 }
 
 int check_free_space()
@@ -1992,19 +1998,20 @@ int check_free_space()
 	/* use statfs to determine the number of blocks left */
 	FAR struct statfs statfs_buf;
 	if (statfs(mountpoint, &statfs_buf) != OK) {
-		errx(ERROR, "ERR: statfs");
+		warnx("ERR: statfs");
+		return PX4_ERROR;
 	}
 
 	/* use a threshold of 50 MiB */
-	if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
+	if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
 		mavlink_and_console_log_critical(mavlink_fd,
 			"[sdlog2] no space on MicroSD: %u MiB",
 			(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
 		/* we do not need a flag to remember that we sent this warning because we will exit anyway */
-		return ERROR;
+		return PX4_ERROR;
 
 	/* use a threshold of 100 MiB to send a warning */
-	} else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
+	} else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
 		mavlink_and_console_log_critical(mavlink_fd,
 			"[sdlog2] space on MicroSD low: %u MiB",
 			(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
@@ -2012,7 +2019,7 @@ int check_free_space()
 		space_warning_sent = true;
 	}
 
-	return OK;
+	return PX4_OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)

src/modules/segway/segway_main.cpp

@@ -39,7 +39,7 @@
  * Segway controller using control library
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -106,7 +106,7 @@ int segway_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 
-		deamon_task = task_spawn_cmd("segway",
+		deamon_task = px4_task_spawn_cmd("segway",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,

src/modules/sensors/module.mk

@@ -38,7 +38,7 @@
 MODULE_COMMAND	= sensors
 MODULE_PRIORITY	= "SCHED_PRIORITY_MAX-5"
 
-SRCS		= sensors.cpp \
+SRCS =		  sensors.cpp \
 		  sensor_params.c
 
 MODULE_STACKSIZE = 1200

src/modules/sensors/sensor_params.c

@@ -41,7 +41,7 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <systemlib/param/param.h>
 
 /**

src/modules/sensors/sensors.cpp

@@ -46,7 +46,10 @@
  * @author Anton Babushkin <anton@px4.io>
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_time.h>
 
 #include <fcntl.h>
 #include <poll.h>
@@ -59,7 +62,8 @@
 #include <math.h>
 #include <mathlib/mathlib.h>
 
-#include <nuttx/analog/adc.h>
+#include <px4_adc.h>
+//#include <nuttx/analog/adc.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
@@ -111,7 +115,7 @@
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 #define ADC_BATTERY_VOLTAGE_CHANNEL	10
-#define ADC_BATTERY_CURRENT_CHANNEL	-1
+#define ADC_BATTERY_CURRENT_CHANNEL	((uint8_t)(-1))
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
 #endif
 
@@ -124,8 +128,8 @@
 
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 #define ADC_BATTERY_VOLTAGE_CHANNEL	10
-#define ADC_BATTERY_CURRENT_CHANNEL	-1
-#define ADC_AIRSPEED_VOLTAGE_CHANNEL	-1
+#define ADC_BATTERY_CURRENT_CHANNEL	((uint8_t)(-1))
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	((uint8_t)(-1))
 #endif
 
 #define BATT_V_LOWPASS			0.001f
@@ -223,7 +227,7 @@ private:
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_baro1_sub;			/**< raw baro data subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
+	//int		_airspeed_sub;			/**< airspeed subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -655,7 +659,7 @@ Sensors::~Sensors()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_sensors_task);
+				px4_task_delete(_sensors_task);
 				break;
 			}
 		} while (_sensors_task != -1);
@@ -684,7 +688,7 @@ Sensors::parameters_update()
 		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
 
 		/* handle blowup in the scaling factor calculation */
-		if (!isfinite(tmpScaleFactor) ||
+		if (!PX4_ISFINITE(tmpScaleFactor) ||
 		    (tmpRevFactor < 0.000001f) ||
 		    (tmpRevFactor > 0.2f)) {
 			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
@@ -830,18 +834,18 @@ Sensors::parameters_update()
 
 	/* set px4flow rotation */
 	int	flowfd;
-	flowfd = open(PX4FLOW0_DEVICE_PATH, 0);
+	flowfd = px4_open(PX4FLOW0_DEVICE_PATH, 0);
 
 	if (flowfd >= 0) {
-		int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
+		int flowret = px4_ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
 
 		if (flowret) {
 			warnx("flow rotation could not be set");
-			close(flowfd);
+			px4_close(flowfd);
 			return ERROR;
 		}
 
-		close(flowfd);
+		px4_close(flowfd);
 	}
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
@@ -856,27 +860,27 @@ Sensors::parameters_update()
 					 M_DEG_TO_RAD_F * _parameters.board_offset[1],
 					 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
 
-	_board_rotation = _board_rotation * board_rotation_offset;
+	_board_rotation = board_rotation_offset * _board_rotation;
 
 	/* update barometer qnh setting */
 	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
 	int	barofd;
-	barofd = open(BARO0_DEVICE_PATH, 0);
+	barofd = px4_open(BARO0_DEVICE_PATH, 0);
 
 	if (barofd < 0) {
 		warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH);
 		return ERROR;
 
 	} else {
-		int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		int baroret = px4_ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
 
 		if (baroret) {
 			warnx("qnh could not be set");
-			close(barofd);
+			px4_close(barofd);
 			return ERROR;
 		}
 
-		close(barofd);
+		px4_close(barofd);
 	}
 
 	return OK;
@@ -887,7 +891,7 @@ Sensors::accel_init()
 {
 	int	fd;
 
-	fd = open(ACCEL0_DEVICE_PATH, 0);
+	fd = px4_open(ACCEL0_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH);
@@ -896,12 +900,12 @@ Sensors::accel_init()
 	} else {
 
 		/* set the accel internal sampling rate to default rate */
-		ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT);
+		px4_ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT);
 
 		/* set the driver to poll at default rate */
-		ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+		px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
 
-		close(fd);
+		px4_close(fd);
 	}
 
 	return OK;
@@ -912,7 +916,7 @@ Sensors::gyro_init()
 {
 	int	fd;
 
-	fd = open(GYRO0_DEVICE_PATH, 0);
+	fd = px4_open(GYRO0_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH);
@@ -921,10 +925,10 @@ Sensors::gyro_init()
 	} else {
 
 		/* set the gyro internal sampling rate to default rate */
-		ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
+		px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
 
 		/* set the driver to poll at default rate */
-		ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+		px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
 
 	}
 
@@ -937,7 +941,7 @@ Sensors::mag_init()
 	int	fd;
 	int	ret;
 
-	fd = open(MAG0_DEVICE_PATH, 0);
+	fd = px4_open(MAG0_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH);
@@ -947,19 +951,19 @@ Sensors::mag_init()
 	/* try different mag sampling rates */
 
 
-	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 150);
 
 	if (ret == OK) {
 		/* set the pollrate accordingly */
-		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+		px4_ioctl(fd, SENSORIOCSPOLLRATE, 150);
 
 	} else {
-		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 100);
 
 		/* if the slower sampling rate still fails, something is wrong */
 		if (ret == OK) {
 			/* set the driver to poll also at the slower rate */
-			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+			px4_ioctl(fd, SENSORIOCSPOLLRATE, 100);
 
 		} else {
 			warnx("FATAL: mag sampling rate could not be set");
@@ -967,7 +971,7 @@ Sensors::mag_init()
 		}
 	}
 
-	close(fd);
+	px4_close(fd);
 
 	return OK;
 }
@@ -977,7 +981,7 @@ Sensors::baro_init()
 {
 	int	fd;
 
-	fd = open(BARO0_DEVICE_PATH, 0);
+	fd = px4_open(BARO0_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH);
@@ -985,9 +989,9 @@ Sensors::baro_init()
 	}
 
 	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	px4_ioctl(fd, SENSORIOCSPOLLRATE, 150);
 
-	close(fd);
+	px4_close(fd);
 
 	return OK;
 }
@@ -996,7 +1000,7 @@ int
 Sensors::adc_init()
 {
 
-	_fd_adc = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
+	_fd_adc = px4_open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
 
 	if (_fd_adc < 0) {
 		warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH);
@@ -1358,7 +1362,7 @@ Sensors::parameter_update_poll(bool forced)
 			res = ERROR;
 			(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 
-			int fd = open(str, 0);
+			int fd = px4_open(str, 0);
 
 			if (fd < 0) {
 				continue;
@@ -1376,12 +1380,12 @@ Sensors::parameter_update_poll(bool forced)
 				failed = failed || (OK != param_get(param_find(str), &device_id));
 
 				if (failed) {
-					close(fd);
+					px4_close(fd);
 					continue;
 				}
 
 				/* if the calibration is for this device, apply it */
-				if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) {
+				if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) {
 					struct gyro_scale gscale = {};
 					(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
 					failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@@ -1400,7 +1404,7 @@ Sensors::parameter_update_poll(bool forced)
 						warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
 					} else {
 						/* apply new scaling and offsets */
-						res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
+						res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
 							warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
 						} else {
@@ -1415,7 +1419,7 @@ Sensors::parameter_update_poll(bool forced)
 				gyro_count++;
 			}
 
-			close(fd);
+			px4_close(fd);
 		}
 
 		/* run through all accel sensors */
@@ -1424,7 +1428,7 @@ Sensors::parameter_update_poll(bool forced)
 			res = ERROR;
 			(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 
-			int fd = open(str, 0);
+			int fd = px4_open(str, 0);
 
 			if (fd < 0) {
 				continue;
@@ -1442,12 +1446,12 @@ Sensors::parameter_update_poll(bool forced)
 				failed = failed || (OK != param_get(param_find(str), &device_id));
 
 				if (failed) {
-					close(fd);
+					px4_close(fd);
 					continue;
 				}
 
 				/* if the calibration is for this device, apply it */
-				if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) {
+				if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) {
 					struct accel_scale gscale = {};
 					(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 					failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@@ -1466,7 +1470,7 @@ Sensors::parameter_update_poll(bool forced)
 						warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
 					} else {
 						/* apply new scaling and offsets */
-						res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
+						res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
 							warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
 						} else {
@@ -1481,7 +1485,7 @@ Sensors::parameter_update_poll(bool forced)
 				accel_count++;
 			}
 
-			close(fd);
+			px4_close(fd);
 		}
 
 		/* run through all mag sensors */
@@ -1490,7 +1494,7 @@ Sensors::parameter_update_poll(bool forced)
 			res = ERROR;
 			(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
 
-			int fd = open(str, 0);
+			int fd = px4_open(str, 0);
 
 			if (fd < 0) {
 				/* the driver is not running, abort */
@@ -1515,12 +1519,12 @@ Sensors::parameter_update_poll(bool forced)
 				failed = failed || (OK != param_get(param_find(str), &device_id));
 
 				if (failed) {
-					close(fd);
+					px4_close(fd);
 					continue;
 				}
 
 				/* if the calibration is for this device, apply it */
-				if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) {
+				if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) {
 					struct mag_scale gscale = {};
 					(void)sprintf(str, "CAL_MAG%u_XOFF", i);
 					failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@@ -1537,7 +1541,7 @@ Sensors::parameter_update_poll(bool forced)
 
 					(void)sprintf(str, "CAL_MAG%u_ROT", i);
 
-					if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) {
+					if (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) {
 						/* mag is internal */
 						_mag_rotation[s] = _board_rotation;
 						/* reset param to -1 to indicate internal mag */
@@ -1589,7 +1593,7 @@ Sensors::parameter_update_poll(bool forced)
 						warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
 					} else {
 						/* apply new scaling and offsets */
-						res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
+						res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
 							warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
 						} else {
@@ -1604,10 +1608,10 @@ Sensors::parameter_update_poll(bool forced)
 				mag_count++;
 			}
 
-			close(fd);
+			px4_close(fd);
 		}
 
-		int fd = open(AIRSPEED0_DEVICE_PATH, 0);
+		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
 
 		/* this sensor is optional, abort without error */
 
@@ -1617,11 +1621,11 @@ Sensors::parameter_update_poll(bool forced)
 				1.0f,
 			};
 
-			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+			if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 				warn("WARNING: failed to set scale / offsets for airspeed sensor");
 			}
 
-			close(fd);
+			px4_close(fd);
 		}
 
 		/* do not output this for now, as its covered in preflight checks */
@@ -1687,7 +1691,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 		/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
 		struct adc_msg_s buf_adc[12];
 		/* read all channels available */
-		int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
+		int ret = px4_read(_fd_adc, &buf_adc, sizeof(buf_adc));
 
 		if (ret >= (int)sizeof(buf_adc[0])) {
 
@@ -1968,7 +1972,7 @@ Sensors::rc_poll()
 			_rc.channels[i] *= _parameters.rev[i];
 
 			/* handle any parameter-induced blowups */
-			if (!isfinite(_rc.channels[i])) {
+			if (!PX4_ISFINITE(_rc.channels[i])) {
 				_rc.channels[i] = 0.0f;
 			}
 		}
@@ -2170,7 +2174,7 @@ Sensors::task_main()
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
 	/* wakeup source(s) */
-	struct pollfd fds[1];
+	px4_pollfd_struct_t fds[1];
 
 	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
 	fds[0].fd = _gyro_sub;
@@ -2181,7 +2185,7 @@ Sensors::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 50ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
 
 		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 
@@ -2240,7 +2244,7 @@ Sensors::task_main()
 
 exit_immediate:
 	_sensors_task = -1;
-	_exit(ret);
+	px4_task_exit(ret);
 }
 
 int
@@ -2249,11 +2253,11 @@ Sensors::start()
 	ASSERT(_sensors_task == -1);
 
 	/* start the task */
-	_sensors_task = task_spawn_cmd("sensors_task",
+	_sensors_task = px4_task_spawn_cmd("sensors_task",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       2000,
-				       (main_t)&Sensors::task_main_trampoline,
+				       (px4_main_t)&Sensors::task_main_trampoline,
 				       nullptr);
 
 	/* wait until the task is up and running or has failed */
@@ -2271,46 +2275,53 @@ Sensors::start()
 int sensors_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		errx(1, "usage: sensors {start|stop|status}");
+		warnx("usage: sensors {start|stop|status}");
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (sensors::g_sensors != nullptr) {
-			errx(0, "already running");
+			warnx("already running");
+			return 0;
 		}
 
 		sensors::g_sensors = new Sensors;
 
 		if (sensors::g_sensors == nullptr) {
-			errx(1, "alloc failed");
+			warnx("alloc failed");
+			return 1;
 		}
 
 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
 			sensors::g_sensors = nullptr;
-			err(1, "start failed");
+			warnx("start failed");
+			return 1;
 		}
 
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		if (sensors::g_sensors == nullptr) {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 
 		delete sensors::g_sensors;
 		sensors::g_sensors = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			errx(0, "is running");
+			warnx("is running");
+			return 0;
 
 		} else {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 	}
 

src/modules/simulator/module.mk

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build simulator 
+#
+
+MODULE_COMMAND		= simulator
+
+SRCS			= simulator.cpp
+
+ifneq ($(PX4_TARGET_OS), qurt)
+SRCS			+= simulator_mavlink.cpp
+endif
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed

src/modules/simulator/simulator.cpp

@@ -0,0 +1,258 @@
+/****************************************************************************
+ *
+ *   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 simulator.cpp
+ * A device simulator
+ */
+
+#include <px4_log.h>
+#include <px4_tasks.h>
+#include <px4_time.h>
+#include <pthread.h>
+#include <poll.h>
+#include <systemlib/err.h>
+#include <errno.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/types.h> 
+#include <drivers/drv_led.h>
+
+#include "simulator.h"
+
+using namespace simulator;
+
+static px4_task_t g_sim_task = -1;
+
+Simulator *Simulator::_instance = NULL;
+
+Simulator *Simulator::getInstance()
+{
+	return _instance;
+}
+
+bool Simulator::getMPUReport(uint8_t *buf, int len)
+{
+	return _mpu.copyData(buf, len);
+}
+
+bool Simulator::getRawAccelReport(uint8_t *buf, int len)
+{
+	return _accel.copyData(buf, len);
+}
+
+bool Simulator::getBaroSample(uint8_t *buf, int len)
+{
+	return _baro.copyData(buf, len);
+}
+
+int Simulator::start(int argc, char *argv[])
+{
+	int ret = 0;
+	_instance = new Simulator();
+	if (_instance) {
+		PX4_INFO("Simulator started");
+		drv_led_start();
+		if (argv[2][1] == 's') {
+#ifndef __PX4_QURT
+			_instance->updateSamples();
+#endif
+		} else {
+			_instance->publishSensorsCombined();
+		}
+	}
+	else {
+		PX4_WARN("Simulator creation failed");
+		ret = 1;
+	}
+	return ret;
+}
+
+void Simulator::publishSensorsCombined() {
+
+	struct baro_report baro;
+	memset(&baro,0,sizeof(baro));
+	baro.pressure = 120000.0f;
+
+	// acceleration report
+	struct accel_report accel;
+	memset(&accel,0,sizeof(accel));
+	accel.z = 9.81f;
+	accel.range_m_s2 = 80.0f;
+
+	// gyro report
+	struct gyro_report gyro;
+	memset(&gyro, 0 ,sizeof(gyro));
+
+	// mag report
+	struct mag_report mag;
+	memset(&mag, 0 ,sizeof(mag));
+	// init publishers
+	_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
+	_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+	_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+	_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
+
+	struct sensor_combined_s sensors;
+	memset(&sensors, 0, sizeof(sensors));
+	// fill sensors with some data
+	sensors.accelerometer_m_s2[2] = 9.81f;
+	sensors.magnetometer_ga[0] = 0.2f;
+	sensors.timestamp = hrt_absolute_time();
+	sensors.accelerometer_timestamp = hrt_absolute_time();
+	sensors.magnetometer_timestamp = hrt_absolute_time();
+	sensors.baro_timestamp = hrt_absolute_time();
+	// advertise
+	_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors);
+
+	hrt_abstime time_last = hrt_absolute_time();
+	uint64_t delta;
+	for(;;) {
+		delta = hrt_absolute_time() - time_last;
+		if(delta > (uint64_t)1000000) {
+			time_last = hrt_absolute_time();
+			sensors.timestamp = time_last;
+			sensors.accelerometer_timestamp = time_last;
+			sensors.magnetometer_timestamp = time_last;
+			sensors.baro_timestamp = time_last;
+			baro.timestamp = time_last;
+			accel.timestamp = time_last;
+			gyro.timestamp = time_last;
+			mag.timestamp = time_last;
+			// publish the sensor values
+			//PX4_DEBUG("Publishing SensorsCombined\n");
+			orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &sensors);
+			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro);
+			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro);
+			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+		}
+		else {
+			usleep(1000000-delta);
+		}
+	}
+}
+
+static void usage()
+{
+	PX4_WARN("Usage: simulator {start -[sc] |stop}");
+	PX4_WARN("Simulate raw sensors:     simulator start -s");
+	PX4_WARN("Publish sensors combined: simulator start -p");
+}
+
+__BEGIN_DECLS
+extern int simulator_main(int argc, char *argv[]);
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+extern "C" {
+
+int simulator_main(int argc, char *argv[])
+{
+	int ret = 0;
+	if (argc == 3 && strcmp(argv[1], "start") == 0) {
+		if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
+			if (g_sim_task >= 0) {
+				warnx("Simulator already started");
+				return 0;
+			}
+			g_sim_task = px4_task_spawn_cmd("Simulator",
+				SCHED_DEFAULT,
+				SCHED_PRIORITY_MAX - 5,
+				1500,
+				Simulator::start,
+				argv);
+		}
+		else
+		{
+			usage();
+			ret = -EINVAL;
+		}
+	}
+	else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
+		if (g_sim_task < 0) {
+			PX4_WARN("Simulator not running");
+		}
+		else {
+			px4_task_delete(g_sim_task);
+			g_sim_task = -1;
+		}
+	}
+	else {
+		usage();
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+}
+
+bool static _led_state[2] = { false , false };
+
+__EXPORT void led_init()
+{
+	PX4_DEBUG("LED_INIT");
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1 || led == 0)
+	{
+		PX4_DEBUG("LED%d_ON", led);
+		_led_state[led] = true;
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1 || led == 0)
+	{
+		PX4_DEBUG("LED%d_OFF", led);
+		_led_state[led] = false;
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1 || led == 0)
+	{
+		_led_state[led] = !_led_state[led];
+		PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF");
+
+	}
+}
+

src/modules/simulator/simulator.h

@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   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 simulator.h
+ * A device simulator
+ */
+
+#pragma once
+
+#include <semaphore.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_baro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <v1.0/mavlink_types.h>
+#include <v1.0/common/mavlink.h>
+#ifndef __PX4_QURT
+#include <sys/socket.h>
+#include <netinet/in.h>
+#endif
+
+namespace simulator {
+
+// FIXME - what is the endianness of these on actual device?
+#pragma pack(push, 1)
+struct RawAccelData {
+        int16_t x;
+        int16_t y;
+        int16_t z;
+};
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+struct RawMPUData {
+	uint8_t	accel_x[2];
+	uint8_t	accel_y[2];
+	uint8_t	accel_z[2];
+	uint8_t	temp[2];
+	uint8_t	gyro_x[2];
+	uint8_t	gyro_y[2];
+	uint8_t	gyro_z[2];
+};
+#pragma pack(pop)
+
+struct RawBaroData {
+	uint8_t		d[3];
+};
+
+template <typename RType> class Report {
+public:
+	Report(int readers) :
+        	_max_readers(readers),
+        	_report_len(sizeof(RType))
+	{
+        	sem_init(&_lock, 0, _max_readers);
+	}
+
+	~Report() {};
+
+	bool copyData(void *outbuf, int len)
+	{
+		if (len != _report_len) {
+			return false;
+		}
+		read_lock();
+		memcpy(outbuf, &_buf[_readidx], _report_len);
+		read_unlock();
+		return true;
+	}
+	void writeData(void *inbuf)
+	{
+		write_lock();
+		memcpy(&_buf[!_readidx], inbuf, _report_len);
+		_readidx = !_readidx;
+		write_unlock();
+	}
+
+protected:
+	void read_lock() { sem_wait(&_lock); }
+	void read_unlock() { sem_post(&_lock); }
+	void write_lock() 
+	{
+		for (int i=0; i<_max_readers; i++) {
+                	sem_wait(&_lock);
+        	}
+	}
+	void write_unlock()
+	{
+		for (int i=0; i<_max_readers; i++) {
+			sem_post(&_lock);
+		}
+	}
+
+	int _readidx;
+	sem_t _lock;
+	const int _max_readers;
+	const int _report_len;
+	RType _buf[2];
+};
+
+};
+
+class Simulator {
+public:
+	static Simulator *getInstance();
+
+	enum sim_dev_t {
+		SIM_GYRO,
+		SIM_ACCEL,
+		SIM_MAG
+	};
+
+	struct sample {
+		float x;
+		float y;
+		float z;
+		sample() : x(0), y(0), z(0) {}
+		sample(float a, float b, float c) : x(a), y(b), z(c) {}
+	};
+
+	static int start(int argc, char *argv[]);
+
+	bool getRawAccelReport(uint8_t *buf, int len);
+	bool getMPUReport(uint8_t *buf, int len);
+	bool getBaroSample(uint8_t *buf, int len);
+private:
+	Simulator() :
+	_accel(1),
+	_mpu(1),
+	_baro(1),
+	_sensor_combined_pub(-1)
+#ifndef __PX4_QURT
+	,
+	_manual_control_sp_pub(-1),
+	_actuator_outputs_sub(-1),
+	_vehicle_attitude_sub(-1),
+	_sensor{},
+	_manual_control_sp{},
+	_actuators{},
+	_attitude{}
+#endif
+	{}
+	~Simulator() { _instance=NULL; }
+
+#ifndef __PX4_QURT
+	void updateSamples();
+#endif
+
+	static Simulator *_instance;
+
+	// simulated sensor instances
+	simulator::Report<simulator::RawAccelData> 	_accel;
+	simulator::Report<simulator::RawMPUData>	_mpu;
+	simulator::Report<simulator::RawBaroData>	_baro;
+
+	// uORB publisher handlers
+	orb_advert_t _accel_pub;
+	orb_advert_t _baro_pub;
+	orb_advert_t _gyro_pub;
+	orb_advert_t _mag_pub;
+	orb_advert_t _sensor_combined_pub;
+
+	// class methods
+	void publishSensorsCombined();
+
+#ifndef __PX4_QURT
+	// uORB publisher handlers
+	orb_advert_t _manual_control_sp_pub;
+
+	// uORB subscription handlers
+	int _actuator_outputs_sub;
+	int _vehicle_attitude_sub;
+
+	// uORB data containers
+	struct sensor_combined_s _sensor;
+	struct manual_control_setpoint_s _manual_control_sp;
+	struct actuator_outputs_s _actuators;
+	struct vehicle_attitude_s _attitude;
+
+	int _fd;
+	unsigned char _buf[200];
+	hrt_abstime _time_last;
+	struct sockaddr_in _srcaddr;
+	socklen_t _addrlen = sizeof(_srcaddr);
+
+	void poll_topics();
+	void handle_message(mavlink_message_t *msg);
+	void send_data();
+	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
+	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
+	static void *sending_trampoline(void *);
+	void send();
+#endif
+};

src/modules/simulator/simulator_mavlink.cpp

@@ -0,0 +1,421 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <px4_log.h>
+#include <px4_time.h>
+#include "simulator.h"
+#include "errno.h"
+#include <drivers/drv_pwm_output.h>
+
+using namespace simulator;
+
+#define SEND_INTERVAL 	20
+#define UDP_PORT 	14550
+#define PIXHAWK_DEVICE "/dev/ttyACM0"
+
+static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+
+void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
+	float out[8];
+
+	const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+
+	// for now we only support quadrotors
+	unsigned n = 4;
+
+	for (unsigned i = 0; i < 8; i++) {
+		if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
+			if (i < n) {
+				// scale PWM out 900..2100 us to 0..1 for rotors */
+				out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+
+			} else {
+				// scale PWM out 900..2100 us to -1..1 for other channels */
+				out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+			}
+
+		} else {
+			// send 0 when disarmed and for disabled channels */
+			out[i] = 0.0f;
+		}
+	}
+
+	actuator_msg.time_usec = hrt_absolute_time();
+	actuator_msg.roll_ailerons = out[0];
+	actuator_msg.pitch_elevator = out[1];
+	actuator_msg.yaw_rudder = out[2];
+	actuator_msg.throttle = out[3];
+	actuator_msg.aux1 = out[4];
+	actuator_msg.aux2 = out[5];
+	actuator_msg.aux3 = out[6];
+	actuator_msg.aux4 = out[7];
+	actuator_msg.mode = 0; // need to put something here
+	actuator_msg.nav_mode = 0;
+}
+
+void Simulator::send_data() {
+	// check if it's time to send new data
+	hrt_abstime time_now = hrt_absolute_time();
+	if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) {
+		_time_last = time_now;
+		mavlink_hil_controls_t msg;
+		pack_actuator_message(msg);
+		send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
+		// can add more messages here, can also setup different timings
+	}
+}
+
+static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) {
+	manual->timestamp = hrt_absolute_time();
+	manual->x = man_msg->x / 1000.0f;
+	manual->y = man_msg->y / 1000.0f;
+	manual->r = man_msg->r / 1000.0f;
+	manual->z = man_msg->z / 1000.0f;
+}
+
+void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) {
+	hrt_abstime timestamp = hrt_absolute_time();
+	sensor->timestamp = timestamp;
+	sensor->gyro_raw[0] = imu->xgyro * 1000.0f;
+	sensor->gyro_raw[1] = imu->ygyro * 1000.0f;
+	sensor->gyro_raw[2] = imu->zgyro * 1000.0f;
+	sensor->gyro_rad_s[0] = imu->xgyro;
+	sensor->gyro_rad_s[1] = imu->ygyro;
+	sensor->gyro_rad_s[2] = imu->zgyro;
+
+	sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2;
+	sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2;
+	sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2;
+	sensor->accelerometer_m_s2[0] = imu->xacc;
+	sensor->accelerometer_m_s2[1] = imu->yacc;
+	sensor->accelerometer_m_s2[2] = imu->zacc;
+	sensor->accelerometer_mode = 0; // TODO what is this?
+	sensor->accelerometer_range_m_s2 = 32.7f; // int16
+	sensor->accelerometer_timestamp = timestamp;
+	sensor->timestamp = timestamp;
+
+	sensor->adc_voltage_v[0] = 0.0f;
+	sensor->adc_voltage_v[1] = 0.0f;
+	sensor->adc_voltage_v[2] = 0.0f;
+
+	sensor->magnetometer_raw[0] = imu->xmag * 1000.0f;
+	sensor->magnetometer_raw[1] = imu->ymag * 1000.0f;
+	sensor->magnetometer_raw[2] = imu->zmag * 1000.0f;
+	sensor->magnetometer_ga[0] = imu->xmag;
+	sensor->magnetometer_ga[1] = imu->ymag;
+	sensor->magnetometer_ga[2] = imu->zmag;
+	sensor->magnetometer_range_ga = 32.7f; // int16
+	sensor->magnetometer_mode = 0; // TODO what is this
+	sensor->magnetometer_cuttoff_freq_hz = 50.0f;
+	sensor->magnetometer_timestamp = timestamp;
+
+	sensor->baro_pres_mbar = imu->abs_pressure;
+	sensor->baro_alt_meter = imu->pressure_alt;
+	sensor->baro_temp_celcius = imu->temperature;
+	sensor->baro_timestamp = timestamp;
+
+	sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa
+	sensor->differential_pressure_timestamp = timestamp;
+}
+
+void Simulator::handle_message(mavlink_message_t *msg) {
+	switch(msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_SENSOR:
+			mavlink_hil_sensor_t imu;
+			mavlink_msg_hil_sensor_decode(msg, &imu);
+			fill_sensors_from_imu_msg(&_sensor, &imu);
+
+			// publish message
+			if(_sensor_combined_pub < 0) {
+				_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
+			} else {
+				orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor);
+			}
+			break;
+
+		case MAVLINK_MSG_ID_MANUAL_CONTROL:
+
+			mavlink_manual_control_t man_ctrl_sp;
+			mavlink_msg_manual_control_decode(msg, &man_ctrl_sp);
+			fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp);
+
+			// publish message
+			if(_manual_control_sp_pub < 0) {
+				_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp);
+			} else {
+				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp);
+			}
+			break;
+	}
+}
+
+void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
+	component_ID = 0;
+	uint8_t payload_len = mavlink_message_lengths[msgid];
+	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	/* header */
+	buf[0] = MAVLINK_STX;
+	buf[1] = payload_len;
+	/* no idea which numbers should be here*/
+	buf[2] = 100;
+	buf[3] = 0;
+	buf[4] = component_ID;
+	buf[5] = msgid;
+
+	/* payload */
+	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
+
+	/* checksum */
+	uint16_t checksum;
+	crc_init(&checksum);
+	crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
+	crc_accumulate(mavlink_message_crcs[msgid], &checksum);
+
+	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
+	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
+
+	ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen);
+	if (len <= 0) {
+		PX4_WARN("Failed sending mavlink message");
+	}
+}
+
+void Simulator::poll_topics() {
+	// copy new data if available
+	bool updated;
+	orb_check(_actuator_outputs_sub, &updated);
+	if(updated) {
+		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
+	}
+
+	orb_check(_vehicle_attitude_sub, &updated);
+	if(updated) {
+		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude);
+	}
+}
+
+void *Simulator::sending_trampoline(void *) {
+	_instance->send();
+	return 0;	// why do I have to put this???
+}
+
+void Simulator::send() {
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd = _actuator_outputs_sub;
+	fds[0].events = POLLIN;
+
+	_time_last = hrt_absolute_time();
+
+	while(true) {
+		// wait for up to 100ms for data
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		//timed out
+		if (pret == 0)
+			continue;
+
+		// this is undesirable but not much we can do
+		if (pret < 0) {
+			PX4_WARN("poll error %d, %d", pret, errno);
+			// sleep a bit before next try
+			usleep(100000);
+			continue;
+		}
+
+		if (fds[0].revents & POLLIN) {
+			// got new data to read, update all topics
+			poll_topics();
+			send_data();
+		}
+	}
+}
+
+void Simulator::updateSamples()
+{
+	// udp socket data
+	struct sockaddr_in _myaddr;
+	const int _port = UDP_PORT;
+
+	struct baro_report baro;
+	memset(&baro,0,sizeof(baro));
+	baro.pressure = 120000.0f;
+
+	// acceleration report
+	struct accel_report accel;
+	memset(&accel,0,sizeof(accel));
+	accel.z = 9.81f;
+	accel.range_m_s2 = 80.0f;
+
+	// gyro report
+	struct gyro_report gyro;
+	memset(&gyro, 0 ,sizeof(gyro));
+
+	// mag report
+	struct mag_report mag;
+	memset(&mag, 0 ,sizeof(mag));
+
+	// init publishers
+	_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
+	_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+	_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+	_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
+
+	// subscribe to topics
+	_actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
+	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+
+	// try to setup udp socket for communcation with simulator
+	memset((char *)&_myaddr, 0, sizeof(_myaddr));
+	_myaddr.sin_family = AF_INET;
+	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+	_myaddr.sin_port = htons(_port);
+
+	if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+		PX4_WARN("create socket failed\n");
+		return;
+	}
+
+	if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
+		PX4_WARN("bind failed\n");
+		return;
+	}
+
+	// create a thread for sending data to the simulator
+	pthread_t sender_thread;
+
+	// initialize threads
+	pthread_attr_t sender_thread_attr;
+	pthread_attr_init(&sender_thread_attr);
+	pthread_attr_setstacksize(&sender_thread_attr, 1000);
+
+	struct sched_param param;
+	(void)pthread_attr_getschedparam(&sender_thread_attr, &param);
+
+	/* low priority */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT;
+	(void)pthread_attr_setschedparam(&sender_thread_attr, &param);
+	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
+	pthread_attr_destroy(&sender_thread_attr);
+
+	// setup serial connection to autopilot (used to get manual controls)
+	int serial_fd = open(PIXHAWK_DEVICE, O_RDWR);
+
+	if (serial_fd < 0) {
+		PX4_WARN("failed to open %s", PIXHAWK_DEVICE);
+	}
+
+	// tell the device to stream some messages
+	char command[] = "\nsh /etc/init.d/rc.usb\n";
+	int w = ::write(serial_fd, command, sizeof(command));
+
+	if (w <= 0) {
+		PX4_WARN("failed to send streaming command to %s", PIXHAWK_DEVICE);
+	}
+
+	char serial_buf[1024];
+
+	struct pollfd fds[2];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+	fds[1].fd = serial_fd;
+	fds[1].events = POLLIN;
+
+	int len = 0;
+	// wait for new mavlink messages to arrive
+	while (true) {
+
+		int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
+
+		//timed out
+		if (pret == 0)
+			continue;
+
+		// this is undesirable but not much we can do
+		if (pret < 0) {
+			PX4_WARN("poll error %d, %d", pret, errno);
+			// sleep a bit before next try
+			usleep(100000);
+			continue;
+		}
+
+		// got data from simulator
+		if (fds[0].revents & POLLIN) {
+			len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
+			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, &status))
+					{
+						// have a message, handle it
+						handle_message(&msg);
+					}
+				}
+			}
+		}
+
+		// got data from PIXHAWK
+		if (fds[1].revents & POLLIN) {
+			len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
+			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, serial_buf[i], &msg, &status))
+					{
+						// have a message, handle it
+						handle_message(&msg);
+					}
+				}
+			}
+		}
+
+		// publish these messages so that attitude estimator does not complain
+		hrt_abstime time_last = hrt_absolute_time();
+		baro.timestamp = time_last;
+		accel.timestamp = time_last;
+		gyro.timestamp = time_last;
+		mag.timestamp = time_last;
+		// publish the sensor values
+		orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
+		orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro);
+		orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+	}
+}

src/modules/systemlib/bson/tinybson.c

@@ -37,6 +37,7 @@
  * A simple subset SAX-style BSON parser and generator.
  */
 
+#include <px4_posix.h>
 #include <unistd.h>
 #include <string.h>
 #include <stdlib.h>
@@ -53,13 +54,24 @@
 #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
+
 static int
 read_x(bson_decoder_t decoder, void *p, size_t s)
 {
 	CODER_CHECK(decoder);
 
-	if (decoder->fd > -1)
-		return (read(decoder->fd, p, s) == (int)s) ? 0 : -1;
+	if (decoder->fd > -1) {
+		return (BSON_READ(decoder->fd, p, s) == (int)s) ? 0 : -1;
+	}
 
 	if (decoder->buf != NULL) {
 		/* staged operations to avoid integer overflow for corrupt data */
@@ -301,7 +313,7 @@ write_x(bson_encoder_t encoder, const void *p, size_t s)
 	CODER_CHECK(encoder);
 
 	if (encoder->fd > -1)
-		return (write(encoder->fd, p, s) == (int)s) ? 0 : -1;
+		return (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1;
 
 	/* do we need to extend the buffer? */
 	while ((encoder->bufpos + s) > encoder->bufsize) {
@@ -408,7 +420,7 @@ bson_encoder_fini(bson_encoder_t encoder)
 	}
 
 	/* sync file */
-	fsync(encoder->fd);
+	BSON_FSYNC(encoder->fd);
 
 	return 0;
 }

src/modules/systemlib/circuit_breaker.cpp

@@ -42,7 +42,8 @@
  * parameter needs to set to the key (magic).
  */
 
-#include <px4.h>
+#include <stdint.h>
+#include <px4_defines.h>
 #include <systemlib/circuit_breaker.h>
 
 bool circuit_breaker_enabled(const char *breaker, int32_t magic)

src/modules/systemlib/circuit_breaker_params.c

@@ -120,3 +120,19 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
  * @group Circuit Breaker
  */
 PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
+
+/**
+ * Circuit breaker for GPS failure detection
+ *
+ * Setting this parameter to 240024 will disable the GPS failure detection.
+ * If this check is enabled, then the sensor check will fail if the GPS module
+ * is missing. It will also check for excessive signal noise on the GPS receiver
+ * and warn the user if detected.
+ * 
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 240024
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);

src/modules/systemlib/conversions.c

@@ -37,7 +37,7 @@
  * Implementation of commonly used conversions.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <float.h>
 
 #include "conversions.h"

src/modules/systemlib/cpuload.c

@@ -41,16 +41,16 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  */
-#include <nuttx/config.h>
-#include <nuttx/sched.h>
+#include <px4_config.h>
+//#include <nuttx/sched.h>
 
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
 
-#include <arch/arch.h>
+//#include <arch/arch.h>
 
-#include <debug.h>
+//#include <debug.h>
 
 #include <sys/time.h>
 
@@ -61,6 +61,8 @@
 
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 
+#ifdef __PX4_NUTTX
+
 __EXPORT void sched_note_start(FAR struct tcb_s *tcb);
 __EXPORT void sched_note_stop(FAR struct tcb_s *tcb);
 __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb);
@@ -167,4 +169,7 @@ void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb)
 	}
 }
 
+#else
+__EXPORT struct system_load_s system_load;
+#endif
 #endif /* CONFIG_SCHED_INSTRUMENTATION */

src/modules/systemlib/cpuload.h

@@ -37,13 +37,15 @@
 
 __BEGIN_DECLS
 
-#include <nuttx/sched.h>
+#include <sched.h>
 
 struct system_load_taskinfo_s {
 	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
 	uint64_t curr_start_time;		///< Start time of the current scheduling slot
 	uint64_t start_time;			///< FIRST start time of task
+#ifdef __PX4_NUTTX
 	FAR struct tcb_s *tcb;			///<
+#endif
 	bool valid;						///< Task is currently active / valid
 };
 

src/modules/systemlib/err.c

@@ -38,7 +38,7 @@
  * the same names.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdlib.h>
 #include <errno.h>

src/modules/systemlib/err.h

@@ -65,6 +65,7 @@
 #ifndef _SYSTEMLIB_ERR_H
 #define _SYSTEMLIB_ERR_H
 
+#include <px4_log.h>
 #include <stdarg.h>
 #include "visibility.h"
 
@@ -72,6 +73,14 @@ __BEGIN_DECLS
 
 __EXPORT const char *getprogname(void);
 
+#ifdef __PX4_POSIX
+
+#define err(...)					ERROR
+#define errx(...)					ERROR
+#define warn(...) 					PX4_WARN(__VA_ARGS__)
+#define warnx(...) 					PX4_WARN(__VA_ARGS__)
+
+#else
 __EXPORT void	err(int eval, const char *fmt, ...)		__attribute__((noreturn, format(printf, 2, 3)));
 __EXPORT void	verr(int eval, const char *fmt, va_list)	__attribute__((noreturn, format(printf, 2, 0)));
 __EXPORT void	errc(int eval, int code, const char *fmt, ...)	__attribute__((noreturn, format(printf, 3, 4)));
@@ -84,6 +93,7 @@ __EXPORT void	warnc(int code, const char *fmt, ...)		__attribute__((format(print
 __EXPORT void	vwarnc(int code, const char *fmt, va_list)	__attribute__((format(printf, 2, 0)));
 __EXPORT void	warnx(const char *fmt, ...)			__attribute__((format(printf, 1, 2)));
 __EXPORT void	vwarnx(const char *fmt, va_list)		__attribute__((format(printf, 1, 0)));
+#endif
 
 __END_DECLS
 

src/modules/systemlib/mavlink_log.c

@@ -39,6 +39,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <px4_posix.h>
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>
@@ -124,5 +125,5 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...)
 	char text[MAVLINK_LOG_MAXLEN + 1];
 	vsnprintf(text, sizeof(text), fmt, ap);
 	va_end(ap);
-	ioctl(_fd, severity, (unsigned long)&text[0]);
+	px4_ioctl(_fd, severity, (unsigned long)&text[0]);
 }

src/modules/systemlib/mcu_version.c

@@ -42,7 +42,8 @@
 
 #include "mcu_version.h"
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_defines.h>
 
 #ifdef CONFIG_ARCH_CHIP_STM32
 #include <up_arch.h>

src/modules/systemlib/mixer/mixer.cpp

@@ -37,7 +37,7 @@
  * Programmable multi-channel mixer library.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <sys/types.h>
 #include <stdint.h>

src/modules/systemlib/mixer/mixer.h

@@ -128,7 +128,7 @@
 #ifndef _SYSTEMLIB_MIXER_MIXER_H
 #define _SYSTEMLIB_MIXER_MIXER_H value
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include "drivers/drv_mixer.h"
 
 #include <uORB/topics/multirotor_motor_limits.h>

src/modules/systemlib/mixer/mixer_group.cpp

@@ -37,7 +37,7 @@
  * Mixer collection.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <sys/types.h>
 #include <stdint.h>

src/modules/systemlib/mixer/mixer_load.c

@@ -37,7 +37,7 @@
  * Programmable multi-channel mixer library.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <string.h>
 #include <stdio.h>
 #include <ctype.h>

src/modules/systemlib/mixer/mixer_load.h

@@ -40,7 +40,7 @@
 #ifndef _SYSTEMLIB_MIXER_LOAD_H
 #define _SYSTEMLIB_MIXER_LOAD_H value
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 __BEGIN_DECLS