Linux: enabled commander module

The commander module now compiles for Linux.

state_machine_helper_linux.cpp iterates over the virtual devices vs
all devices under /dev as per NuttX when disabling publishing.

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

src/modules/commander/airspeed_calibration.cpp

@@ -41,6 +41,7 @@
 #include "commander_helper.h"
 
 #include <stdio.h>
+#include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
 #include <math.h>

src/modules/commander/calibration_routines.cpp

@@ -39,6 +39,7 @@
  */
 
 #include <stdio.h>
+#include <unistd.h>
 #include <math.h>
 #include <float.h>
 #include <poll.h>
@@ -53,7 +54,7 @@
 #include "commander_helper.h"
 
 // FIXME: Fix return codes
-static const int ERROR = -1;
+//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,

src/modules/commander/commander.cpp

@@ -53,7 +53,7 @@
 #include <errno.h>
 #include <systemlib/err.h>
 #include <systemlib/circuit_breaker.h>
-#include <debug.h>
+//#include <debug.h>
 #include <sys/prctl.h>
 #include <sys/stat.h>
 #include <string.h>

src/modules/commander/commander_helper.cpp

@@ -41,6 +41,7 @@
  *
  */
 
+#include <px4_defines.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <stdint.h>
@@ -110,7 +111,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,7 +131,7 @@ int buzzer_init()
 		return ERROR;
 	}
 
-	return OK;
+	return PX4_OK;
 }
 
 void buzzer_deinit()

src/modules/commander/gyro_calibration.cpp

@@ -42,6 +42,7 @@
 #include "commander_helper.h"
 
 #include <stdio.h>
+#include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
 #include <math.h>

src/modules/commander/gyro_calibration_linux.cpp

@@ -0,0 +1,274 @@
+/****************************************************************************
+ *
+ *   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 gyro_calibration.cpp
+ *
+ * Gyroscope calibration routine
+ */
+
+#include "gyro_calibration.h"
+#include "calibration_messages.h"
+#include "commander_helper.h"
+
+#include <px4_posix.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_gyro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static const char *sensor_name = "gyro";
+
+int do_gyro_calibration(int mavlink_fd)
+{
+	const unsigned max_gyros = 3;
+
+	int32_t device_id[3];
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "HOLD STILL");
+
+	/* wait for the user to respond */
+	sleep(2);
+
+	struct gyro_scale gyro_scale_zero = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	struct gyro_scale gyro_scale[max_gyros] = {};
+
+	int res = OK;
+
+	/* store board ID */
+	uint32_t mcu_id[3];
+	mcu_unique_id(&mcu_id[0]);
+
+	/* store last 32bit number - not unique, but unique in a given set */
+	(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
+
+	char str[30];
+
+	for (unsigned s = 0; s < max_gyros; s++) {
+
+		/* ensure all scale fields are initialized tha same as the first struct */
+		(void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
+
+		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+		/* reset all offsets to zero and all scales to one */
+		int fd = px4_open(str, 0);
+
+		if (fd < 0) {
+			continue;
+		}
+
+		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_FAILED_RESET_CAL_MSG, s);
+		}
+	}
+
+	unsigned calibration_counter[max_gyros] = { 0 };
+	const unsigned calibration_count = 5000;
+
+	struct gyro_report gyro_report_0 = {};
+
+	if (res == OK) {
+		/* determine gyro mean values */
+		unsigned poll_errcount = 0;
+
+		/* subscribe to gyro sensor topic */
+		int sub_sensor_gyro[max_gyros];
+		px4_pollfd_struct_t fds[max_gyros];
+
+		for (unsigned s = 0; s < max_gyros; s++) {
+			sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+			fds[s].fd = sub_sensor_gyro[s];
+			fds[s].events = POLLIN;
+		}
+
+		struct gyro_report gyro_report;
+
+		/* use first gyro to pace, but count correctly per-gyro for statistics */
+		while (calibration_counter[0] < calibration_count) {
+			/* wait blocking for new data */
+
+			int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
+
+			if (poll_ret > 0) {
+
+				for (unsigned s = 0; s < max_gyros; s++) {
+					bool changed;
+					orb_check(sub_sensor_gyro[s], &changed);
+
+					if (changed) {
+						orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
+
+						if (s == 0) {
+							orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
+						}
+
+						gyro_scale[s].x_offset += gyro_report.x;
+						gyro_scale[s].y_offset += gyro_report.y;
+						gyro_scale[s].z_offset += gyro_report.z;
+						calibration_counter[s]++;
+					}
+
+					if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
+						mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
+					}
+				}
+
+			} else {
+				poll_errcount++;
+			}
+
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+				res = ERROR;
+				break;
+			}
+		}
+
+		for (unsigned s = 0; s < max_gyros; s++) {
+			px4_close(sub_sensor_gyro[s]);
+
+			gyro_scale[s].x_offset /= calibration_counter[s];
+			gyro_scale[s].y_offset /= calibration_counter[s];
+			gyro_scale[s].z_offset /= calibration_counter[s];
+		}
+	}
+
+	if (res == OK) {
+		/* check offsets */
+		float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
+		float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
+		float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
+
+		/* maximum allowable calibration error in radians */
+		const float maxoff = 0.002f;
+
+		if (!isfinite(gyro_scale[0].x_offset) ||
+		    !isfinite(gyro_scale[0].y_offset) ||
+		    !isfinite(gyro_scale[0].z_offset) ||
+		    fabsf(xdiff) > maxoff ||
+		    fabsf(ydiff) > maxoff ||
+		    fabsf(zdiff) > maxoff) {
+			mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration");
+			res = ERROR;
+		}
+	}
+
+	if (res == OK) {
+		/* set offset parameters to new values */
+		bool failed = false;
+
+		for (unsigned s = 0; s < max_gyros; s++) {
+
+			/* if any reasonable amount of data is missing, skip */
+			if (calibration_counter[s] < calibration_count / 2) {
+				continue;
+			}
+
+			(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
+			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset)));
+			(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
+			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset)));
+			(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
+			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset)));
+			(void)sprintf(str, "CAL_GYRO%u_ID", s);
+			failed |= (OK != param_set(param_find(str), &(device_id[s])));
+
+			/* apply new scaling and offsets */
+			(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+			int fd = px4_open(str, 0);
+
+			if (fd < 0) {
+				failed = true;
+				continue;
+			}
+
+			res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
+			px4_close(fd);
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			}
+		}
+
+		if (failed) {
+			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
+			res = ERROR;
+		}
+	}
+
+	if (res == OK) {
+		/* auto-save to EEPROM */
+		res = param_save_default();
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+		}
+	}
+
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+
+	} else {
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+	}
+
+	return res;
+}

