src/platforms/posix: move to platforms/posix/src/px4/common and src/drivers/driver_framework_wrapper

src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_ak8963_wrapper
+	MAIN df_ak8963_wrapper
+	SRCS
+		df_ak8963_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_ak8963
+	)

src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp

@@ -0,0 +1,459 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_ak8963_wrapper.cpp
+ * Lightweight driver to access the AK8963 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_mag.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <board_config.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <ak8963/AK8963.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_ak8963_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfAK8963Wrapper : public AK8963
+{
+public:
+	DfAK8963Wrapper(enum Rotation rotation);
+	~DfAK8963Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+	void print_calibration();
+
+private:
+	int _publish(struct mag_sensor_data &data);
+
+	void _update_mag_calibration();
+
+	orb_advert_t		_mag_topic;
+
+	int			_param_update_sub;
+
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
+	matrix::Dcmf	_rotation_matrix;
+
+	int			_mag_orb_class_instance;
+
+	perf_counter_t		_mag_sample_perf;
+
+};
+
+DfAK8963Wrapper::DfAK8963Wrapper(enum Rotation rotation) :
+	AK8963(MAG_DEVICE_PATH),
+	_mag_topic(nullptr),
+	_param_update_sub(-1),
+	_mag_calibration{},
+	_mag_orb_class_instance(-1),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
+{
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+
+	// Get sensor rotation matrix
+	_rotation_matrix = get_rot_matrix(rotation);
+}
+
+DfAK8963Wrapper::~DfAK8963Wrapper()
+{
+	perf_free(_mag_sample_perf);
+}
+
+int DfAK8963Wrapper::start()
+{
+	/* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("AK8963 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = AK8963::start();
+
+	if (ret != 0) {
+		PX4_ERR("AK8963 start fail: %d", ret);
+		return ret;
+	}
+
+	/* Force getting the calibration values. */
+	_update_mag_calibration();
+
+	return 0;
+}
+
+int DfAK8963Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = AK8963::stop();
+
+	if (ret != 0) {
+		PX4_ERR("AK8963 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfAK8963Wrapper::_update_mag_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+	}
+}
+
+void DfAK8963Wrapper::print_calibration()
+{
+	PX4_INFO("calibration x_offset: %f", (double)_mag_calibration.x_offset);
+	PX4_INFO("calibration x_scale: %f", (double)_mag_calibration.x_scale);
+	PX4_INFO("calibration y_offset: %f", (double)_mag_calibration.y_offset);
+	PX4_INFO("calibration y_scale: %f", (double)_mag_calibration.y_scale);
+	PX4_INFO("calibration z_offset: %f", (double)_mag_calibration.z_offset);
+	PX4_INFO("calibration z_scale: %f", (double)_mag_calibration.z_scale);
+}
+
+int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_mag_calibration();
+	}
+
+	/* Publish mag first. */
+	perf_begin(_mag_sample_perf);
+
+	mag_report mag_report = {};
+	mag_report.timestamp = hrt_absolute_time();
+	mag_report.is_external = true;
+
+	// TODO: remove these (or get the values)
+	mag_report.x_raw = 0;
+	mag_report.y_raw = 0;
+	mag_report.z_raw = 0;
+
+	matrix::Vector3f mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga);
+
+	// apply sensor rotation on the accel measurement
+	mag_val = _rotation_matrix * mag_val;
+
+	mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+	mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+	mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+
+
+	// TODO: get these right
+	//mag_report.scaling = -1.0f;
+
+	mag_report.device_id = m_id.dev_id;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_mag_topic == nullptr) {
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+							 &_mag_orb_class_instance, ORB_PRIO_HIGH);
+
+		} else {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
+	}
+
+	perf_end(_mag_sample_perf);
+
+	return 0;
+};
+
+
+namespace df_ak8963_wrapper
+{
+
+DfAK8963Wrapper *g_dev = nullptr;
+
+int start(enum Rotation rotation);
+int stop();
+int info();
+void usage();
+
+int start(enum Rotation rotation)
+{
+	g_dev = new DfAK8963Wrapper(rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfAK8963Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfAK8963Wrapper start failed");
+		return ret;
+	}
+
+	// Open the MAG sensor
+	DevHandle h;
+	DevMgr::getHandle(MAG_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    MAG_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	g_dev->print_calibration();
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_ak8963_wrapper 'start', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_ak8963_wrapper
+
+
+int
+df_ak8963_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			df_ak8963_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_ak8963_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_ak8963_wrapper::start(rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_ak8963_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_ak8963_wrapper::info();
+	}
+
+	else {
+		df_ak8963_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_bebop_bus_wrapper
+	MAIN df_bebop_bus_wrapper
+	SRCS
+		df_bebop_bus_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_bebop_bus
+	)

src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp

@@ -0,0 +1,627 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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 df_bebop_bus_wrapper.cpp
+ *
+ * This is a wrapper around the Parrot Bebop bus driver of the DriverFramework. It sends the
+ * motor and contol commands to the Bebop and reads its status and informations.
+ */
+
+#include <stdlib.h>
+#include <stdint.h>
+
+#include <px4_tasks.h>
+#include <px4_getopt.h>
+#include <px4_posix.h>
+
+#include <errno.h>
+#include <string.h>
+#include <math.h>
+
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/esc_status.h>
+
+#include <lib/mixer/mixer.h>
+#include <lib/mixer/mixer_load.h>
+#include <battery/battery.h>
+
+#include <bebop_bus/BebopBus.hpp>
+#include <DevMgr.hpp>
+
+extern "C" { __EXPORT int df_bebop_bus_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+class DfBebopBusWrapper : public BebopBus
+{
+public:
+	DfBebopBusWrapper();
+	~DfBebopBusWrapper() = default;
+
+	/// Start and initialize the driver
+	int start();
+
+	/// Stop the driver
+	int stop();
+
+	/// Print various infos (version, type, flights, errors
+	int print_info();
+
+	/// Start the motors
+	int start_motors();
+
+	/// Stop the motors
+	int stop_motors();
+
+	/// Reset pending errors on the Bebop hardware
+	int clear_errors();
+
+	/// Set the ESC speeds [front left, front right, back right, back left]
+	int set_esc_speeds(const float speed_scaled[4]);
+
+	/// Capture the last throttle value for the battey computation
+	void set_last_throttle(float throttle) {_last_throttle = throttle;};
+
+private:
+	orb_advert_t _battery_topic;
+	orb_advert_t _esc_topic;
+
+	Battery _battery;
+	bool _armed;
+	float _last_throttle;
+
+	int _battery_orb_class_instance;
+
+	// map for bebop motor index to PX4 motor index
+	const uint8_t _esc_map[4] = {0, 2, 3, 1};
+
+	int _publish(struct bebop_state_data &data);
+};
+
+DfBebopBusWrapper::DfBebopBusWrapper() :
+	BebopBus(BEBOP_BUS_DEVICE_PATH), _battery_topic(nullptr), _esc_topic(nullptr), _battery(), _armed(false),
+	_last_throttle(0.0f),
+	_battery_orb_class_instance(-1)
+{}
+
+int DfBebopBusWrapper::start()
+{
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("BebopBus init fail: %d", ret);
+		return ret;
+	}
+
+	ret = BebopBus::start();
+
+	if (ret < 0) {
+		PX4_ERR("BebopBus start fail: %d", ret);
+		return ret;
+	}
+
+	// Signal bus start on Bebop
+	BebopBus::_play_sound(BebopBus::BOOT);
+
+	return 0;
+}
+
+int DfBebopBusWrapper::stop()
+{
+
+	int ret = BebopBus::stop();
+
+	if (ret < 0) {
+		PX4_ERR("BebopBus stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfBebopBusWrapper::print_info()
+{
+	bebop_bus_info info;
+	int ret = _get_info(&info);
+
+	if (ret < 0) {
+		return -1;
+	}
+
+	PX4_INFO("Bebop Controller Info");
+	PX4_INFO("  Software Version: %d.%d", info.version_major, info.version_minor);
+	PX4_INFO("  Software Type: %d", info.type);
+	PX4_INFO("  Number of controlled motors: %d", info.n_motors_controlled);
+	PX4_INFO("  Number of flights: %d", info.n_flights);
+	PX4_INFO("  Last flight time: %d", info.last_flight_time);
+	PX4_INFO("  Total flight time: %d", info.total_flight_time);
+	PX4_INFO("  Last Error: %d\n", info.last_error);
+
+	return 0;
+}
+
+int DfBebopBusWrapper::start_motors()
+{
+	_armed = true;
+	return BebopBus::_start_motors();
+}
+
+int DfBebopBusWrapper::stop_motors()
+{
+	_armed = false;
+	return BebopBus::_stop_motors();
+}
+
+int DfBebopBusWrapper::clear_errors()
+{
+	return BebopBus::_clear_errors();
+}
+
+int DfBebopBusWrapper::set_esc_speeds(const float speed_scaled[4])
+{
+	return BebopBus::_set_esc_speed(speed_scaled);
+}
+
+int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
+{
+
+	battery_status_s battery_report;
+	const hrt_abstime timestamp = hrt_absolute_time();
+
+	// TODO Check if this is the right way for the Bebop
+	// We don't have current measurements
+	_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed,
+				     &battery_report);
+
+	esc_status_s esc_status = {};
+
+	uint16_t esc_speed_setpoint_rpm[4] = {};
+	BebopBus::_get_esc_speed_setpoint(esc_speed_setpoint_rpm);
+	esc_status.timestamp = hrt_absolute_time();
+	esc_status.esc_count = 4;
+
+	for (int i = 0; i < 4; i++) {
+		esc_status.esc[_esc_map[i]].timestamp = esc_status.timestamp;
+		esc_status.esc[_esc_map[i]].esc_rpm = data.rpm[i];
+		esc_status.esc[_esc_map[i]].esc_setpoint_raw = esc_speed_setpoint_rpm[i];
+	}
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_battery_topic == nullptr) {
+			_battery_topic = orb_advertise_multi(ORB_ID(battery_status), &battery_report,
+							     &_battery_orb_class_instance, ORB_PRIO_LOW);
+
+		} else {
+			orb_publish(ORB_ID(battery_status), _battery_topic, &battery_report);
+		}
+
+		if (_esc_topic == nullptr) {
+			_esc_topic = orb_advertise(ORB_ID(esc_status), &esc_status);
+
+		} else {
+			orb_publish(ORB_ID(esc_status), _esc_topic, &esc_status);
+		}
+
+	}
+
+	return 0;
+}
+
+namespace df_bebop_bus_wrapper
+{
+
+DfBebopBusWrapper *g_dev = nullptr;      // interface to the Bebop's I2C device
+volatile bool _task_should_exit = false; // flag indicating if bebop esc control task should exit
+static bool _is_running = false;         // flag indicating if bebop esc  app is running
+static bool _motors_running = false;     // flag indicating if the motors are running
+static px4_task_t _task_handle = -1;     // handle to the task main thread
+
+static const char *MIXER_FILENAME = "/home/root/bebop.main.mix";
+
+// subscriptions
+int     _controls_sub;
+int     _armed_sub;
+
+// publications
+orb_advert_t    _outputs_pub = nullptr;
+orb_advert_t    _rc_pub = nullptr;
+
+// topic structures
+actuator_controls_s _controls[1];
+actuator_outputs_s  _outputs;
+actuator_armed_s    _armed;
+
+MixerGroup *_mixers = nullptr;
+
+int start();
+int stop();
+int info();
+int clear_errors();
+void usage();
+void task_main(int argc, char *argv[]);
+
+/* mixers initialization */
+int initialize_mixers(const char *mixers_filename);
+int mixers_control_callback(uintptr_t handle, uint8_t control_group,
+			    uint8_t control_index, float &input);
+
+int mixers_control_callback(uintptr_t handle,
+			    uint8_t control_group,
+			    uint8_t control_index,
+			    float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls[control_group].control[control_index];
+
+	return 0;
+}
+
+int initialize_mixers(const char *mixers_filename)
+{
+	char buf[2048] = {0};
+	size_t buflen = sizeof(buf);
+
+	PX4_INFO("Trying to initialize mixers from config file %s", mixers_filename);
+
+	if (load_mixer_file(mixers_filename, &buf[0], sizeof(buf)) < 0) {
+		PX4_ERR("can't load mixer: %s", mixers_filename);
+		return -1;
+	}
+
+	if (_mixers == nullptr) {
+		_mixers = new MixerGroup(mixers_control_callback, (uintptr_t)_controls);
+	}
+
+	if (_mixers == nullptr) {
+		PX4_ERR("No mixers available");
+		return -1;
+
+	} else {
+		int ret = _mixers->load_from_buf(buf, buflen);
+
+		if (ret != 0) {
+			PX4_ERR("Unable to parse mixers file");
+			delete _mixers;
+			_mixers = nullptr;
+			ret = -1;
+		}
+
+		return 0;
+	}
+}
+
+void task_main(int argc, char *argv[])
+{
+	_is_running = true;
+
+	// Subscribe for orb topics
+	_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	// Start disarmed
+	_armed.armed = false;
+	_armed.prearmed = false;
+	_motors_running = false;
+
+	// Set up poll topic
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd     = _controls_sub;
+	fds[0].events = POLLIN;
+	/* Don't limit poll intervall for now, 250 Hz should be fine. */
+	//orb_set_interval(_controls_sub, 10);
+
+	// Set up mixers
+	if (initialize_mixers(MIXER_FILENAME) < 0) {
+		PX4_ERR("Mixer initialization failed.");
+		return;
+	}
+
+	// Main loop
+	while (!_task_should_exit) {
+
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
+
+		/* Timed out, do a periodic check for _task_should_exit. */
+		if (pret == 0) {
+			continue;
+		}
+
+		/* This is undesirable but not much we can do. */
+		if (pret < 0) {
+			PX4_WARN("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls[0]);
+
+			_outputs.timestamp = _controls[0].timestamp;
+
+
+			if (_mixers != nullptr) {
+				/* do mixing */
+				_outputs.noutputs = _mixers->mix(_outputs.output, 4);
+			}
+
+			// Set last throttle for battery calculations
+			g_dev->set_last_throttle(_controls[0].control[3]);
+
+			/* disable unused ports by setting their output to NaN */
+			for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
+			     i++) {
+				_outputs.output[i] = NAN;
+			}
+
+			// Check if the outputs are in range and rescale
+			for (size_t i = 0; i < _outputs.noutputs; ++i) {
+				if (i < _outputs.noutputs &&
+				    PX4_ISFINITE(_outputs.output[i]) &&
+				    _outputs.output[i] >= -1.0f &&
+				    _outputs.output[i] <= 1.0f) {
+					/* scale for Bebop output 0.0 - 1.0 */
+					_outputs.output[i] = (_outputs.output[i] + 1.0f) / 2.0f;
+
+				} else {
+					/*
+					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * This will be clearly visible on the servo status and will limit the risk of accidentally
+					 * spinning motors. It would be deadly in flight.
+					 */
+					_outputs.output[i] = 0.0;
+				}
+			}
+
+			// Adjust order of BLDCs from PX4 to Bebop
+			float motor_out[4];
+			motor_out[0] = _outputs.output[2];
+			motor_out[1] = _outputs.output[0];
+			motor_out[2] = _outputs.output[3];
+			motor_out[3] = _outputs.output[1];
+
+			g_dev->set_esc_speeds(motor_out);
+
+			if (_outputs_pub != nullptr) {
+				orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
+
+			} else {
+				_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
+			}
+		}
+
+		bool updated;
+		orb_check(_armed_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+		}
+
+		const bool lockdown = _armed.manual_lockdown || _armed.lockdown || _armed.force_failsafe;
+
+		// Start the motors if armed but not alreay running
+		if (_armed.armed && !lockdown && !_motors_running) {
+			g_dev->start_motors();
+			_motors_running = true;
+		}
+
+		// Stop motors if not armed or killed, but running
+		if ((!_armed.armed || lockdown) && _motors_running) {
+			g_dev->stop_motors();
+			_motors_running = false;
+		}
+	}
+
+	orb_unsubscribe(_controls_sub);
+	orb_unsubscribe(_armed_sub);
+
+	_is_running = false;
+
+}
+
+int start()
+{
+	g_dev = new DfBebopBusWrapper();
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfBebopBusWrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfBebopBusWrapper start failed");
+		return ret;
+	}
+
+	// Open the Bebop dirver
+	DevHandle h;
+	DevMgr::getHandle(BEBOP_BUS_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    BEBOP_BUS_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	// Start the task to forward the motor control commands
+	_task_handle = px4_task_spawn_cmd("bebop_bus_esc_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_MAX,
+					  2000,
+					  (px4_main_t)&task_main,
+					  nullptr);
+
+	if (_task_handle < 0) {
+		PX4_ERR("task start failed");
+		return -1;
+	}
+
+	_is_running = true;
+	return 0;
+}
+
+int stop()
+{
+	// Stop bebop motor control task
+	_task_should_exit = true;
+
+	while (_is_running) {
+		usleep(200000);
+		PX4_INFO(".");
+	}
+
+	_task_handle = -1;
+
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	// Stop DF device
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	int ret = g_dev->print_info();
+
+	if (ret != 0) {
+		PX4_ERR("Unable to print info");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * Clear errors present on the Bebop hardware
+ */
+int
+clear_errors()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	int ret = g_dev->clear_errors();
+
+	if (ret != 0) {
+		PX4_ERR("Unable to clear errors");
+		return ret;
+	}
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_bebop_bus_wrapper 'start', 'info', 'clear_errors', 'stop'");
+}
+
+} /* df_bebop_bus_wrapper */
+
+
+int
+df_bebop_bus_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		df_bebop_bus_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_bebop_bus_wrapper::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_bebop_bus_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_bebop_bus_wrapper::info();
+	}
+
+	else if (!strcmp(verb, "clear_errors")) {
+		ret = df_bebop_bus_wrapper::clear_errors();
+	}
+
+	else {
+		df_bebop_bus_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_bebop_rangefinder_wrapper
+	MAIN df_bebop_rangefinder_wrapper
+	SRCS
+		df_bebop_rangefinder_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_bebop_rangefinder
+	)

src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp

@@ -0,0 +1,308 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_bebop_rangefinder_wrapper.cpp
+ * Driver to access the Bebop rangefinder of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+#include <string>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_range_finder.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+#include <bebop_rangefinder/BebopRangeFinder.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_bebop_rangefinder_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfBebopRangeFinderWrapper : public BebopRangeFinder
+{
+public:
+	DfBebopRangeFinderWrapper();
+	~DfBebopRangeFinderWrapper() = default;
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct bebop_range &data);
+
+	orb_advert_t		_range_topic;
+
+	int			_orb_class_instance;
+
+	// perf_counter_t		_range_sample_perf;
+
+};
+
+DfBebopRangeFinderWrapper::DfBebopRangeFinderWrapper(/*enum Rotation rotation*/) :
+	BebopRangeFinder(BEBOP_RANGEFINDER_DEVICE_PATH),
+	_range_topic(nullptr),
+	_orb_class_instance(-1)
+{
+}
+
+int DfBebopRangeFinderWrapper::start()
+{
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("BebopRangeFinder init fail: %d", ret);
+		return ret;
+	}
+
+	ret = BebopRangeFinder::start();
+
+	if (ret != 0) {
+		PX4_ERR("BebopRangeFinder start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfBebopRangeFinderWrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = BebopRangeFinder::stop();
+
+	if (ret != 0) {
+		PX4_ERR("BebopRangeFinder stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfBebopRangeFinderWrapper::_publish(struct bebop_range &data)
+{
+	struct distance_sensor_s distance_data;
+
+	memset(&distance_data, 0, sizeof(distance_sensor_s));
+
+	distance_data.timestamp = hrt_absolute_time();
+	distance_data.min_distance = float(BEBOP_RANGEFINDER_MIN_DISTANCE_M); /* m */
+	distance_data.max_distance = float(BEBOP_RANGEFINDER_MAX_DISTANCE_M); /* m */
+
+	distance_data.current_distance = float(data.height_m);
+
+	distance_data.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
+
+	distance_data.id = 0; // TODO set proper ID
+
+	distance_data.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+	distance_data.variance = 1.0f; // TODO set correct value
+
+	distance_data.signal_quality = -1;
+
+	if (_range_topic == nullptr) {
+		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &distance_data,
+						   &_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _range_topic, &distance_data);
+	}
+
+	return 0;
+};
+
+
+namespace df_bebop_rangefinder_wrapper
+{
+
+DfBebopRangeFinderWrapper *g_dev = nullptr;
+
+int start();
+int stop();
+int info();
+void usage();
+
+int start()
+{
+	g_dev = new DfBebopRangeFinderWrapper();
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfBebopRangeFinderWrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfBebopRangeFinderWrapper start failed");
+		return ret;
+	}
+
+	// Open the range sensor
+	DevHandle h;
+	DevMgr::getHandle(BEBOP_RANGEFINDER_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    BEBOP_RANGEFINDER_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_bebop_rangefinder_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_bebop_rangefinder_wrapper
+
+
+int
+df_bebop_rangefinder_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		// Add rotation if necessary
+		default:
+			df_bebop_rangefinder_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_bebop_rangefinder_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_bebop_rangefinder_wrapper::start(/*rotation*/);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_bebop_rangefinder_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_bebop_rangefinder_wrapper::info();
+	}
+
+	else {
+		df_bebop_rangefinder_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_bmp280_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_bmp280_wrapper
+	MAIN df_bmp280_wrapper
+	SRCS
+		df_bmp280_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_bmp280
+	)

src/drivers/driver_framework_wrapper/df_bmp280_wrapper/df_bmp280_wrapper.cpp

@@ -0,0 +1,300 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_bmp280_wrapper.cpp
+ * Lightweight driver to access the BMP280 of the DriverFramework.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_baro.h>
+
+#include <board_config.h>
+//#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+#include <bmp280/BMP280.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_bmp280_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfBmp280Wrapper : public BMP280
+{
+public:
+	DfBmp280Wrapper();
+	~DfBmp280Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct baro_sensor_data &data);
+
+	orb_advert_t		_baro_topic;
+
+	int			_baro_orb_class_instance;
+
+	perf_counter_t		_baro_sample_perf;
+
+};
+
+DfBmp280Wrapper::DfBmp280Wrapper() :
+	BMP280(BARO_DEVICE_PATH),
+	_baro_topic(nullptr),
+	_baro_orb_class_instance(-1),
+	_baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read"))
+{
+}
+
+DfBmp280Wrapper::~DfBmp280Wrapper()
+{
+	perf_free(_baro_sample_perf);
+}
+
+int DfBmp280Wrapper::start()
+{
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("BMP280 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = BMP280::start();
+
+	if (ret != 0) {
+		PX4_ERR("BMP280 start fail: %d", ret);
+		return ret;
+	}
+
+#if defined(__DF_BBBLUE)
+	PX4_INFO("BMP280 started");
+#endif
+
+	return 0;
+}
+
+int DfBmp280Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = BMP280::stop();
+
+	if (ret != 0) {
+		PX4_ERR("BMP280 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
+{
+	perf_begin(_baro_sample_perf);
+
+	sensor_baro_s baro_report = {};
+	baro_report.timestamp   = hrt_absolute_time();
+	baro_report.error_count = data.error_counter;
+
+	baro_report.pressure    = data.pressure_pa / 100.0f; // to mbar
+	baro_report.temperature = data.temperature_c;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_baro_topic == nullptr) {
+			_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+							  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
+		}
+	}
+
+	perf_end(_baro_sample_perf);
+
+	return 0;
+};
+
+
+namespace df_bmp280_wrapper
+{
+
+DfBmp280Wrapper *g_dev = nullptr;
+
+int start(/* enum Rotation rotation */);
+int stop();
+int info();
+void usage();
+
+int start(/*enum Rotation rotation*/)
+{
+	g_dev = new DfBmp280Wrapper(/*rotation*/);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfBmp280Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfBmp280Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(BARO_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    BARO_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_bmp280_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_bmp280_wrapper
+
+
+int
+df_bmp280_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		df_bmp280_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_bmp280_wrapper::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_bmp280_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_bmp280_wrapper::info();
+	}
+
+	else {
+		df_bmp280_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_hmc5883_wrapper
+	MAIN df_hmc5883_wrapper
+	SRCS
+		df_hmc5883_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_hmc5883
+	)

src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp

@@ -0,0 +1,462 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_hmc5883_wrapper.cpp
+ * Lightweight driver to access the HMC5883 of the DriverFramework.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_mag.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <board_config.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <hmc5883/HMC5883.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_hmc5883_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfHmc5883Wrapper : public HMC5883
+{
+public:
+	DfHmc5883Wrapper(enum Rotation rotation, const char *path);
+	~DfHmc5883Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct mag_sensor_data &data);
+
+	void _update_mag_calibration();
+
+	orb_advert_t		_mag_topic;
+
+	int			_param_update_sub;
+
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
+	matrix::Dcmf      _rotation_matrix;
+
+	int			_mag_orb_class_instance;
+
+	perf_counter_t		_mag_sample_perf;
+
+};
+
+DfHmc5883Wrapper::DfHmc5883Wrapper(enum Rotation rotation, const char *path) :
+	HMC5883(path),
+	_mag_topic(nullptr),
+	_param_update_sub(-1),
+	_mag_calibration{},
+	_mag_orb_class_instance(-1),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
+{
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+
+	// Get sensor rotation matrix
+	_rotation_matrix = get_rot_matrix(rotation);
+}
+
+DfHmc5883Wrapper::~DfHmc5883Wrapper()
+{
+	perf_free(_mag_sample_perf);
+}
+
+int DfHmc5883Wrapper::start()
+{
+	/* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("HMC5883 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = HMC5883::start();
+
+	if (ret != 0) {
+		PX4_ERR("HMC5883 start fail: %d", ret);
+		return ret;
+	}
+
+	/* Force getting the calibration values. */
+	_update_mag_calibration();
+
+	return 0;
+}
+
+int DfHmc5883Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = HMC5883::stop();
+
+	if (ret != 0) {
+		PX4_ERR("HMC5883 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfHmc5883Wrapper::_update_mag_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+	}
+}
+
+
+int DfHmc5883Wrapper::_publish(struct mag_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_mag_calibration();
+	}
+
+	/* Publish mag first. */
+	perf_begin(_mag_sample_perf);
+
+	mag_report mag_report = {};
+	mag_report.timestamp = hrt_absolute_time();
+	mag_report.is_external = true;
+
+	/* The standard external mag by 3DR has x pointing to the
+	 * right, y pointing backwards, and z down, therefore switch x
+	 * and y and invert y. */
+	const float tmp = data.field_x_ga;
+	data.field_x_ga = -data.field_y_ga;
+	data.field_y_ga = tmp;
+
+	// TODO: remove these (or get the values)
+	mag_report.x_raw = 0;
+	mag_report.y_raw = 0;
+	mag_report.z_raw = 0;
+
+	matrix::Vector3f mag_val(data.field_x_ga,
+				 data.field_y_ga,
+				 data.field_z_ga);
+
+	// apply sensor rotation on the accel measurement
+	mag_val = _rotation_matrix * mag_val;
+
+	// Apply calibration after rotation.
+	mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+	mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+	mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+
+	// TODO: get these right
+	//mag_report.scaling = -1.0f;
+
+	mag_report.device_id = m_id.dev_id;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_mag_topic == nullptr) {
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+							 &_mag_orb_class_instance, ORB_PRIO_HIGH);
+
+		} else {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
+	}
+
+	perf_end(_mag_sample_perf);
+
+	return 0;
+};
+
+
+namespace df_hmc5883_wrapper
+{
+
+DfHmc5883Wrapper *g_dev = nullptr;
+
+int start(enum Rotation rotation, const char *path);
+int stop();
+int info();
+void usage();
+
+int start(enum Rotation rotation, const char *path)
+{
+	g_dev = new DfHmc5883Wrapper(rotation, path);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfHmc5883Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfHmc5883Wrapper start failed");
+		return ret;
+	}
+
+	// Open the MAG sensor
+	DevHandle h;
+	DevMgr::getHandle(path, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    path, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_hmc5883_wrapper
+
+
+int
+df_hmc5883_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+	const char *device_path = MAG_DEVICE_PATH;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:D:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		case 'D':
+			device_path = myoptarg;
+			break;
+
+		default:
+			df_hmc5883_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_hmc5883_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_hmc5883_wrapper::start(rotation, device_path);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_hmc5883_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_hmc5883_wrapper::info();
+	}
+
+	else {
+		df_hmc5883_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_isl29501_wrapper
+	MAIN df_isl29501_wrapper
+	SRCS
+		df_isl29501_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_isl29501
+	)

src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp

@@ -0,0 +1,378 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_isl29501_wrapper.cpp
+ * Driver to access the ISL29501 of the DriverFramework.
+ *
+ * @author Zach Lovett <zach.lovett@3drobotics.com>
+ * @author Siddharth B Purohit <sid@3drobotics.com>
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+#include <string>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_range_finder.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+#include <isl29501/isl29501.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_isl29501_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfISL29501Wrapper : public ISL29501
+{
+public:
+	DfISL29501Wrapper();
+	~DfISL29501Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct range_sensor_data &data);
+
+	orb_advert_t		_range_topic;
+
+	int			_orb_class_instance;
+
+	// perf_counter_t		_range_sample_perf;
+
+};
+
+DfISL29501Wrapper::DfISL29501Wrapper(/*enum Rotation rotation*/) :
+	ISL29501(ISL_DEVICE_PATH),
+	_range_topic(nullptr),
+	_orb_class_instance(-1)
+{
+}
+
+DfISL29501Wrapper::~DfISL29501Wrapper()
+{
+}
+
+int DfISL29501Wrapper::start()
+{
+	int ret;
+	ret = ISL29501::init();
+
+	if (ret != 0) {
+		PX4_ERR("ISL init fail: %d", ret);
+		return ret;
+	}
+
+	ret = ISL29501::start();
+
+	if (ret != 0) {
+		PX4_ERR("ISL start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfISL29501Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = ISL29501::stop();
+
+	if (ret != 0) {
+		PX4_ERR("ISL stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
+{
+	struct distance_sensor_s d;
+
+	memset(&d, 0, sizeof(d));
+
+	d.timestamp = hrt_absolute_time();
+
+	d.min_distance = float(ISL_MIN_DISTANCE); /* m */
+
+	d.max_distance = float(ISL_MAX_DISTANCE); /* m */
+
+	d.current_distance = float(data.dist);
+
+	d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+
+	d.id = 0; // TODO set proper ID
+
+	d.variance = 0.0f;
+
+	d.signal_quality = -1;
+
+	if (_range_topic == nullptr) {
+		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
+						   &_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
+	}
+
+	return 0;
+};
+
+
+namespace df_isl29501_wrapper
+{
+
+DfISL29501Wrapper *g_dev = nullptr;
+
+int start();
+int stop();
+int info();
+int probe();
+int calibration();
+void usage();
+
+int start()
+{
+	PX4_ERR("start");
+	g_dev = new DfISL29501Wrapper();
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfISL29501Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfISL29501Wrapper start failed");
+		return ret;
+	}
+
+	// Open the range sensor
+	DevHandle h;
+	DevMgr::getHandle(ISL_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    ISL_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+/**
+ * Who am i
+ */
+int
+probe()
+{
+	int ret;
+
+	if (g_dev == nullptr) {
+		ret = start();
+
+		if (ret) {
+			PX4_ERR("Failed to start");
+			return ret;
+		}
+	}
+
+	ret = g_dev->probe();
+
+	if (ret) {
+		PX4_ERR("Failed to probe");
+		return ret;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+/**
+ * Calibration
+ * runs calibration routine for ISL29501
+ * TODO: implement calibration user interface and parameter system to store calib
+ * Note: Currently only serves debugging purpose, user is required to manually
+ * set offset inside code.
+ */
+int
+calibration()
+{
+	int ret;
+
+	if (g_dev == nullptr) {
+		ret = start();
+
+		if (ret) {
+			PX4_ERR("Failed to start");
+			return ret;
+		}
+	}
+
+	ret = g_dev->calibration();
+
+	if (ret) {
+		PX4_ERR("Failed to calibrate");
+		return ret;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_isl_wrapper 'start', 'info', 'stop', 'calib', 'probe'");
+}
+
+} // namespace df_isl_wrapper
+
+
+int
+df_isl29501_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		df_isl29501_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_isl29501_wrapper::start(/*rotation*/);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_isl29501_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_isl29501_wrapper::info();
+	}
+
+	else if (!strcmp(verb, "probe")) {
+		ret = df_isl29501_wrapper::probe();
+	}
+
+	else if (!strcmp(verb, "calib")) {
+		df_isl29501_wrapper::calibration();
+		return 1;
+	}
+
+	else {
+		df_isl29501_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_lsm9ds1_wrapper
+	MAIN df_lsm9ds1_wrapper
+	SRCS
+		df_lsm9ds1_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_lsm9ds1
+	)

src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp

@@ -0,0 +1,881 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_lsm9ds1_wrapper.cpp
+ * Lightweight driver to access the MPU9250 of the DriverFramework.
+ *
+ * @author Miguel Arroyo <miguel@arroyo.me>
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/err.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/mavlink_log.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/device/integrator.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <lsm9ds1/LSM9DS1.hpp>
+#include <DevMgr.hpp>
+
+// We don't want to auto publish, therefore set this to 0.
+#define LSM9DS1_NEVER_AUTOPUBLISH_US 0
+
+
+extern "C" { __EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfLsm9ds1Wrapper : public LSM9DS1
+{
+public:
+	DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation);
+	~DfLsm9ds1Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+	/**
+	 * Print some debug info.
+	 */
+	void		info();
+
+private:
+	int _publish(struct imu_sensor_data &data);
+
+	void _update_accel_calibration();
+	void _update_gyro_calibration();
+	void _update_mag_calibration();
+
+	orb_advert_t		    _accel_topic;
+	orb_advert_t		    _gyro_topic;
+	orb_advert_t        	    _mag_topic;
+	orb_advert_t		    _mavlink_log_pub;
+
+	int			    _param_update_sub;
+
+	struct accel_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _accel_calibration;
+
+	struct gyro_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _gyro_calibration;
+
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
+	matrix::Dcmf	    _rotation_matrix;
+	int			    _accel_orb_class_instance;
+	int			    _gyro_orb_class_instance;
+	int         _mag_orb_class_instance;
+
+	Integrator		    _accel_int;
+	Integrator		    _gyro_int;
+
+	unsigned		    _publish_count;
+
+	perf_counter_t		    _read_counter;
+	perf_counter_t		    _error_counter;
+	perf_counter_t		    _fifo_overflow_counter;
+	perf_counter_t		    _fifo_corruption_counter;
+	perf_counter_t		    _gyro_range_hit_counter;
+	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _publish_perf;
+
+	hrt_abstime		    _last_accel_range_hit_time;
+	uint64_t		    _last_accel_range_hit_count;
+
+	bool _mag_enabled;
+};
+
+DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
+	LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG),
+	_accel_topic(nullptr),
+	_gyro_topic(nullptr),
+	_mag_topic(nullptr),
+	_mavlink_log_pub(nullptr),
+	_param_update_sub(-1),
+	_accel_calibration{},
+	_gyro_calibration{},
+	_mag_calibration{},
+	_accel_orb_class_instance(-1),
+	_gyro_orb_class_instance(-1),
+	_mag_orb_class_instance(-1),
+	_accel_int(LSM9DS1_NEVER_AUTOPUBLISH_US, false),
+	_gyro_int(LSM9DS1_NEVER_AUTOPUBLISH_US, true),
+	_publish_count(0),
+	_read_counter(perf_alloc(PC_COUNT, "lsm9ds1_reads")),
+	_error_counter(perf_alloc(PC_COUNT, "lsm9ds1_errors")),
+	_fifo_overflow_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_overflows")),
+	_fifo_corruption_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_corruptions")),
+	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_gyro_range_hits")),
+	_accel_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_accel_range_hits")),
+	_publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")),
+	_last_accel_range_hit_time(0),
+	_last_accel_range_hit_count(0),
+	_mag_enabled(mag_enabled)
+{
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+
+	if (_mag_enabled) {
+		_mag_calibration.x_scale = 1.0f;
+		_mag_calibration.y_scale = 1.0f;
+		_mag_calibration.z_scale = 1.0f;
+		_mag_calibration.x_offset = 0.0f;
+		_mag_calibration.y_offset = 0.0f;
+		_mag_calibration.z_offset = 0.0f;
+	}
+
+	// Get sensor rotation matrix
+	_rotation_matrix = get_rot_matrix(rotation);
+}
+
+DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper()
+{
+	perf_free(_read_counter);
+	perf_free(_error_counter);
+	perf_free(_fifo_overflow_counter);
+	perf_free(_fifo_corruption_counter);
+	perf_free(_gyro_range_hit_counter);
+	perf_free(_accel_range_hit_counter);
+
+	perf_free(_publish_perf);
+}
+
+int DfLsm9ds1Wrapper::start()
+{
+	// TODO: don't publish garbage here
+	sensor_accel_s accel_report = {};
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
+					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	if (_accel_topic == nullptr) {
+		PX4_ERR("sensor_accel advert fail");
+		return -1;
+	}
+
+	// TODO: don't publish garbage here
+	sensor_gyro_s gyro_report = {};
+	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
+					  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	if (_gyro_topic == nullptr) {
+		PX4_ERR("sensor_gyro advert fail");
+		return -1;
+	}
+
+	if (_mag_enabled) {
+		mag_report mag_report = {};
+		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+						 &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		if (_mag_topic == nullptr) {
+			PX4_ERR("sensor_mag advert fail");
+			return -1;
+		}
+	}
+
+	/* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("LSM9DS1 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = LSM9DS1::start();
+
+	if (ret != 0) {
+		PX4_ERR("LSM9DS1 start fail: %d", ret);
+		return ret;
+	}
+
+	PX4_DEBUG("LSM9DS1 device id is: %d", m_id.dev_id);
+
+	/* Force getting the calibration values. */
+	_update_accel_calibration();
+	_update_gyro_calibration();
+	_update_mag_calibration();
+
+	return 0;
+}
+
+int DfLsm9ds1Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = LSM9DS1::stop();
+
+	if (ret != 0) {
+		PX4_ERR("LSM9DS1 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfLsm9ds1Wrapper::info()
+{
+	perf_print_counter(_read_counter);
+	perf_print_counter(_error_counter);
+	perf_print_counter(_fifo_overflow_counter);
+	perf_print_counter(_fifo_corruption_counter);
+	perf_print_counter(_gyro_range_hit_counter);
+	perf_print_counter(_accel_range_hit_counter);
+
+	perf_print_counter(_publish_perf);
+}
+
+void DfLsm9ds1Wrapper::_update_gyro_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_GYRO%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+}
+
+void DfLsm9ds1Wrapper::_update_accel_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_ACC%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+}
+
+void DfLsm9ds1Wrapper::_update_mag_calibration()
+{
+	if (!_mag_enabled) {
+		return;
+	}
+
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+}
+
+int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_accel_calibration();
+		_update_gyro_calibration();
+	}
+
+	matrix::Vector3f vec_integrated_unused;
+	uint32_t integral_dt_unused;
+	matrix::Vector3f accel_val(data.accel_m_s2_x,
+				   data.accel_m_s2_y,
+				   data.accel_m_s2_z);
+	// apply sensor rotation on the accel measurement
+	accel_val = _rotation_matrix * accel_val;
+	// Apply calibration after rotation
+	accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
+	accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
+	accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
+	_accel_int.put_with_interval(data.fifo_sample_interval_us,
+				     accel_val,
+				     vec_integrated_unused,
+				     integral_dt_unused);
+	matrix::Vector3f gyro_val(data.gyro_rad_s_x,
+				  data.gyro_rad_s_y,
+				  data.gyro_rad_s_z);
+	// apply sensor rotation on the gyro measurement
+	gyro_val = _rotation_matrix * gyro_val;
+	// Apply calibration after rotation
+	gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
+	gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
+	gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+	_gyro_int.put_with_interval(data.fifo_sample_interval_us,
+				    gyro_val,
+				    vec_integrated_unused,
+				    integral_dt_unused);
+
+	// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
+	// Therefore, only publish every forth time.
+	++_publish_count;
+
+	if (_publish_count < 4) {
+		return 0;
+	}
+
+	_publish_count = 0;
+
+	// Update all the counters.
+	perf_set_count(_read_counter, data.read_counter);
+	perf_set_count(_error_counter, data.error_counter);
+	perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
+	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
+	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
+	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+
+	perf_begin(_publish_perf);
+
+	sensor_accel_s accel_report = {};
+	sensor_gyro_s gyro_report = {};
+	mag_report mag_report = {};
+
+	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+	mag_report.timestamp = accel_report.timestamp;
+	mag_report.is_external = false;
+
+	// TODO: get these right
+	gyro_report.scaling = -1.0f;
+	gyro_report.device_id = m_id.dev_id;
+
+	accel_report.scaling = -1.0f;
+	accel_report.device_id = m_id.dev_id;
+
+	if (_mag_enabled) {
+		mag_report.scaling = -1.0f;
+		mag_report.device_id = m_id.dev_id;
+	}
+
+	// TODO: remove these (or get the values)
+	gyro_report.x_raw = 0;
+	gyro_report.y_raw = 0;
+	gyro_report.z_raw = 0;
+
+	accel_report.x_raw = 0;
+	accel_report.y_raw = 0;
+	accel_report.z_raw = 0;
+
+	if (_mag_enabled) {
+		mag_report.x_raw = 0;
+		mag_report.y_raw = 0;
+		mag_report.z_raw = 0;
+	}
+
+	matrix::Vector3f gyro_val_filt;
+	matrix::Vector3f accel_val_filt;
+
+	// Read and reset.
+	matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
+	matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
+
+	// Use the filtered (by integration) values to get smoother / less noisy data.
+	gyro_report.x = gyro_val_filt(0);
+	gyro_report.y = gyro_val_filt(1);
+	gyro_report.z = gyro_val_filt(2);
+
+	accel_report.x = accel_val_filt(0);
+	accel_report.y = accel_val_filt(1);
+	accel_report.z = accel_val_filt(2);
+
+	if (_mag_enabled) {
+		matrix::Vector3f mag_val(data.mag_ga_x,
+					 data.mag_ga_y,
+					 data.mag_ga_z);
+		mag_val = _rotation_matrix * mag_val;
+		mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+		mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+		mag_val(2) = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+
+		mag_report.x = mag_val(0);
+		mag_report.y = mag_val(1);
+		mag_report.z = mag_val(2);
+	}
+
+	gyro_report.x_integral = gyro_val_integ(0);
+	gyro_report.y_integral = gyro_val_integ(1);
+	gyro_report.z_integral = gyro_val_integ(2);
+
+	accel_report.x_integral = accel_val_integ(0);
+	accel_report.y_integral = accel_val_integ(1);
+	accel_report.z_integral = accel_val_integ(2);
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+
+		if (_gyro_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
+		}
+
+		if (_accel_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
+
+		if (_mag_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
+		// Report if there are high vibrations, every 10 times it happens.
+		const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
+
+		// Report every 5s.
+		const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
+
+		if (threshold_reached && due_to_report) {
+			mavlink_log_critical(&_mavlink_log_pub,
+					     "High accelerations, range exceeded %llu times",
+					     data.accel_range_hit_counter);
+
+			_last_accel_range_hit_time = hrt_absolute_time();
+			_last_accel_range_hit_count = data.accel_range_hit_counter;
+		}
+	}
+
+	perf_end(_publish_perf);
+
+	// TODO: check the return codes of this function
+	return 0;
+};
+
+
+namespace df_lsm9ds1_wrapper
+{
+
+DfLsm9ds1Wrapper *g_dev = nullptr;
+
+int start(bool mag_enabled, enum Rotation rotation);
+int stop();
+int info();
+void usage();
+
+int start(bool mag_enabled, enum Rotation rotation)
+{
+	g_dev = new DfLsm9ds1Wrapper(mag_enabled, rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfLsm9ds1Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfLsm9ds1Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    IMU_DEVICE_ACC_GYRO, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	DevMgr::getHandle(IMU_DEVICE_MAG, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    IMU_DEVICE_MAG, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_INFO("state @ %p", g_dev);
+	g_dev->info();
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_lsm9ds1_wrapper 'start', 'info', 'stop', 'start_without_mag'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_lsm9ds1_wrapper
+
+
+int
+df_lsm9ds1_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			df_lsm9ds1_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_lsm9ds1_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start_without_mag")) {
+		ret = df_lsm9ds1_wrapper::start(false, rotation);
+	}
+
+	else if (!strcmp(verb, "start")) {
+		ret = df_lsm9ds1_wrapper::start(true, rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_lsm9ds1_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_lsm9ds1_wrapper::info();
+	}
+
+	else {
+		df_lsm9ds1_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_ltc2946_wrapper
+	MAIN df_ltc2946_wrapper
+	SRCS
+		df_ltc2946_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_ltc2946
+	)

src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp

@@ -0,0 +1,264 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2018 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 df_ltc2946_wrapper.cpp
+ * Driver to access the LTC2946 of the DriverFramework.
+ *
+ * @author Christoph Tobler <christoph@px4.io>
+ */
+
+
+#include <string>
+#include <px4_config.h>
+#include <systemlib/err.h>
+#include <battery/battery.h>
+#include <drivers/drv_hrt.h>
+
+#include <ltc2946/LTC2946.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+
+
+extern "C" { __EXPORT int df_ltc2946_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfLtc2946Wrapper : public LTC2946
+{
+public:
+	DfLtc2946Wrapper();
+	~DfLtc2946Wrapper();
+
+	int		start();
+	int		stop();
+
+private:
+	int _publish(const struct ltc2946_sensor_data &data);
+
+	orb_advert_t	_battery_pub{nullptr};
+	battery_status_s _battery_status{};
+	Battery _battery{};
+
+	int _actuator_ctrl_0_sub{-1};
+	int _vcontrol_mode_sub{-1};
+
+};
+
+DfLtc2946Wrapper::DfLtc2946Wrapper() :
+	LTC2946(LTC2946_DEVICE_PATH)
+{
+	_battery.reset(&_battery_status);
+
+	// subscriptions
+	_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
+	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+}
+
+DfLtc2946Wrapper::~DfLtc2946Wrapper()
+{
+	orb_unsubscribe(_actuator_ctrl_0_sub);
+	orb_unsubscribe(_vcontrol_mode_sub);
+}
+
+int DfLtc2946Wrapper::start()
+{
+	int ret;
+
+	/* Init device and start sensor. */
+	ret = init();
+
+	//
+	if (ret != 0) {
+		PX4_ERR("LTC2946 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = LTC2946::start();
+
+	if (ret != 0) {
+		PX4_ERR("LTC2946 start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfLtc2946Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = LTC2946::stop();
+
+	if (ret != 0) {
+		PX4_ERR("LTC2946 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data)
+{
+	hrt_abstime t = hrt_absolute_time();
+	bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V;
+
+	actuator_controls_s ctrl;
+	orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
+	vehicle_control_mode_s vcontrol_mode;
+	orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
+
+	_battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A,
+				     connected, true, 1,
+				     ctrl.control[actuator_controls_s::INDEX_THROTTLE],
+				     vcontrol_mode.flag_armed,  &_battery_status);
+
+	int instance;
+	orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &instance, ORB_PRIO_DEFAULT);
+
+	return 0;
+}
+
+
+namespace df_ltc2946_wrapper
+{
+
+DfLtc2946Wrapper *g_dev = nullptr;
+
+int start();
+int stop();
+int info();
+void usage();
+
+int start()
+{
+	PX4_WARN("starting LTC2946");
+	g_dev = new DfLtc2946Wrapper();
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfLtc2946Wrapper object");
+		return -1;
+
+	} else {
+		PX4_INFO("started LTC2946");
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfLtc2946Wrapper start failed");
+		return ret;
+	}
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_ltc2946_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_ltc2946_wrapper
+
+
+int
+df_ltc2946_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+
+	if (argc <= 1) {
+		df_ltc2946_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[1];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_ltc2946_wrapper::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_ltc2946_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_ltc2946_wrapper::info();
+	}
+
+	else {
+		df_ltc2946_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_mpu6050_wrapper
+	MAIN df_mpu6050_wrapper
+	SRCS
+		df_mpu6050_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_mpu6050
+	)

src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp

@@ -0,0 +1,713 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_mpu6050_wrapper.cpp
+ * Lightweight driver to access the MPU6050 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/err.h>
+#include <perf/perf_counter.h>
+#include <systemlib/mavlink_log.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/device/integrator.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <mpu6050/MPU6050.hpp>
+#include <DevMgr.hpp>
+
+// We don't want to auto publish, therefore set this to 0.
+#define MPU6050_NEVER_AUTOPUBLISH_US 0
+
+
+extern "C" { __EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfMPU6050Wrapper : public MPU6050
+{
+public:
+	DfMPU6050Wrapper(enum Rotation rotation);
+	~DfMPU6050Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+	/**
+	 * Print some debug info.
+	 */
+	void		info();
+
+private:
+	int _publish(struct imu_sensor_data &data);
+
+	void _update_accel_calibration();
+	void _update_gyro_calibration();
+
+	orb_advert_t		    _accel_topic;
+	orb_advert_t		    _gyro_topic;
+
+	orb_advert_t		    _mavlink_log_pub;
+
+	int			    _param_update_sub;
+
+	struct accel_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _accel_calibration;
+
+	struct gyro_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _gyro_calibration;
+
+	matrix::Dcmf	    _rotation_matrix;
+
+	int			    _accel_orb_class_instance;
+	int			    _gyro_orb_class_instance;
+
+	Integrator		    _accel_int;
+	Integrator		    _gyro_int;
+
+	unsigned		    _publish_count;
+
+	perf_counter_t		    _read_counter;
+	perf_counter_t		    _error_counter;
+	perf_counter_t		    _fifo_overflow_counter;
+	perf_counter_t		    _fifo_corruption_counter;
+	perf_counter_t		    _gyro_range_hit_counter;
+	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _publish_perf;
+
+	hrt_abstime		    _last_accel_range_hit_time;
+	uint64_t		    _last_accel_range_hit_count;
+
+};
+
+DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) :
+	MPU6050(IMU_DEVICE_PATH),
+	_accel_topic(nullptr),
+	_gyro_topic(nullptr),
+	_mavlink_log_pub(nullptr),
+	_param_update_sub(-1),
+	_accel_calibration{},
+	_gyro_calibration{},
+	_accel_orb_class_instance(-1),
+	_gyro_orb_class_instance(-1),
+	_accel_int(MPU6050_NEVER_AUTOPUBLISH_US, false),
+	_gyro_int(MPU6050_NEVER_AUTOPUBLISH_US, true),
+	_publish_count(0),
+	_read_counter(perf_alloc(PC_COUNT, "mpu6050_reads")),
+	_error_counter(perf_alloc(PC_COUNT, "mpu6050_errors")),
+	_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_overflows")),
+	_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_corruptions")),
+	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_gyro_range_hits")),
+	_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_accel_range_hits")),
+	_publish_perf(perf_alloc(PC_ELAPSED, "mpu6050_publish")),
+	_last_accel_range_hit_time(0),
+	_last_accel_range_hit_count(0)
+{
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+
+	// Get sensor rotation matrix
+	_rotation_matrix = get_rot_matrix(rotation);
+}
+
+DfMPU6050Wrapper::~DfMPU6050Wrapper()
+{
+	perf_free(_read_counter);
+	perf_free(_error_counter);
+	perf_free(_fifo_overflow_counter);
+	perf_free(_fifo_corruption_counter);
+	perf_free(_gyro_range_hit_counter);
+	perf_free(_accel_range_hit_counter);
+
+	perf_free(_publish_perf);
+}
+
+int DfMPU6050Wrapper::start()
+{
+	/* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("MPU6050 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = MPU6050::start();
+
+	if (ret != 0) {
+		PX4_ERR("MPU6050 start fail: %d", ret);
+		return ret;
+	}
+
+	PX4_DEBUG("MPU6050 device id is: %d", m_id.dev_id);
+
+	/* Force getting the calibration values. */
+	_update_accel_calibration();
+	_update_gyro_calibration();
+
+	return 0;
+}
+
+int DfMPU6050Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = MPU6050::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MPU6050 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfMPU6050Wrapper::info()
+{
+	perf_print_counter(_read_counter);
+	perf_print_counter(_error_counter);
+	perf_print_counter(_fifo_overflow_counter);
+	perf_print_counter(_fifo_corruption_counter);
+	perf_print_counter(_gyro_range_hit_counter);
+	perf_print_counter(_accel_range_hit_counter);
+
+	perf_print_counter(_publish_perf);
+}
+
+void DfMPU6050Wrapper::_update_gyro_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_GYRO%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+}
+
+void DfMPU6050Wrapper::_update_accel_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_ACC%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+}
+
+int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_accel_calibration();
+		_update_gyro_calibration();
+	}
+
+	uint64_t now = hrt_absolute_time();
+
+	matrix::Vector3f vec_integrated_unused;
+	uint32_t integral_dt_unused;
+
+	matrix::Vector3f accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z);
+
+	// apply sensor rotation on the accel measurement
+	accel_val = _rotation_matrix * accel_val;
+
+	accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
+	accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
+	accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
+
+	_accel_int.put(now,
+		       accel_val,
+		       vec_integrated_unused,
+		       integral_dt_unused);
+
+	matrix::Vector3f gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z);
+
+	// apply sensor rotation on the gyro measurement
+	gyro_val = _rotation_matrix * gyro_val;
+
+	gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
+	gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
+	gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+
+	_gyro_int.put(now,
+		      gyro_val,
+		      vec_integrated_unused,
+		      integral_dt_unused);
+
+	// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
+	// and wait for more packets.
+	if (!data.is_last_fifo_sample) {
+		return 0;
+	}
+
+	// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
+	// Therefore, only publish every forth time.
+	++_publish_count;
+
+	if (_publish_count < 4) {
+		return 0;
+	}
+
+	_publish_count = 0;
+
+	// Update all the counters.
+	perf_set_count(_read_counter, data.read_counter);
+	perf_set_count(_error_counter, data.error_counter);
+	perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
+	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
+	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
+	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+
+	perf_begin(_publish_perf);
+
+	sensor_accel_s accel_report = {};
+	sensor_gyro_s gyro_report = {};
+
+	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+
+	// TODO: get these right
+	gyro_report.scaling = -1.0f;
+	gyro_report.device_id = m_id.dev_id;
+
+	accel_report.scaling = -1.0f;
+	accel_report.device_id = m_id.dev_id;
+
+	// TODO: remove these (or get the values)
+	gyro_report.x_raw = 0;
+	gyro_report.y_raw = 0;
+	gyro_report.z_raw = 0;
+
+	accel_report.x_raw = 0;
+	accel_report.y_raw = 0;
+	accel_report.z_raw = 0;
+
+	matrix::Vector3f gyro_val_filt;
+	matrix::Vector3f accel_val_filt;
+
+	// Read and reset.
+	matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
+	matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
+
+	// Use the filtered (by integration) values to get smoother / less noisy data.
+	gyro_report.x = gyro_val_filt(0);
+	gyro_report.y = gyro_val_filt(1);
+	gyro_report.z = gyro_val_filt(2);
+
+	accel_report.x = accel_val_filt(0);
+	accel_report.y = accel_val_filt(1);
+	accel_report.z = accel_val_filt(2);
+
+	gyro_report.x_integral = gyro_val_integ(0);
+	gyro_report.y_integral = gyro_val_integ(1);
+	gyro_report.z_integral = gyro_val_integ(2);
+
+	accel_report.x_integral = accel_val_integ(0);
+	accel_report.y_integral = accel_val_integ(1);
+	accel_report.z_integral = accel_val_integ(2);
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_gyro_topic == nullptr) {
+			_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
+							  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
+		}
+
+		if (_accel_topic == nullptr) {
+			_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
+							   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
+
+		// Report if there are high vibrations, every 10 times it happens.
+		const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
+
+		// Report every 5s.
+		const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
+
+		if (threshold_reached && due_to_report) {
+			mavlink_log_critical(&_mavlink_log_pub,
+					     "High accelerations, range exceeded %llu times",
+					     data.accel_range_hit_counter);
+
+			_last_accel_range_hit_time = hrt_absolute_time();
+			_last_accel_range_hit_count = data.accel_range_hit_counter;
+		}
+	}
+
+	perf_end(_publish_perf);
+
+	// TODO: check the return codes of this function
+	return 0;
+};
+
+
+namespace df_mpu6050_wrapper
+{
+
+DfMPU6050Wrapper *g_dev = nullptr;
+
+int start(enum Rotation rotation);
+int stop();
+int info();
+void usage();
+
+int start(enum Rotation rotation)
+{
+	g_dev = new DfMPU6050Wrapper(rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfMPU6050Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfMPU6050Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(IMU_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    IMU_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_INFO("state @ %p", g_dev);
+	g_dev->info();
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_mpu6050_wrapper 'start', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_mpu6050_wrapper
+
+
+int
+df_mpu6050_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			df_mpu6050_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_mpu6050_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		ret = df_mpu6050_wrapper::start(rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_mpu6050_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_mpu6050_wrapper::info();
+	}
+
+	else {
+		df_mpu6050_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_mpu9250_wrapper
+	MAIN df_mpu9250_wrapper
+	SRCS
+		df_mpu9250_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_mpu9250
+	)

src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp

@@ -0,0 +1,924 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_mpu9250_wrapper.cpp
+ * Lightweight driver to access the MPU9250 of the DriverFramework.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/err.h>
+#include <perf/perf_counter.h>
+#include <systemlib/mavlink_log.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/device/integrator.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+#include <lib/conversion/rotation.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <mpu9250/MPU9250.hpp>
+#include <DevMgr.hpp>
+
+#define MPU9250_ACCEL_DEFAULT_RATE 1000
+#define MPU9250_GYRO_DEFAULT_RATE 1000
+
+#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU9250_PUB_RATE 280
+
+
+extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfMpu9250Wrapper : public MPU9250
+{
+public:
+	DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation);
+	~DfMpu9250Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+	/**
+	 * Print some debug info.
+	 */
+	void		info();
+
+private:
+	int _publish(struct imu_sensor_data &data);
+
+	void _update_accel_calibration();
+	void _update_gyro_calibration();
+	void _update_mag_calibration();
+
+	orb_advert_t		    _accel_topic;
+	orb_advert_t		    _gyro_topic;
+	orb_advert_t		    _mag_topic;
+
+	orb_advert_t		    _mavlink_log_pub;
+
+	int			    _param_update_sub;
+
+	struct accel_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _accel_calibration;
+
+	struct gyro_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _gyro_calibration;
+
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
+	int			    _accel_orb_class_instance;
+	int			    _gyro_orb_class_instance;
+	int			    _mag_orb_class_instance;
+
+	Integrator		    _accel_int;
+	Integrator		    _gyro_int;
+
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
+
+	perf_counter_t		    _read_counter;
+	perf_counter_t		    _error_counter;
+	perf_counter_t		    _fifo_overflow_counter;
+	perf_counter_t		    _fifo_corruption_counter;
+	perf_counter_t		    _gyro_range_hit_counter;
+	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _mag_fifo_overflow_counter;
+	perf_counter_t		    _publish_perf;
+
+	hrt_abstime		    _last_accel_range_hit_time;
+	uint64_t		    _last_accel_range_hit_count;
+
+	bool _mag_enabled;
+
+	enum Rotation _rotation;
+};
+
+DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) :
+	MPU9250(IMU_DEVICE_PATH, mag_enabled),
+	_accel_topic(nullptr),
+	_gyro_topic(nullptr),
+	_mag_topic(nullptr),
+	_mavlink_log_pub(nullptr),
+	_param_update_sub(-1),
+	_accel_calibration{},
+	_gyro_calibration{},
+	_mag_calibration{},
+	_accel_orb_class_instance(-1),
+	_gyro_orb_class_instance(-1),
+	_mag_orb_class_instance(-1),
+	_accel_int(1000000 / MPU9250_PUB_RATE, false),
+	_gyro_int(1000000 / MPU9250_PUB_RATE, true),
+	_accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
+	_error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")),
+	_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_overflows")),
+	_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")),
+	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")),
+	_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")),
+	_mag_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_mag_fifo_overflows")),
+	_publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")),
+	_last_accel_range_hit_time(0),
+	_last_accel_range_hit_count(0),
+	_mag_enabled(mag_enabled),
+	_rotation(rotation)
+{
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+
+	if (_mag_enabled) {
+		_mag_calibration.x_scale = 1.0f;
+		_mag_calibration.y_scale = 1.0f;
+		_mag_calibration.z_scale = 1.0f;
+		_mag_calibration.x_offset = 0.0f;
+		_mag_calibration.y_offset = 0.0f;
+		_mag_calibration.z_offset = 0.0f;
+	}
+
+	// set software low pass filter for controllers
+	param_t param_handle = param_find("IMU_ACCEL_CUTOFF");
+	float param_val = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
+
+	if (param_handle != PARAM_INVALID && (param_get(param_handle, &param_val) == PX4_OK)) {
+		_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
+		_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
+		_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
+
+	} else {
+		PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
+	}
+
+	param_handle = param_find("IMU_GYRO_CUTOFF");
+	param_val = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
+
+	if (param_handle != PARAM_INVALID && (param_get(param_handle, &param_val) == PX4_OK)) {
+		_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
+		_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
+		_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
+
+	} else {
+		PX4_ERR("IMU_GYRO_CUTOFF param invalid");
+	}
+}
+
+DfMpu9250Wrapper::~DfMpu9250Wrapper()
+{
+	perf_free(_read_counter);
+	perf_free(_error_counter);
+	perf_free(_fifo_overflow_counter);
+	perf_free(_fifo_corruption_counter);
+	perf_free(_gyro_range_hit_counter);
+	perf_free(_accel_range_hit_counter);
+
+	if (_mag_enabled) {
+		perf_free(_mag_fifo_overflow_counter);
+	}
+
+	perf_free(_publish_perf);
+}
+
+int DfMpu9250Wrapper::start()
+{
+	// TODO: don't publish garbage here
+	sensor_accel_s accel_report = {};
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
+					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	if (_accel_topic == nullptr) {
+		PX4_ERR("sensor_accel advert fail");
+		return -1;
+	}
+
+	// TODO: don't publish garbage here
+	sensor_gyro_s gyro_report = {};
+	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
+					  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	if (_gyro_topic == nullptr) {
+		PX4_ERR("sensor_gyro advert fail");
+		return -1;
+	}
+
+	if (_mag_enabled) {
+	}
+
+	/* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("MPU9250 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = MPU9250::start();
+
+	if (ret != 0) {
+		PX4_ERR("MPU9250 start fail: %d", ret);
+		return ret;
+	}
+
+	PX4_DEBUG("MPU9250 device id is: %d", m_id.dev_id);
+
+	/* Force getting the calibration values. */
+	_update_accel_calibration();
+	_update_gyro_calibration();
+	_update_mag_calibration();
+
+	return 0;
+}
+
+int DfMpu9250Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = MPU9250::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MPU9250 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfMpu9250Wrapper::info()
+{
+	perf_print_counter(_read_counter);
+	perf_print_counter(_error_counter);
+	perf_print_counter(_fifo_overflow_counter);
+	perf_print_counter(_fifo_corruption_counter);
+	perf_print_counter(_gyro_range_hit_counter);
+	perf_print_counter(_accel_range_hit_counter);
+
+	if (_mag_enabled) {
+		perf_print_counter(_mag_fifo_overflow_counter);
+	}
+
+	perf_print_counter(_publish_perf);
+}
+
+void DfMpu9250Wrapper::_update_gyro_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_GYRO%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+}
+
+void DfMpu9250Wrapper::_update_accel_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_ACC%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+}
+
+void DfMpu9250Wrapper::_update_mag_calibration()
+{
+	if (!_mag_enabled) {
+		return;
+	}
+
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+}
+
+int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_accel_calibration();
+		_update_gyro_calibration();
+		_update_mag_calibration();
+	}
+
+	sensor_accel_s accel_report = {};
+	sensor_gyro_s gyro_report = {};
+	mag_report mag_report = {};
+
+	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+
+	accel_report.error_count = gyro_report.error_count = mag_report.error_count = data.error_counter;
+
+	// ACCEL
+
+	float xraw_f = data.accel_m_s2_x;
+	float yraw_f = data.accel_m_s2_y;
+	float zraw_f = data.accel_m_s2_z;
+
+	// apply user specified rotation
+	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
+
+	// MPU9250 driver from DriverFramework does not provide any raw values
+	// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
+	accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000];
+	accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000];
+	accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000];
+
+	// adjust values according to the calibration
+	float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale;
+	float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale;
+	float z_in_new = (zraw_f - _accel_calibration.z_offset) * _accel_calibration.z_scale;
+
+	accel_report.x = _accel_filter_x.apply(x_in_new);
+	accel_report.y = _accel_filter_y.apply(y_in_new);
+	accel_report.z = _accel_filter_z.apply(z_in_new);
+
+	matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
+	matrix::Vector3f aval_integrated;
+
+	_accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
+	accel_report.x_integral = aval_integrated(0);
+	accel_report.y_integral = aval_integrated(1);
+	accel_report.z_integral = aval_integrated(2);
+
+	// GYRO
+
+	xraw_f = data.gyro_rad_s_x;
+	yraw_f = data.gyro_rad_s_y;
+	zraw_f = data.gyro_rad_s_z;
+
+	// apply user specified rotation
+	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
+
+	// MPU9250 driver from DriverFramework does not provide any raw values
+	// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
+	gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000];
+	gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000];
+	gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000];
+
+	// adjust values according to the calibration
+	float x_gyro_in_new = (xraw_f - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
+	float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
+	float z_gyro_in_new = (zraw_f - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+
+	gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new);
+	gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new);
+	gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new);
+
+	matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
+	matrix::Vector3f gval_integrated;
+
+	bool sensor_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
+	gyro_report.x_integral = gval_integrated(0);
+	gyro_report.y_integral = gval_integrated(1);
+	gyro_report.z_integral = gval_integrated(2);
+
+
+	// if gyro integrator did not return a sample we can return here
+	// Note: the accel integrator receives the same timestamp as the gyro integrator
+	// so we do not need to handle it seperately
+	if (!sensor_notify) {
+		return 0;
+	}
+
+	// Update all the counters.
+	perf_set_count(_read_counter, data.read_counter);
+	perf_set_count(_error_counter, data.error_counter);
+	perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
+	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
+	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
+	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+
+	if (_mag_enabled) {
+		perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
+	}
+
+	perf_begin(_publish_perf);
+
+	// TODO: get these right
+	gyro_report.scaling = -1.0f;
+	gyro_report.device_id = m_id.dev_id;
+
+	accel_report.scaling = -1.0f;
+	accel_report.device_id = m_id.dev_id;
+
+	if (_mag_enabled) {
+		mag_report.timestamp = accel_report.timestamp;
+		mag_report.is_external = false;
+
+		mag_report.scaling = -1.0f;
+		mag_report.device_id = m_id.dev_id;
+
+		xraw_f = data.mag_ga_x;
+		yraw_f = data.mag_ga_y;
+		zraw_f = data.mag_ga_z;
+
+		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
+
+		// MPU9250 driver from DriverFramework does not provide any raw values
+		// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
+		mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000]
+		mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000]
+		mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000]
+
+		mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+		mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+		mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+	}
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked) && sensor_notify) {
+
+		if (_gyro_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
+		}
+
+		if (_accel_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
+
+		if (_mag_enabled) {
+
+			if (_mag_topic == nullptr) {
+				_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+								 &_mag_orb_class_instance, ORB_PRIO_LOW);
+
+			} else {
+				orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+			}
+		}
+
+		// Report if there are high vibrations, every 10 times it happens.
+		const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
+
+		// Report every 5s.
+		const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
+
+		if (threshold_reached && due_to_report) {
+			mavlink_log_critical(&_mavlink_log_pub,
+					     "High accelerations, range exceeded %llu times",
+					     data.accel_range_hit_counter);
+
+			_last_accel_range_hit_time = hrt_absolute_time();
+			_last_accel_range_hit_count = data.accel_range_hit_counter;
+		}
+	}
+
+	perf_end(_publish_perf);
+
+	// TODO: check the return codes of this function
+	return 0;
+};
+
+
+namespace df_mpu9250_wrapper
+{
+
+DfMpu9250Wrapper *g_dev = nullptr;
+
+int start(bool mag_enabled, enum Rotation rotation);
+int stop();
+int info();
+void usage();
+
+int start(bool mag_enabled, enum Rotation rotation)
+{
+	g_dev = new DfMpu9250Wrapper(mag_enabled, rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfMpu9250Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfMpu9250Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(IMU_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    IMU_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_INFO("state @ %p", g_dev);
+	g_dev->info();
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_mpu9250_wrapper
+
+
+int
+df_mpu9250_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			df_mpu9250_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_mpu9250_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start_without_mag")) {
+		ret = df_mpu9250_wrapper::start(false, rotation);
+	}
+
+	else if (!strcmp(verb, "start")) {
+		ret = df_mpu9250_wrapper::start(true, rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_mpu9250_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_mpu9250_wrapper::info();
+	}
+
+	else {
+		df_mpu9250_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_ms5607_wrapper
+	MAIN df_ms5607_wrapper
+	SRCS
+		df_ms5607_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_ms5607
+	)

src/drivers/driver_framework_wrapper/df_ms5607_wrapper/df_ms5607_wrapper.cpp

@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_ms5607_wrapper.cpp
+ * Lightweight driver to access the MS5607 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_baro.h>
+#include <drivers/drv_hrt.h>
+
+#include <board_config.h>
+
+#include <ms5607/MS5607.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_ms5607_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfMS5607Wrapper : public MS5607
+{
+public:
+	DfMS5607Wrapper();
+	~DfMS5607Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct baro_sensor_data &data);
+
+	orb_advert_t		_baro_topic;
+
+	int			_baro_orb_class_instance;
+
+	perf_counter_t		_baro_sample_perf;
+
+};
+
+DfMS5607Wrapper::DfMS5607Wrapper() :
+	MS5607(BARO_DEVICE_PATH),
+	_baro_topic(nullptr),
+	_baro_orb_class_instance(-1),
+	_baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read"))
+{
+}
+
+DfMS5607Wrapper::~DfMS5607Wrapper()
+{
+	perf_free(_baro_sample_perf);
+}
+
+int DfMS5607Wrapper::start()
+{
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("MS5607 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = MS5607::start();
+
+	if (ret != 0) {
+		PX4_ERR("MS5607 start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5607Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = MS5607::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MS5607 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5607Wrapper::_publish(struct baro_sensor_data &data)
+{
+	perf_begin(_baro_sample_perf);
+
+	sensor_baro_s baro_report{};
+	baro_report.timestamp = hrt_absolute_time();
+
+	baro_report.pressure = data.pressure_pa / 100.0f; // convert to mbar
+	baro_report.temperature = data.temperature_c;
+	baro_report.error_count = data.error_counter;
+	baro_report.device_id = m_id.dev_id;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_baro_topic == nullptr) {
+			_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+							  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
+		}
+	}
+
+	perf_end(_baro_sample_perf);
+
+	return 0;
+};
+
+
+namespace df_ms5607_wrapper
+{
+
+DfMS5607Wrapper *g_dev = nullptr;
+
+int start(/* enum Rotation rotation */);
+int stop();
+int info();
+void usage();
+
+int start(/*enum Rotation rotation*/)
+{
+	g_dev = new DfMS5607Wrapper(/*rotation*/);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfMS5607Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfMS5607Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(BARO_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    BARO_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_ms5607_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_ms5607_wrapper
+
+
+int
+df_ms5607_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		df_ms5607_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_ms5607_wrapper::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_ms5607_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_ms5607_wrapper::info();
+	}
+
+	else {
+		df_ms5607_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_ms5611_wrapper
+	MAIN df_ms5611_wrapper
+	SRCS
+		df_ms5611_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_ms5611
+	)
+

src/drivers/driver_framework_wrapper/df_ms5611_wrapper/df_ms5611_wrapper.cpp

@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_ms5611_wrapper.cpp
+ * Lightweight driver to access the MS5611 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_baro.h>
+#include <drivers/drv_hrt.h>
+
+#include <board_config.h>
+
+#include <ms5611/MS5611.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_ms5611_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfMS5611Wrapper : public MS5611
+{
+public:
+	DfMS5611Wrapper();
+	~DfMS5611Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct baro_sensor_data &data);
+
+	orb_advert_t		_baro_topic;
+
+	int			_baro_orb_class_instance;
+
+	perf_counter_t		_baro_sample_perf;
+
+};
+
+DfMS5611Wrapper::DfMS5611Wrapper() :
+	MS5611(BARO_DEVICE_PATH),
+	_baro_topic(nullptr),
+	_baro_orb_class_instance(-1),
+	_baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read"))
+{
+}
+
+DfMS5611Wrapper::~DfMS5611Wrapper()
+{
+	perf_free(_baro_sample_perf);
+}
+
+int DfMS5611Wrapper::start()
+{
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("MS5611 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = MS5611::start();
+
+	if (ret != 0) {
+		PX4_ERR("MS5611 start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5611Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = MS5611::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MS5611 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5611Wrapper::_publish(struct baro_sensor_data &data)
+{
+	perf_begin(_baro_sample_perf);
+
+	sensor_baro_s baro_report{};
+	baro_report.timestamp = hrt_absolute_time();
+
+	baro_report.pressure = data.pressure_pa / 100.0f; // convert to mbar
+	baro_report.temperature = data.temperature_c;
+	baro_report.error_count = data.error_counter;
+	baro_report.device_id = m_id.dev_id;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_baro_topic == nullptr) {
+			_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+							  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
+		}
+	}
+
+	perf_end(_baro_sample_perf);
+
+	return 0;
+};
+
+
+namespace df_ms5611_wrapper
+{
+
+DfMS5611Wrapper *g_dev = nullptr;
+
+int start(/* enum Rotation rotation */);
+int stop();
+int info();
+void usage();
+
+int start(/*enum Rotation rotation*/)
+{
+	g_dev = new DfMS5611Wrapper(/*rotation*/);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfMS5611Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfMS5611Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(BARO_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    BARO_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_ms5611_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_ms5611_wrapper
+
+
+int
+df_ms5611_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		df_ms5611_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_ms5611_wrapper::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_ms5611_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_ms5611_wrapper::info();
+	}
+
+	else {
+		df_ms5611_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+
+include_directories(../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_trone_wrapper
+	MAIN df_trone_wrapper
+	SRCS
+		df_trone_wrapper.cpp
+	DEPENDS
+		git_driverframework
+		df_driver_framework
+		df_trone
+	)
+

src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp

@@ -0,0 +1,362 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 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 df_trone_wrapper.cpp
+ * Driver to access the TROne of the DriverFramework.
+ *
+ * @author Nicolas de Palezieux <ndepal@gmail.com>
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+#include <string>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_range_finder.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+#include <trone/TROne.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_trone_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfTROneWrapper : public TROne
+{
+public:
+	DfTROneWrapper(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	~DfTROneWrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct range_sensor_data &data);
+
+	uint8_t _rotation;
+
+	orb_advert_t		_range_topic;
+
+	int			_orb_class_instance;
+
+	// perf_counter_t		_range_sample_perf;
+
+};
+
+DfTROneWrapper::DfTROneWrapper(uint8_t rotation) :
+	TROne(TRONE_DEVICE_PATH),
+	_rotation(rotation),
+	_range_topic(nullptr),
+	_orb_class_instance(-1)
+{
+}
+
+DfTROneWrapper::~DfTROneWrapper()
+{
+}
+
+int DfTROneWrapper::start()
+{
+	int ret;
+
+	/* Init device and start sensor. */
+	ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("TROne init fail: %d", ret);
+		return ret;
+	}
+
+	ret = TROne::start();
+
+	if (ret != 0) {
+		PX4_ERR("TROne start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfTROneWrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = TROne::stop();
+
+	if (ret != 0) {
+		PX4_ERR("TROne stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfTROneWrapper::_publish(struct range_sensor_data &data)
+{
+	struct distance_sensor_s d;
+
+	memset(&d, 0, sizeof(d));
+
+	d.timestamp = hrt_absolute_time();
+
+	d.min_distance = float(TRONE_MIN_DISTANCE); /* m */
+
+	d.max_distance = float(TRONE_MAX_DISTANCE); /* m */
+
+	d.current_distance = float(data.dist);
+
+	d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+
+	d.id = 0; // TODO set proper ID
+
+	d.orientation = _rotation;
+
+	d.variance = 0.0f;
+
+	d.signal_quality = -1;
+
+	if (_range_topic == nullptr) {
+		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
+						   &_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
+	}
+
+	return 0;
+};
+
+
+namespace df_trone_wrapper
+{
+
+DfTROneWrapper *g_dev = nullptr;
+
+int start(uint8_t rotation);
+int stop();
+int info();
+int probe();
+void usage();
+
+int start(uint8_t rotation)
+{
+	PX4_ERR("start");
+	g_dev = new DfTROneWrapper(rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfTROneWrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfTROneWrapper start failed");
+		return ret;
+	}
+
+	// Open the range sensor
+	DevHandle h;
+	DevMgr::getHandle(TRONE_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    TRONE_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+/**
+ * Who am i
+ */
+int
+probe()
+{
+	int ret;
+
+	if (g_dev == nullptr) {
+		ret = start(distance_sensor_s::ROTATION_DOWNWARD_FACING);
+
+		if (ret) {
+			PX4_ERR("Failed to start");
+			return ret;
+		}
+	}
+
+	ret = g_dev->probe();
+
+	if (ret) {
+		PX4_ERR("Failed to probe");
+		return ret;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_trone_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_trone_wrapper
+
+
+int
+df_trone_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			df_trone_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_trone_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_trone_wrapper::start(rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_trone_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_trone_wrapper::info();
+	}
+
+	else if (!strcmp(verb, "probe")) {
+		ret = df_trone_wrapper::probe();
+	}
+
+	else {
+		df_trone_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}