Linux: added builtins to show devices and topics

list_devices will list virtual devices starting with "/dev/".
list_topics will list topics ("/obj/")

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

6 files changed, 35 insertions, 746 deletions

diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp
index e5004b684..0d02d2b42 100644
--- a/src/drivers/device/vdev.cpp
+++ b/src/drivers/device/vdev.cpp
@@ -461,16 +461,35 @@ void VDev::showDevices()
 	int i=0;
 	printf("Devices:\n");
 	for (; i<PX4_MAX_DEV; ++i) {
-		if (devmap[i]) {
+		if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
 			printf("   %s\n", devmap[i]->name);
 		}
 	}
 }
 
+void VDev::showTopics()
+{
+	int i=0;
+	printf("Devices:\n");
+	for (; i<PX4_MAX_DEV; ++i) {
+		if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
+			printf("   %s\n", devmap[i]->name);
+		}
+	}
+}
+
+const char *VDev::topicList(unsigned int *next)
+{
+	for (;*next<PX4_MAX_DEV; (*next)++)
+		if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0)
+			return devmap[(*next)++]->name;
+	return NULL;
+}
+
 const char *VDev::devList(unsigned int *next)
 {
 	for (;*next<PX4_MAX_DEV; (*next)++)
-		if (devmap[*next])
+		if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0)
 			return devmap[(*next)++]->name;
 	return NULL;
 }
diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h
index 1ce48285f..6448b8aa9 100644
--- a/src/drivers/device/vdev.h
+++ b/src/drivers/device/vdev.h
@@ -334,7 +334,9 @@ public:
 
 	static VDev *getDev(const char *path);
 	static void showDevices(void);
+	static void showTopics(void);
 	static const char *devList(unsigned int *next);
+	static const char *topicList(unsigned int *next);
 
 protected:
 
diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp
index 50df5535d..31f141267 100644
--- a/src/drivers/device/vdev_posix.cpp
+++ b/src/drivers/device/vdev_posix.cpp
@@ -274,10 +274,20 @@ void px4_show_devices()
 	VDev::showDevices();
 }
 
+void px4_show_topics()
+{
+	VDev::showTopics();
+}
+
 const char * px4_get_device_names(unsigned int *handle)
 {
 	return VDev::devList(handle);
 }
 
+const char * px4_get_topic_names(unsigned int *handle)
+{
+	return VDev::topicList(handle);
+}
+
 }
 
diff --git a/src/modules/commander/gyro_calibration_linux.cpp b/src/modules/commander/gyro_calibration_linux.cpp
deleted file mode 100644
index f490487f3..000000000
--- a/src/modules/commander/gyro_calibration_linux.cpp
+++ /dev/null
@@ -1,274 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}
diff --git a/src/modules/commander/mag_calibration_linux.cpp b/src/modules/commander/mag_calibration_linux.cpp
deleted file mode 100644
index 49ccbf3ef..000000000
--- a/src/modules/commander/mag_calibration_linux.cpp
+++ /dev/null
@@ -1,470 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}
diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h
index 2eff37f88..ed4edbb24 100644
--- a/src/platforms/px4_posix.h
+++ b/src/platforms/px4_posix.h
@@ -84,6 +84,8 @@ __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 void		px4_show_topics(void);
 __EXPORT const char *	px4_get_device_names(unsigned int *handle);
+__EXPORT const char *	px4_get_topic_names(unsigned int *handle);
 
 __END_DECLS