src/modules/commander/mag_calibration_linux.cpp

@@ -0,0 +1,470 @@
+/****************************************************************************
+ *
+ *   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 mag_calibration.cpp
+ *
+ * Magnetometer calibration routine
+ */
+
+#include "mag_calibration.h"
+#include "commander_helper.h"
+#include "calibration_routines.h"
+#include "calibration_messages.h"
+
+#include <px4_posix.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_mag.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static const char *sensor_name = "mag";
+static const unsigned max_mags = 3;
+
+int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
+int mag_calibration_worker(detect_orientation_return orientation, void* worker_data);
+
+/// Data passed to calibration worker routine
+typedef struct  {
+	int		mavlink_fd;
+	unsigned	done_count;
+	int		sub_mag[max_mags];
+	unsigned int	calibration_points_perside;
+	unsigned int	calibration_interval_perside_seconds;
+	uint64_t	calibration_interval_perside_useconds;
+	unsigned int	calibration_counter_total;
+	bool		side_data_collected[detect_orientation_side_count];
+	float*		x[max_mags];
+	float*		y[max_mags];
+	float*		z[max_mags];
+} mag_worker_data_t;
+
+
+int do_mag_calibration(int mavlink_fd)
+{
+	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	int result = OK;
+	
+	// Determine which mags are available and reset each
+
+	int32_t	device_ids[max_mags];
+	char str[30];
+
+	for (size_t i=0; i<max_mags; i++) {
+		device_ids[i] = 0; // signals no mag
+	}
+	
+	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+		// Reset mag id to mag not available
+		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
+		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
+		if (result != OK) {
+			mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag);
+			break;
+		}
+
+		// Attempt to open mag
+		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
+		int fd = px4_open(str, O_RDONLY);
+		if (fd < 0) {
+			continue;
+		}
+
+		// Get device id for this mag
+		device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+
+		// Reset mag scale
+		result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
+
+		if (result != OK) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
+		}
+
+		if (result == OK) {
+			/* calibrate range */
+			result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
+
+			if (result != OK) {
+				mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
+				/* this is non-fatal - mark it accordingly */
+				result = OK;
+			}
+		}
+
+		px4_close(fd);
+	}
+
+	if (result == OK) {
+		// Calibrate all mags at the same time
+		result = mag_calibrate_all(mavlink_fd, device_ids);
+	}
+	
+	if (result == OK) {
+		/* auto-save to EEPROM */
+		result = param_save_default();
+		if (result != OK) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+		}
+	}
+	
+	if (result == OK) {
+		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+	} else {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+	}
+
+	return result;
+}
+
+int mag_calibration_worker(detect_orientation_return orientation, void* data)
+{
+	int result = OK;
+	
+	unsigned int calibration_counter_side;
+
+	mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
+	
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
+	sleep(2);
+	
+	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
+	unsigned poll_errcount = 0;
+	
+	calibration_counter_side = 0;
+	
+	while (hrt_absolute_time() < calibration_deadline &&
+	       calibration_counter_side < worker_data->calibration_points_perside) {
+		
+		// Wait clocking for new data on all 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) {
+				fds[fd_count].fd = worker_data->sub_mag[cur_mag];
+				fds[fd_count].events = POLLIN;
+				fd_count++;
+			}
+		}
+		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++) {
+				if (worker_data->sub_mag[cur_mag] >= 0) {
+					struct mag_report mag;
+
+					orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
+					
+					worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
+					worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
+					worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
+					
+				}
+			}
+			
+			worker_data->calibration_counter_total++;
+			calibration_counter_side++;
+			
+			// Progress indicator for side
+			mavlink_and_console_log_info(worker_data->mavlink_fd,
+						     "%s %s side calibration: progress <%u>",
+						     sensor_name,
+						     detect_orientation_str(orientation),
+						     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+		} else {
+			poll_errcount++;
+		}
+		
+		if (poll_errcount > worker_data->calibration_points_perside * 3) {
+			result = ERROR;
+			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			break;
+		}
+	}
+	
+	// Mark the opposite side as collected as well. No need to collect opposite side since it
+	// would generate similar points.
+	detect_orientation_return alternateOrientation = orientation;
+	switch (orientation) {
+		case DETECT_ORIENTATION_TAIL_DOWN:
+			alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN;
+			break;
+		case DETECT_ORIENTATION_NOSE_DOWN:
+			alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN;
+			break;
+		case DETECT_ORIENTATION_LEFT:
+			alternateOrientation = DETECT_ORIENTATION_RIGHT;
+			break;
+		case DETECT_ORIENTATION_RIGHT:
+			alternateOrientation = DETECT_ORIENTATION_LEFT;
+			break;
+		case DETECT_ORIENTATION_UPSIDE_DOWN:
+			alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP;
+			break;
+		case DETECT_ORIENTATION_RIGHTSIDE_UP:
+			alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN;
+			break;
+		case DETECT_ORIENTATION_ERROR:
+			warnx("Invalid orientation in mag_calibration_worker");
+			break;
+	}
+	worker_data->side_data_collected[alternateOrientation] = true;
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation));
+	
+	worker_data->done_count++;
+	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
+	
+	return result;
+}
+
+int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
+{
+	int result = OK;
+
+	mag_worker_data_t worker_data;
+	
+	worker_data.mavlink_fd = mavlink_fd;
+	worker_data.done_count = 0;
+	worker_data.calibration_counter_total = 0;
+	worker_data.calibration_points_perside = 80;
+	worker_data.calibration_interval_perside_seconds = 20;
+	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
+
+	// Initialize to collect all sides
+	for (size_t cur_side=0; cur_side<6; cur_side++) {
+		worker_data.side_data_collected[cur_side] = false;
+	}
+	
+	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		// Initialize to no subscription
+		worker_data.sub_mag[cur_mag] = -1;
+		
+		// Initialize to no memory allocated
+		worker_data.x[cur_mag] = NULL;
+		worker_data.y[cur_mag] = NULL;
+		worker_data.z[cur_mag] = NULL;
+	}
+
+	const unsigned int calibration_sides = 3;
+	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
+	
+	char str[30];
+	
+	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
+			mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
+			result = ERROR;
+		}
+	}
+
+	
+	// Setup subscriptions to mag sensors
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				// Mag in this slot is available
+				worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
+				if (worker_data.sub_mag[cur_mag] < 0) {
+					mavlink_and_console_log_critical(mavlink_fd, "Mag #%u not found, abort", cur_mag);
+					result = ERROR;
+					break;
+				}
+			}
+		}
+	}
+	
+	// Limit update rate to get equally spaced measurements over time (in ms)
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				// Mag in this slot is available
+				unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
+				
+				//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
+				orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
+			}
+		}
+		
+	}
+
+	result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
+	
+	// Close subscriptions
+	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		if (worker_data.sub_mag[cur_mag] >= 0) {
+			px4_close(worker_data.sub_mag[cur_mag]);
+		}
+	}
+	
+	// Calculate calibration values for each mag
+	
+	
+	float sphere_x[max_mags];
+	float sphere_y[max_mags];
+	float sphere_z[max_mags];
+	float sphere_radius[max_mags];
+	
+	// Sphere fit the data to get calibration values
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				// Mag in this slot is available and we should have values for it to calibrate
+				
+				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
+							 worker_data.calibration_counter_total,
+							 100, 0.0f,
+							 &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])) {
+					mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
+					result = ERROR;
+				}
+			}
+		}
+	}
+	
+	// Data points are no longer needed
+	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		free(worker_data.x[cur_mag]);
+		free(worker_data.y[cur_mag]);
+		free(worker_data.z[cur_mag]);
+	}
+	
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				int fd_mag = -1;
+				struct mag_scale mscale;
+				
+				// Set new scale
+				
+				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
+				fd_mag = px4_open(str, 0);
+				if (fd_mag < 0) {
+					mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag);
+					result = ERROR;
+				}
+				
+				if (result == OK) {
+					result = px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale);
+					if (result != OK) {
+						mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag);
+						result = ERROR;
+					}
+				}
+
+				if (result == OK) {
+					mscale.x_offset = sphere_x[cur_mag];
+					mscale.y_offset = sphere_y[cur_mag];
+					mscale.z_offset = sphere_z[cur_mag];
+
+					result = px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
+					if (result != OK) {
+						mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag);
+						result = ERROR;
+					}
+				}
+				
+				// Mag device no longer needed
+				if (fd_mag >= 0) {
+					px4_close(fd_mag);
+				}
+
+				if (result == OK) {
+					bool failed = false;
+					
+					/* set parameters */
+					(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
+					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
+					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
+					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
+					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
+					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
+					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
+
+					if (failed) {
+						mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag);
+						result = ERROR;
+					} else {
+						mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
+									     cur_mag,
+									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
+						mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f",
+									     cur_mag,
+									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
+					}
+				}
+			}
+		}
+	}
+
+	return result;
+}

