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>

makefiles/linux/config_linux_default.mk

@@ -46,6 +46,7 @@ MODULES		+= modules/uORB
 MODULES		+= modules/dataman
 MODULES		+= modules/sdlog2
 MODULES		+= modules/simulator
+MODULES		+= modules/commander
 
 #
 # Libraries

src/drivers/device/vdev.cpp

@@ -467,5 +467,13 @@ void VDev::showDevices()
 	}
 }
 
+const char *VDev::devList(unsigned int *next)
+{
+	for (;*next<PX4_MAX_DEV; (*next)++)
+		if (devmap[*next])
+			return devmap[(*next)++]->name;
+	return NULL;
+}
+
 } // namespace device
 

src/drivers/device/vdev.h

@@ -334,6 +334,7 @@ public:
 
 	static VDev *getDev(const char *path);
 	static void showDevices(void);
+	static const char *devList(unsigned int *next);
 
 protected:
 

src/drivers/device/vdev_posix.cpp

@@ -274,5 +274,10 @@ void px4_show_devices()
 	VDev::showDevices();
 }
 
+const char * px4_get_device_names(unsigned int *handle)
+{
+	return VDev::devList(handle);
+}
+
 }
 

src/drivers/drv_led.h

@@ -52,9 +52,9 @@
 #define LED_BLUE		0
 #define LED_SAFETY		2
 
-#define LED_ON			_IOC(_LED_BASE, 0)
-#define LED_OFF			_IOC(_LED_BASE, 1)
-#define LED_TOGGLE		_IOC(_LED_BASE, 2)
+#define LED_ON			_PX4_IOC(_LED_BASE, 0)
+#define LED_OFF			_PX4_IOC(_LED_BASE, 1)
+#define LED_TOGGLE		_PX4_IOC(_LED_BASE, 2)
 
 __BEGIN_DECLS
 

src/drivers/drv_rgbled.h

@@ -50,7 +50,7 @@
  */
 
 #define _RGBLEDIOCBASE		(0x2900)
-#define _RGBLEDIOC(_n)		(_IOC(_RGBLEDIOCBASE, _n))
+#define _RGBLEDIOC(_n)		(_PX4_IOC(_RGBLEDIOCBASE, _n))
 
 /** play the named script in *(char *)arg, repeating forever */
 #define RGBLED_PLAY_SCRIPT_NAMED	_RGBLEDIOC(1)

src/drivers/drv_tone_alarm.h

@@ -65,7 +65,7 @@
 #define TONEALARM0_DEVICE_PATH "/dev/tone_alarm0"
 
 #define _TONE_ALARM_BASE	0x7400
-#define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
+#define TONE_SET_ALARM		_PX4_IOC(_TONE_ALARM_BASE, 1)
 
 /* structure describing one note in a tone pattern */
 struct tone_note {

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

src/platforms/px4_config.h

@@ -52,6 +52,9 @@
 /** time in ms between checks for work in work queues **/
 #define CONFIG_SCHED_WORKPERIOD 10
 
+#define CONFIG_SCHED_INSTRUMENTATION 1
+#define CONFIG_MAX_TASKS 32
+
 #define px4_errx(x, ...) errx(x, __VA_ARGS__)
 
 #endif

src/platforms/px4_posix.h

@@ -84,5 +84,6 @@ __EXPORT ssize_t	px4_write(int fd, const void *buffer, size_t buflen);
 __EXPORT int		px4_ioctl(int fd, int cmd, unsigned long arg);
 __EXPORT int		px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout);
 __EXPORT void		px4_show_devices(void);
+__EXPORT const char *	px4_get_device_names(unsigned int *handle);
 
 __END_DECLS