src/modules/commander/module.mk

@@ -38,17 +38,25 @@
 MODULE_COMMAND	 	= commander
 SRCS		 	= commander.cpp \
 			commander_params.c \
-			state_machine_helper.cpp \
 			commander_helper.cpp \
 			calibration_routines.cpp \
-			accelerometer_calibration.cpp \
-			gyro_calibration.cpp \
-			mag_calibration.cpp \
 			baro_calibration.cpp \
+			accelerometer_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp \
 			PreflightCheck.cpp
 
+ifdef ($(PX4_TARGET_OS),nuttx)
+SRCS += 
+			state_machine_helper.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp 
+else
+SRCS +=			state_machine_helper_linux.cpp \
+			gyro_calibration_linux.cpp \
+			mag_calibration_linux.cpp 
+endif
+
 MODULE_STACKSIZE = 5000
 
 MAXOPTIMIZATION	 = -Os

src/modules/commander/state_machine_helper_linux.cpp

@@ -0,0 +1,675 @@
+/****************************************************************************
+ *
+ *   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 <uORB/topics/airspeed.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_airspeed.h>
+#include <drivers/drv_device.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+#include "commander_helper.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,       false,         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,    false, 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) {
+			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) {
+			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+			feedback_provided = true;
+			valid_transition = false;
+		}
+
+		// 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)
+{
+	int ret;
+	bool failed = false;
+
+	int fd = px4_open(ACCEL0_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
+		failed = true;
+		goto system_eval;
+	}
+
+	ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
+		failed = true;
+		goto system_eval;
+	}
+
+	/* check measurement result range */
+	struct accel_report acc;
+	ret = px4_read(fd, &acc, sizeof(acc));
+
+	if (ret == sizeof(acc)) {
+		/* evaluate values */
+		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+
+		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
+			/* this is frickin' fatal */
+			failed = true;
+			goto system_eval;
+		}
+	} else {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
+		/* this is frickin' fatal */
+		failed = true;
+		goto system_eval;
+	}
+
+	/* 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) {
+		/* accel done, close it */
+		px4_close(fd);
+		fd = orb_subscribe(ORB_ID(airspeed));
+
+		struct airspeed_s airspeed;
+
+		if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
+			(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
+			failed = true;
+			goto system_eval;
+		}
+
+		if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
+			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+			// XXX do not make this fatal yet
+		}
+	}
+
+system_eval:
+	px4_close(fd);
+	return (failed);
+}

src/modules/systemlib/cpuload.c

@@ -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
 };