cmake remove circular linking and reorganize

- px4_add_module now requires MAIN
 - px4_add_library doesn't automatically link

src/lib/CMakeLists.txt

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   Copyright (c) 2017 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.
+#
+############################################################################
+
+px4_add_git_submodule(TARGET git_ecl PATH "ecl")
+px4_add_git_submodule(TARGET git_matrix PATH "matrix")
+
+add_subdirectory(battery)
+add_subdirectory(controllib)
+add_subdirectory(conversion)
+add_subdirectory(drivers)
+add_subdirectory(ecl)
+add_subdirectory(FlightTasks)
+add_subdirectory(led)
+add_subdirectory(mathlib)
+add_subdirectory(mixer)
+add_subdirectory(perf)
+add_subdirectory(rc)
+add_subdirectory(terrain_estimation)
+add_subdirectory(tunes)
+add_subdirectory(version)

src/lib/FlightTasks/CMakeLists.txt

@@ -30,23 +30,18 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__flight_tasks
-	SRCS
-		tasks/FlightTask.cpp
-		tasks/FlightTaskManual.cpp
-		tasks/FlightTaskManualStabilized.cpp
-		tasks/FlightTaskOrbit.cpp
-		tasks/FlightTaskManualAltitude.cpp
-		tasks/FlightTaskManualAltitudeSmooth.cpp
-		tasks/FlightTaskManualPosition.cpp
-		tasks/FlightTaskManualPositionSmooth.cpp
-		tasks/Utility/ManualSmoothingZ.cpp
-		tasks/Utility/ManualSmoothingXY.cpp
-		SubscriptionArray.cpp
-		FlightTasks.cpp
-	DEPENDS
-		platforms__common
-		modules__uORB
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+
+px4_add_library(flight_tasks
+	tasks/FlightTask.cpp
+	tasks/FlightTaskManual.cpp
+	tasks/FlightTaskManualStabilized.cpp
+	tasks/FlightTaskOrbit.cpp
+	tasks/FlightTaskManualAltitude.cpp
+	tasks/FlightTaskManualAltitudeSmooth.cpp
+	tasks/FlightTaskManualPosition.cpp
+	tasks/FlightTaskManualPositionSmooth.cpp
+	tasks/Utility/ManualSmoothingZ.cpp
+	tasks/Utility/ManualSmoothingXY.cpp
+	SubscriptionArray.cpp
+	FlightTasks.cpp
+)

src/lib/battery/CMakeLists.txt

@@ -0,0 +1,34 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(battery battery.cpp)

src/lib/battery/battery.cpp

@@ -0,0 +1,237 @@
+/****************************************************************************
+ *
+ *   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 battery.cpp
+ *
+ * Library calls for battery functionality.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include "battery.h"
+#include <mathlib/mathlib.h>
+
+Battery::Battery() :
+	ModuleParams(nullptr),
+	_warning(battery_status_s::BATTERY_WARNING_NONE),
+	_last_timestamp(0)
+{
+}
+
+void
+Battery::reset(battery_status_s *battery_status)
+{
+	memset(battery_status, 0, sizeof(*battery_status));
+	battery_status->current_a = -1.f;
+	battery_status->remaining = 1.f;
+	battery_status->scale = 1.f;
+	battery_status->cell_count = _n_cells.get();
+	// TODO: check if it is sane to reset warning to NONE
+	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
+	battery_status->connected = false;
+}
+
+void
+Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
+			     bool connected, bool selected_source, int priority,
+			     float throttle_normalized,
+			     bool armed, battery_status_s *battery_status)
+{
+	reset(battery_status);
+	battery_status->timestamp = timestamp;
+	filterVoltage(voltage_v);
+	filterThrottle(throttle_normalized);
+	filterCurrent(current_a);
+	sumDischarged(timestamp, current_a);
+	estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed);
+	computeScale();
+
+	if (_battery_initialized) {
+		determineWarning(connected);
+	}
+
+	if (_voltage_filtered_v > 2.1f) {
+		_battery_initialized = true;
+		battery_status->voltage_v = voltage_v;
+		battery_status->voltage_filtered_v = _voltage_filtered_v;
+		battery_status->scale = _scale;
+		battery_status->current_a = current_a;
+		battery_status->current_filtered_a = _current_filtered_a;
+		battery_status->discharged_mah = _discharged_mah;
+		battery_status->warning = _warning;
+		battery_status->remaining = _remaining;
+		battery_status->connected = connected;
+		battery_status->system_source = selected_source;
+		battery_status->priority = priority;
+	}
+}
+
+void
+Battery::filterVoltage(float voltage_v)
+{
+	if (!_battery_initialized) {
+		_voltage_filtered_v = voltage_v;
+	}
+
+	// TODO: inspect that filter performance
+	const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;
+
+	if (PX4_ISFINITE(filtered_next)) {
+		_voltage_filtered_v = filtered_next;
+	}
+}
+
+void
+Battery::filterCurrent(float current_a)
+{
+	if (!_battery_initialized) {
+		_current_filtered_a = current_a;
+	}
+
+	// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
+	const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;
+
+	if (PX4_ISFINITE(filtered_next)) {
+		_current_filtered_a = filtered_next;
+	}
+}
+
+void Battery::filterThrottle(float throttle)
+{
+	if (!_battery_initialized) {
+		_throttle_filtered = throttle;
+	}
+
+	const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f;
+
+	if (PX4_ISFINITE(filtered_next)) {
+		_throttle_filtered = filtered_next;
+	}
+}
+
+void
+Battery::sumDischarged(hrt_abstime timestamp, float current_a)
+{
+	// Not a valid measurement
+	if (current_a < 0.f) {
+		// Because the measurement was invalid we need to stop integration
+		// and re-initialize with the next valid measurement
+		_last_timestamp = 0;
+		return;
+	}
+
+	// Ignore first update because we don't know dt.
+	if (_last_timestamp != 0) {
+		const float dt = (timestamp - _last_timestamp) / 1e6;
+		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
+		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
+		_discharged_mah += _discharged_mah_loop;
+	}
+
+	_last_timestamp = timestamp;
+}
+
+void
+Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
+{
+	// remaining battery capacity based on voltage
+	float cell_voltage = voltage_v / _n_cells.get();
+
+	// correct battery voltage locally for load drop to avoid estimation fluctuations
+	if (_r_internal.get() >= 0.f) {
+		cell_voltage += _r_internal.get() * current_a;
+
+	} else {
+		// assume linear relation between throttle and voltage drop
+		cell_voltage += throttle * _v_load_drop.get();
+	}
+
+	_remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f);
+
+	// choose which quantity we're using for final reporting
+	if (_capacity.get() > 0.f) {
+		// if battery capacity is known, fuse voltage measurement with used capacity
+		if (!_battery_initialized) {
+			// initialization of the estimation state
+			_remaining = _remaining_voltage;
+
+		} else {
+			// The lower the voltage the more adjust the estimate with it to avoid deep discharge
+			const float weight_v = 3e-4f * (1 - _remaining_voltage);
+			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
+			// directly apply current capacity slope calculated using current
+			_remaining -= _discharged_mah_loop / _capacity.get();
+			_remaining = math::max(_remaining, 0.f);
+		}
+
+	} else {
+		// else use voltage
+		_remaining = _remaining_voltage;
+	}
+}
+
+void
+Battery::determineWarning(bool connected)
+{
+	if (connected) {
+		// propagate warning state only if the state is higher, otherwise remain in current warning state
+		if (_remaining < _emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
+			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
+
+		} else if (_remaining < _crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
+			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+
+		} else if (_remaining < _low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
+			_warning = battery_status_s::BATTERY_WARNING_LOW;
+		}
+	}
+}
+
+void
+Battery::computeScale()
+{
+	const float voltage_range = (_v_charged.get() - _v_empty.get());
+
+	// reusing capacity calculation to get single cell voltage before drop
+	const float bat_v = _v_empty.get() + (voltage_range * _remaining_voltage);
+
+	_scale = _v_charged.get() / bat_v;
+
+	if (_scale > 1.3f) { // Allow at most 30% compensation
+		_scale = 1.3f;
+
+	} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
+		_scale = 1.f;
+	}
+}

src/lib/battery/battery.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016, 2017 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 battery.h
+ *
+ * Library calls for battery functionality.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#pragma once
+
+#include <uORB/topics/battery_status.h>
+#include <drivers/drv_hrt.h>
+#include <px4_module_params.h>
+
+
+class Battery : public ModuleParams
+{
+public:
+	Battery();
+
+	/**
+	 * Reset all battery stats and report invalid/nothing.
+	 */
+	void reset(battery_status_s *battery_status);
+
+	/**
+	 * Get the battery cell count
+	 */
+	int cell_count() { return _n_cells.get(); }
+
+	/**
+	 * Get the empty voltage per cell
+	 */
+	float empty_cell_voltage() { return _v_empty.get(); }
+
+	/**
+	 * Get the full voltage per cell
+	 */
+	float full_cell_voltage() { return _v_charged.get(); }
+
+	/**
+	 * Update current battery status message.
+	 *
+	 * @param voltage_v: current voltage in V
+	 * @param current_a: current current in A
+	 * @param connected: Battery is connected
+	 * @param selected_source: This battery is on the brick that the selected source for selected_source
+	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
+	 * @param throttle_normalized: throttle from 0 to 1
+	 */
+	void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
+				 bool connected, bool selected_source, int priority,
+				 float throttle_normalized,
+				 bool armed, battery_status_s *status);
+
+private:
+	void filterVoltage(float voltage_v);
+	void filterThrottle(float throttle);
+	void filterCurrent(float current_a);
+	void sumDischarged(hrt_abstime timestamp, float current_a);
+	void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed);
+	void determineWarning(bool connected);
+	void computeScale();
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::BAT_V_EMPTY>) _v_empty,
+		(ParamFloat<px4::params::BAT_V_CHARGED>) _v_charged,
+		(ParamInt<px4::params::BAT_N_CELLS>) _n_cells,
+		(ParamFloat<px4::params::BAT_CAPACITY>) _capacity,
+		(ParamFloat<px4::params::BAT_V_LOAD_DROP>) _v_load_drop,
+		(ParamFloat<px4::params::BAT_R_INTERNAL>) _r_internal,
+		(ParamFloat<px4::params::BAT_LOW_THR>) _low_thr,
+		(ParamFloat<px4::params::BAT_CRIT_THR>) _crit_thr,
+		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _emergency_thr
+	)
+
+	bool _battery_initialized = false;
+	float _voltage_filtered_v = -1.f;
+	float _throttle_filtered = -1.f;
+	float _current_filtered_a = -1.f;
+	float _discharged_mah = 0.f;
+	float _discharged_mah_loop = 0.f;
+	float _remaining_voltage = -1.f;		///< normalized battery charge level remaining based on voltage
+	float _remaining = -1.f;			///< normalized battery charge level, selected based on config param
+	float _scale = 1.f;
+	uint8_t _warning;
+	hrt_abstime _last_timestamp;
+};

src/lib/battery/battery_params.c

@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2017 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 battery_params.c
+ *
+ * Parameters defined by the battery lib.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_config.h>
+#include <parameters/param.h>
+
+/**
+ * Empty cell voltage (5C load)
+ *
+ * Defines the voltage where a single cell of the battery is considered empty.
+ * The voltage should be chosen before the steep dropoff to 2.8V. A typical
+ * lithium battery can only be discharged down to 10% before it drops off
+ * to a voltage level damaging the cells.
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
+
+/**
+ * Full cell voltage (5C load)
+ *
+ * Defines the voltage where a single cell of the battery is considered full
+ * under a mild load. This will never be the nominal voltage of 4.2V
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f);
+
+/**
+ * Low threshold
+ *
+ * Sets the threshold when the battery will be reported as low.
+ * This has to be higher than the critical threshold.
+ *
+ * @group Battery Calibration
+ * @unit norm
+ * @min 0.12
+ * @max 0.4
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f);
+
+/**
+ * Critical threshold
+ *
+ * Sets the threshold when the battery will be reported as critically low.
+ * This has to be lower than the low threshold. This threshold commonly
+ * will trigger RTL.
+ *
+ * @group Battery Calibration
+ * @unit norm
+ * @min 0.05
+ * @max 0.1
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);
+
+/**
+ * Emergency threshold
+ *
+ * Sets the threshold when the battery will be reported as dangerously low.
+ * This has to be lower than the critical threshold. This threshold commonly
+ * will trigger landing.
+ *
+ * @group Battery Calibration
+ * @unit norm
+ * @min 0.03
+ * @max 0.07
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f);
+
+/**
+ * Voltage drop per cell on full throttle
+ *
+ * This implicitely defines the internal resistance
+ * to maximum current ratio and assumes linearity.
+ * A good value to use is the difference between the
+ * 5C and 20-25C load. Not used if BAT_R_INTERNAL is
+ * set.
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @min 0.07
+ * @max 0.5
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
+
+/**
+ * Explicitly defines the per cell internal resistance
+ *
+ * If non-negative, then this will be used in place of
+ * BAT_V_LOAD_DROP for all calculations.
+ *
+ * @group Battery Calibration
+ * @unit Ohms
+ * @min -1.0
+ * @max 0.2
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f);
+
+/**
+ * Number of cells.
+ *
+ * Defines the number of cells the attached battery consists of.
+ *
+ * @group Battery Calibration
+ * @unit S
+ * @value 0 Unconfigured
+ * @value 2 2S Battery
+ * @value 3 3S Battery
+ * @value 4 4S Battery
+ * @value 5 5S Battery
+ * @value 6 6S Battery
+ * @value 7 7S Battery
+ * @value 8 8S Battery
+ * @value 9 9S Battery
+ * @value 10 10S Battery
+ * @value 11 11S Battery
+ * @value 12 12S Battery
+ * @value 13 13S Battery
+ * @value 14 14S Battery
+ * @value 15 15S Battery
+ * @value 16 16S Battery
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(BAT_N_CELLS, 0);
+
+/**
+ * Battery capacity.
+ *
+ * Defines the capacity of the attached battery.
+ *
+ * @group Battery Calibration
+ * @unit mAh
+ * @decimal 0
+ * @min -1.0
+ * @max 100000
+ * @increment 50
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);

src/lib/controllib/CMakeLists.txt

@@ -30,21 +30,16 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__controllib
-	COMPILE_FLAGS
-	SRCS
-		block/Block.cpp
-		block/BlockParam.cpp
-		BlockDerivative.cpp
-		BlockHighPass.cpp
-		BlockIntegral.cpp
-		BlockIntegralTrap.cpp
-		BlockLimit.cpp
-		BlockLimitSym.cpp
-		BlockLowPass.cpp
-		BlockLowPass2.cpp
-	DEPENDS
-		platforms__common
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+
+px4_add_library(controllib
+	block/Block.cpp
+	block/BlockParam.cpp
+	BlockDerivative.cpp
+	BlockHighPass.cpp
+	BlockIntegral.cpp
+	BlockIntegralTrap.cpp
+	BlockLimit.cpp
+	BlockLimitSym.cpp
+	BlockLowPass.cpp
+	BlockLowPass2.cpp
+)

src/lib/controllib/block/BlockParam.hpp

@@ -43,7 +43,7 @@
 
 #include <containers/List.hpp>
 #include <px4_defines.h>
-#include <systemlib/param/param.h>
+#include <parameters/param.h>
 
 namespace control
 {

src/lib/controllib/controllib_test/CMakeLists.txt

@@ -37,7 +37,6 @@ px4_add_module(
 	SRCS
 		controllib_test_main.cpp
 	DEPENDS
-		platforms__common
 	)
 
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+

src/lib/controllib/controllib_test/test_params.c

@@ -1,4 +1,4 @@
-#include <systemlib/param/param.h>
+#include <parameters/param.h>
 // WARNING:
 //  do not changes these unless
 //  you want to recompute the

src/lib/conversion/CMakeLists.txt

@@ -30,12 +30,5 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__conversion
-	COMPILE_FLAGS
-	SRCS
-		rotation.cpp
-	DEPENDS
-		platforms__common
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+
+px4_add_library(conversion rotation.cpp)

src/lib/drivers/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2017 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.
+#
+############################################################################
+
+add_subdirectory(airspeed)
+add_subdirectory(device)
+add_subdirectory(led)

src/lib/drivers/airspeed/CMakeLists.txt

@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(drivers__airspeed airspeed.cpp)
+target_link_libraries(drivers__airspeed PRIVATE drivers__device)

src/lib/drivers/airspeed/airspeed.cpp

@@ -0,0 +1,280 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed.cpp
+ * @author Simon Wilks <simon@px4.io>
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ */
+
+#include <px4_config.h>
+#include <drivers/device/device.h>
+
+#include <drivers/device/i2c.h>
+
+#include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <parameters/param.h>
+#include <perf/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <drivers/airspeed/airspeed.h>
+
+Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
+	I2C("Airspeed", path, bus, address, 100000),
+	_sensor_ok(false),
+	_last_published_sensor_ok(true), /* initialize differently to force publication */
+	_measure_ticks(0),
+	_collect_phase(false),
+	_diff_pres_offset(0.0f),
+	_airspeed_pub(nullptr),
+	_airspeed_orb_class_instance(-1),
+	_subsys_pub(nullptr),
+	_class_instance(-1),
+	_conversion_interval(conversion_interval),
+	_sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "aspd_com_err"))
+{
+	// enable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+Airspeed::~Airspeed()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	if (_class_instance != -1) {
+		unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	orb_unadvertise(_airspeed_pub);
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+Airspeed::init()
+{
+	/* do I2C init (and probe) first */
+	if (I2C::init() != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
+
+	/* advertise sensor topic, measure manually to initialize valid report */
+	measure();
+	differential_pressure_s arp = {};
+
+	/* measurement will have generated a report, publish */
+	_airspeed_pub = orb_advertise_multi(ORB_ID(differential_pressure), &arp, &_airspeed_orb_class_instance,
+					    ORB_PRIO_HIGH - _class_instance);
+
+	if (_airspeed_pub == nullptr) {
+		PX4_WARN("uORB started?");
+	}
+
+	return PX4_OK;
+}
+
+int
+Airspeed::probe()
+{
+	/* on initial power up the device may need more than one retry
+	   for detection. Once it is running the number of retries can
+	   be reduced
+	*/
+	_retries = 4;
+	int ret = measure();
+
+	// drop back to 2 retries once initialised
+	_retries = 2;
+	return ret;
+}
+
+int
+Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signaling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(_conversion_interval);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(_conversion_interval)) {
+						return -EINVAL;
+					}
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+		break;
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0) {
+			return SENSOR_POLLRATE_MANUAL;
+		}
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case AIRSPEEDIOCSSCALE: {
+			struct airspeed_scale *s = (struct airspeed_scale *)arg;
+			_diff_pres_offset = s->offset_pa;
+			return OK;
+		}
+
+	case AIRSPEEDIOCGSCALE: {
+			struct airspeed_scale *s = (struct airspeed_scale *)arg;
+			s->offset_pa = _diff_pres_offset;
+			s->scale = 1.0f;
+			return OK;
+		}
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+void
+Airspeed::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
+}
+
+void
+Airspeed::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+Airspeed::update_status()
+{
+	if (_sensor_ok != _last_published_sensor_ok) {
+		/* notify about state change */
+		struct subsystem_info_s info = {};
+		info.present = true;
+		info.enabled = true;
+		info.ok = _sensor_ok;
+		info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
+
+		if (_subsys_pub != nullptr) {
+			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
+
+		} else {
+			_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
+		}
+
+		_last_published_sensor_ok = _sensor_ok;
+	}
+}
+
+void
+Airspeed::cycle_trampoline(void *arg)
+{
+	Airspeed *dev = (Airspeed *)arg;
+	dev->cycle();
+
+	dev->update_status();
+}

src/lib/drivers/airspeed/airspeed.h

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <string.h>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_workqueue.h>
+#include <systemlib/airspeed.h>
+#include <perf/perf_counter.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/uORB.h>
+
+/* Default I2C bus */
+static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION;
+
+class __EXPORT Airspeed : public device::I2C
+{
+public:
+	Airspeed(int bus, int address, unsigned conversion_interval, const char *path);
+	virtual ~Airspeed();
+
+	virtual int	init();
+
+	virtual int	ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+private:
+	/* this class has pointer data members and should not be copied */
+	Airspeed(const Airspeed &);
+	Airspeed &operator=(const Airspeed &);
+
+protected:
+	virtual int	probe();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	virtual void	cycle() = 0;
+	virtual int	measure() = 0;
+	virtual int	collect() = 0;
+
+	/**
+	 * Update the subsystem status
+	 */
+	void update_status();
+
+	work_s			_work;
+	bool			_sensor_ok;
+	bool			_last_published_sensor_ok;
+	uint32_t		_measure_ticks;
+	bool			_collect_phase;
+	float			_diff_pres_offset;
+
+	orb_advert_t		_airspeed_pub;
+	int			_airspeed_orb_class_instance;
+
+	orb_advert_t		_subsys_pub;
+
+	int			_class_instance;
+
+	unsigned		_conversion_interval;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void	start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void	stop();
+
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void	cycle_trampoline(void *arg);
+
+	/**
+	* add a new report to the reports queue
+	*
+	* @param report		differential_pressure_s report
+	*/
+	void	new_report(const differential_pressure_s &report);
+};
+
+

src/lib/drivers/device/CDev.cpp

@@ -0,0 +1,431 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 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 CDev.cpp
+ *
+ * Character device base class.
+ */
+
+#include "CDev.hpp"
+
+#include <cstring>
+
+#include "px4_posix.h"
+#include "drivers/drv_device.h"
+
+namespace device
+{
+
+CDev::CDev(const char *name, const char *devname) :
+	Device(name),
+	_devname(devname)
+{
+	DEVICE_DEBUG("CDev::CDev");
+
+	int ret = px4_sem_init(&_lock, 0, 1);
+
+	if (ret != 0) {
+		PX4_WARN("SEM INIT FAIL: ret %d", ret);
+	}
+}
+
+CDev::~CDev()
+{
+	DEVICE_DEBUG("CDev::~CDev");
+
+	if (_registered) {
+		unregister_driver(_devname);
+	}
+
+	if (_pollset) {
+		delete[](_pollset);
+	}
+
+	px4_sem_destroy(&_lock);
+}
+
+int
+CDev::register_class_devname(const char *class_devname)
+{
+	DEVICE_DEBUG("CDev::register_class_devname %s", class_devname);
+
+	if (class_devname == nullptr) {
+		return -EINVAL;
+	}
+
+	int class_instance = 0;
+	int ret = -ENOSPC;
+
+	while (class_instance < 4) {
+		char name[32];
+		snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
+		ret = register_driver(name, &fops, 0666, (void *)this);
+
+		if (ret == OK) {
+			break;
+		}
+
+		class_instance++;
+	}
+
+	if (class_instance == 4) {
+		return ret;
+	}
+
+	return class_instance;
+}
+
+int
+CDev::unregister_class_devname(const char *class_devname, unsigned class_instance)
+{
+	DEVICE_DEBUG("CDev::unregister_class_devname");
+	char name[32];
+	snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+	return unregister_driver(name);
+}
+
+int
+CDev::init()
+{
+	DEVICE_DEBUG("CDev::init");
+
+	// base class init first
+	int ret = Device::init();
+
+	if (ret != PX4_OK) {
+		goto out;
+	}
+
+	// now register the driver
+	if (_devname != nullptr) {
+		ret = register_driver(_devname, &fops, 0666, (void *)this);
+
+		if (ret != PX4_OK) {
+			goto out;
+		}
+
+		_registered = true;
+	}
+
+out:
+	return ret;
+}
+
+/*
+ * Default implementations of the character device interface
+ */
+int
+CDev::open(file_t *filep)
+{
+	DEVICE_DEBUG("CDev::open");
+	int ret = PX4_OK;
+
+	lock();
+	/* increment the open count */
+	_open_count++;
+
+	if (_open_count == 1) {
+
+		/* first-open callback may decline the open */
+		ret = open_first(filep);
+
+		if (ret != PX4_OK) {
+			_open_count--;
+		}
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::open_first(file_t *filep)
+{
+	DEVICE_DEBUG("CDev::open_first");
+	return PX4_OK;
+}
+
+int
+CDev::close(file_t *filep)
+{
+	DEVICE_DEBUG("CDev::close");
+	int ret = PX4_OK;
+
+	lock();
+
+	if (_open_count > 0) {
+		/* decrement the open count */
+		_open_count--;
+
+		/* callback cannot decline the close */
+		if (_open_count == 0) {
+			ret = close_last(filep);
+		}
+
+	} else {
+		ret = -EBADF;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::close_last(file_t *filep)
+{
+	DEVICE_DEBUG("CDev::close_last");
+	return PX4_OK;
+}
+
+ssize_t
+CDev::read(file_t *filep, char *buffer, size_t buflen)
+{
+	DEVICE_DEBUG("CDev::read");
+	return -ENOSYS;
+}
+
+ssize_t
+CDev::write(file_t *filep, const char *buffer, size_t buflen)
+{
+	DEVICE_DEBUG("CDev::write");
+	return -ENOSYS;
+}
+
+off_t
+CDev::seek(file_t *filep, off_t offset, int whence)
+{
+	DEVICE_DEBUG("CDev::seek");
+	return -ENOSYS;
+}
+
+int
+CDev::ioctl(file_t *filep, int cmd, unsigned long arg)
+{
+	DEVICE_DEBUG("CDev::ioctl");
+	int ret = -ENOTTY;
+
+	switch (cmd) {
+
+	/* fetch a pointer to the driver's private data */
+	case DIOC_GETPRIV:
+		*(void **)(uintptr_t)arg = (void *)this;
+		ret = PX4_OK;
+		break;
+
+	case DEVIOCSPUBBLOCK:
+		_pub_blocked = (arg != 0);
+		ret = PX4_OK;
+		break;
+
+	case DEVIOCGPUBBLOCK:
+		ret = _pub_blocked;
+		break;
+
+	case DEVIOCGDEVICEID:
+		ret = (int)_device_id.devid;
+		break;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+int
+CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
+{
+	DEVICE_DEBUG("CDev::Poll %s", setup ? "setup" : "teardown");
+	int ret = PX4_OK;
+
+	/*
+	 * Lock against pollnotify() (and possibly other callers)
+	 */
+	lock();
+
+	if (setup) {
+		/*
+		 * Save the file pointer in the pollfd for the subclass'
+		 * benefit.
+		 */
+		fds->priv = (void *)filep;
+		DEVICE_DEBUG("CDev::poll: fds->priv = %p", filep);
+
+		/*
+		 * Handle setup requests.
+		 */
+		ret = store_poll_waiter(fds);
+
+		if (ret == PX4_OK) {
+
+			/*
+			 * Check to see whether we should send a poll notification
+			 * immediately.
+			 */
+			fds->revents |= fds->events & poll_state(filep);
+
+			/* yes? post the notification */
+			if (fds->revents != 0) {
+				px4_sem_post(fds->sem);
+			}
+
+		} else {
+			PX4_WARN("Store Poll Waiter error.");
+		}
+
+	} else {
+		/*
+		 * Handle a teardown request.
+		 */
+		ret = remove_poll_waiter(fds);
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+CDev::poll_notify(pollevent_t events)
+{
+	DEVICE_DEBUG("CDev::poll_notify events = %0x", events);
+
+	/* lock against poll() as well as other wakeups */
+	ATOMIC_ENTER;
+
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (nullptr != _pollset[i]) {
+			poll_notify_one(_pollset[i], events);
+		}
+	}
+
+	ATOMIC_LEAVE;
+}
+
+void
+CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
+{
+	DEVICE_DEBUG("CDev::poll_notify_one");
+
+#ifdef __PX4_NUTTX
+	int value = fds->sem->semcount;
+#else
+	int value = -1;
+	px4_sem_getvalue(fds->sem, &value);
+#endif
+
+	/* update the reported event set */
+	fds->revents |= fds->events & events;
+
+	DEVICE_DEBUG(" Events fds=%p %0x %0x %0x %d", fds, fds->revents, fds->events, events, value);
+
+	/* if the state is now interesting, wake the waiter if it's still asleep */
+	/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
+	if ((fds->revents != 0) && (value <= 0)) {
+		px4_sem_post(fds->sem);
+	}
+}
+
+pollevent_t
+CDev::poll_state(file_t *filep)
+{
+	DEVICE_DEBUG("CDev::poll_notify");
+	/* by default, no poll events to report */
+	return 0;
+}
+
+int
+CDev::store_poll_waiter(px4_pollfd_struct_t *fds)
+{
+	/*
+	 * Look for a free slot.
+	 */
+	DEVICE_DEBUG("CDev::store_poll_waiter");
+
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (nullptr == _pollset[i]) {
+
+			/* save the pollfd */
+			_pollset[i] = fds;
+
+			return PX4_OK;
+		}
+	}
+
+	/* No free slot found. Resize the pollset */
+
+	if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t
+		return -ENOMEM;
+	}
+
+	const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1;
+	px4_pollfd_struct_t **new_pollset = new px4_pollfd_struct_t *[new_count];
+
+	if (!new_pollset) {
+		return -ENOMEM;
+	}
+
+	if (_max_pollwaiters > 0) {
+		memset(new_pollset + _max_pollwaiters, 0, sizeof(px4_pollfd_struct_t *) * (new_count - _max_pollwaiters));
+		memcpy(new_pollset, _pollset, sizeof(px4_pollfd_struct_t *) * _max_pollwaiters);
+		delete[](_pollset);
+	}
+
+	_pollset = new_pollset;
+	_pollset[_max_pollwaiters] = fds;
+	_max_pollwaiters = new_count;
+	return PX4_OK;
+}
+
+int
+CDev::remove_poll_waiter(px4_pollfd_struct_t *fds)
+{
+	DEVICE_DEBUG("CDev::remove_poll_waiter");
+
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (fds == _pollset[i]) {
+
+			_pollset[i] = nullptr;
+			return PX4_OK;
+
+		}
+	}
+
+	PX4_WARN("poll: bad fd state");
+	return -EINVAL;
+}
+
+} // namespace device

src/lib/drivers/device/CDev.hpp

@@ -0,0 +1,319 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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 CDev.hpp
+ *
+ * Definitions for the generic base classes in the device framework.
+ */
+
+#ifndef _DEVICE_CDEV_HPP
+#define _DEVICE_CDEV_HPP
+
+#include "Device.hpp"
+
+#include <px4_config.h>
+#include <px4_posix.h>
+
+#ifdef __PX4_NUTTX
+#include "nuttx/cdev_platform.hpp"
+#else
+#include "posix/cdev_platform.hpp"
+#endif
+
+/**
+ * Namespace encapsulating all device framework classes, functions and data.
+ */
+namespace device
+{
+
+/**
+ * Abstract class for any character device
+ */
+class __EXPORT CDev : public Device
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 */
+	CDev(const char *name, const char *devname); // TODO: dagar remove name and Device inheritance
+
+	virtual ~CDev();
+
+	virtual int	init();
+
+	/**
+	 * Handle an open of the device.
+	 *
+	 * This function is called for every open of the device. The default
+	 * implementation maintains _open_count and always returns OK.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @return		OK if the open is allowed, -errno otherwise.
+	 */
+	virtual int	open(file_t *filep);
+
+	/**
+	 * Handle a close of the device.
+	 *
+	 * This function is called for every close of the device. The default
+	 * implementation maintains _open_count and returns OK as long as it is not zero.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @return		OK if the close was successful, -errno otherwise.
+	 */
+	virtual int	close(file_t *filep);
+
+	/**
+	 * Perform a read from the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @param buffer	Pointer to the buffer into which data should be placed.
+	 * @param buflen	The number of bytes to be read.
+	 * @return		The number of bytes read or -errno otherwise.
+	 */
+	virtual ssize_t	read(file_t *filep, char *buffer, size_t buflen);
+
+	/**
+	 * Perform a write to the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @param buffer	Pointer to the buffer from which data should be read.
+	 * @param buflen	The number of bytes to be written.
+	 * @return		The number of bytes written or -errno otherwise.
+	 */
+	virtual ssize_t	write(file_t *filep, const char *buffer, size_t buflen);
+
+	/**
+	 * Perform a logical seek operation on the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @param offset	The new file position relative to whence.
+	 * @param whence	SEEK_OFS, SEEK_CUR or SEEK_END.
+	 * @return		The previous offset, or -errno otherwise.
+	 */
+	virtual off_t	seek(file_t *filep, off_t offset, int whence);
+
+	/**
+	 * Perform an ioctl operation on the device.
+	 *
+	 * The default implementation handles DIOC_GETPRIV, and otherwise
+	 * returns -ENOTTY. Subclasses should call the default implementation
+	 * for any command they do not handle themselves.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @param cmd		The ioctl command value.
+	 * @param arg		The ioctl argument value.
+	 * @return		OK on success, or -errno otherwise.
+	 */
+	virtual int	ioctl(file_t *filep, int cmd, unsigned long arg);
+
+	/**
+	 * Perform a poll setup/teardown operation.
+	 *
+	 * This is handled internally and should not normally be overridden.
+	 *
+	 * @param filep		Pointer to the internal file structure.
+	 * @param fds		Poll descriptor being waited on.
+	 * @param setup		True if this is establishing a request, false if
+	 *			it is being torn down.
+	 * @return		OK on success, or -errno otherwise.
+	 */
+	virtual int	poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup);
+
+protected:
+	/**
+	 * Pointer to the default cdev file operations table; useful for
+	 * registering clone devices etc.
+	 */
+	static const px4_file_operations_t	fops;
+
+	/**
+	 * Check the current state of the device for poll events from the
+	 * perspective of the file.
+	 *
+	 * This function is called by the default poll() implementation when
+	 * a poll is set up to determine whether the poll should return immediately.
+	 *
+	 * The default implementation returns no events.
+	 *
+	 * @param filep		The file that's interested.
+	 * @return		The current set of poll events.
+	 */
+	virtual pollevent_t poll_state(file_t *filep);
+
+	/**
+	 * Report new poll events.
+	 *
+	 * This function should be called anytime the state of the device changes
+	 * in a fashion that might be interesting to a poll waiter.
+	 *
+	 * @param events	The new event(s) being announced.
+	 */
+	virtual void	poll_notify(pollevent_t events);
+
+	/**
+	 * Internal implementation of poll_notify.
+	 *
+	 * @param fds		A poll waiter to notify.
+	 * @param events	The event(s) to send to the waiter.
+	 */
+	virtual void	poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events);
+
+	/**
+	 * Notification of the first open.
+	 *
+	 * This function is called when the device open count transitions from zero
+	 * to one.  The driver lock is held for the duration of the call.
+	 *
+	 * The default implementation returns OK.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @return		OK if the open should proceed, -errno otherwise.
+	 */
+	virtual int	open_first(file_t *filep);
+
+	/**
+	 * Notification of the last close.
+	 *
+	 * This function is called when the device open count transitions from
+	 * one to zero.  The driver lock is held for the duration of the call.
+	 *
+	 * The default implementation returns OK.
+	 *
+	 * @param filep		Pointer to the NuttX file structure.
+	 * @return		OK if the open should return OK, -errno otherwise.
+	 */
+	virtual int	close_last(file_t *filep);
+
+	/**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @return class_instamce Class instance created, or -errno on failure
+	 */
+	virtual int register_class_devname(const char *class_devname);
+
+	/**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @param class_instance  Device class instance from register_class_devname()
+	 * @return		  OK on success, -errno otherwise
+	 */
+	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
+
+	/**
+	 * Get the device name.
+	 *
+	 * @return the file system string of the device handle
+	 */
+	const char	*get_devname() { return _devname; }
+
+	/**
+	 * Take the driver lock.
+	 *
+	 * Each driver instance has its own lock/semaphore.
+	 *
+	 * Note that we must loop as the wait may be interrupted by a signal.
+	 *
+	 * Careful: lock() calls cannot be nested!
+	 */
+	void		lock()
+	{
+		do {} while (px4_sem_wait(&_lock) != 0);
+	}
+
+	/**
+	 * Release the driver lock.
+	 */
+	void		unlock()
+	{
+		px4_sem_post(&_lock);
+	}
+
+	px4_sem_t	_lock; /**< lock to protect access to all class members (also for derived classes) */
+
+	bool		_pub_blocked{false};		/**< true if publishing should be blocked */
+
+private:
+	const char	*_devname;		/**< device node name */
+	bool		_registered{false};		/**< true if device name was registered */
+
+	uint8_t		_max_pollwaiters{0}; /**< size of the _pollset array */
+	uint16_t	_open_count{0};		/**< number of successful opens */
+
+	px4_pollfd_struct_t	**_pollset{nullptr};
+
+	/**
+	 * Store a pollwaiter in a slot where we can find it later.
+	 *
+	 * Expands the pollset as required.  Must be called with the driver locked.
+	 *
+	 * @return		OK, or -errno on error.
+	 */
+	int		store_poll_waiter(px4_pollfd_struct_t *fds);
+
+	/**
+	 * Remove a poll waiter.
+	 *
+	 * @return		OK, or -errno on error.
+	 */
+	int		remove_poll_waiter(px4_pollfd_struct_t *fds);
+
+	/* do not allow copying this class */
+	CDev(const CDev &);
+	CDev operator=(const CDev &);
+};
+
+} // namespace device
+
+// class instance for primary driver of each class
+enum CLASS_DEVICE {
+	CLASS_DEVICE_PRIMARY = 0,
+	CLASS_DEVICE_SECONDARY = 1,
+	CLASS_DEVICE_TERTIARY = 2
+};
+
+#endif /* _DEVICE_CDEV_HPP */

src/lib/drivers/device/CMakeLists.txt

@@ -0,0 +1,63 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(SRCS_PLATFORM)
+if (${OS} STREQUAL "nuttx")
+	list(APPEND SRCS_PLATFORM
+		nuttx/cdev_platform.cpp
+	)
+	if ("${CONFIG_I2C}" STREQUAL "y")
+		list(APPEND SRCS_PLATFORM nuttx/I2C.cpp)
+	endif()
+
+	if ("${CONFIG_SPI}" STREQUAL "y")
+		list(APPEND SRCS_PLATFORM nuttx/SPI.cpp)
+	endif()
+else()
+	list(APPEND SRCS_PLATFORM
+		posix/I2C.cpp
+		posix/cdev_platform.cpp
+		posix/vfile.cpp
+	)
+endif()
+
+px4_add_library(drivers__device
+	CDev.cpp
+	ringbuffer.cpp
+	integrator.cpp
+	${SRCS_PLATFORM}
+	)
+
+if (${OS} STREQUAL "nuttx")
+	target_link_libraries(drivers__device PRIVATE nuttx_arch)
+endif()

src/lib/drivers/device/Device.hpp

@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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 Device.hpp
+ *
+ * Definitions for the generic base classes in the device framework.
+ */
+
+#ifndef _DEVICE_DEVICE_HPP
+#define _DEVICE_DEVICE_HPP
+
+/*
+ * Includes here should only cover the needs of the framework definitions.
+ */
+#include <px4_config.h>
+#include <px4_posix.h>
+
+#include <drivers/drv_device.h>
+
+#define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__)
+#define DEVICE_DEBUG(FMT, ...) PX4_LOG_NAMED_COND(_name, _debug_enabled, FMT, ##__VA_ARGS__)
+
+/**
+ * Namespace encapsulating all device framework classes, functions and data.
+ */
+namespace device
+{
+
+/**
+ * Fundamental base class for all physical drivers (I2C, SPI).
+ *
+ * This class provides the basic driver template for I2C and SPI devices
+ */
+class __EXPORT Device
+{
+public:
+	/**
+	 * Destructor.
+	 *
+	 * Public so that anonymous devices can be destroyed.
+	 */
+	virtual ~Device() = default;
+
+	/*
+	 * Direct access methods.
+	 */
+
+	/**
+	 * Initialise the driver and make it ready for use.
+	 *
+	 * @return	OK if the driver initialized OK, negative errno otherwise;
+	 */
+	virtual int	init() { return PX4_OK; }
+
+	/**
+	 * Read directly from the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param offset	The device address at which to start reading
+	 * @param data		The buffer into which the read values should be placed.
+	 * @param count		The number of items to read.
+	 * @return		The number of items read on success, negative errno otherwise.
+	 */
+	virtual int	read(unsigned address, void *data, unsigned count) { return -ENODEV; }
+
+	/**
+	 * Write directly to the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param address	The device address at which to start writing.
+	 * @param data		The buffer from which values should be read.
+	 * @param count		The number of items to write.
+	 * @return		The number of items written on success, negative errno otherwise.
+	 */
+	virtual int	write(unsigned address, void *data, unsigned count) { return -ENODEV; }
+
+	/**
+	 * Perform a device-specific operation.
+	 *
+	 * @param operation	The operation to perform.
+	 * @param arg		An argument to the operation.
+	 * @return		Negative errno on error, OK or positive value on success.
+	 */
+	virtual int	ioctl(unsigned operation, unsigned &arg)
+	{
+		switch (operation) {
+		case DEVIOCGDEVICEID:
+			return (int)_device_id.devid;
+		}
+
+		return -ENODEV;
+	}
+
+	/** Device bus types for DEVID */
+	enum DeviceBusType {
+		DeviceBusType_UNKNOWN = 0,
+		DeviceBusType_I2C     = 1,
+		DeviceBusType_SPI     = 2,
+		DeviceBusType_UAVCAN  = 3,
+	};
+
+	/**
+	 * Return the bus ID the device is connected to.
+	 *
+	 * @return The bus ID
+	 */
+	uint8_t get_device_bus() { return _device_id.devid_s.bus; }
+
+	/**
+	 * Return the bus type the device is connected to.
+	 *
+	 * @return The bus type
+	 */
+	DeviceBusType get_device_bus_type() { return _device_id.devid_s.bus_type; }
+
+	/**
+	 * Return the bus address of the device.
+	 *
+	 * @return The bus address
+	 */
+	uint8_t get_device_address() { return _device_id.devid_s.address; }
+
+	void set_device_address(int address) { _device_id.devid_s.address = address; }
+
+	/**
+	 * Set the device type
+	 *
+	 * @return The device type
+	 */
+	void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; }
+
+	virtual bool external() { return false; }
+
+	/*
+	  broken out device elements. The bitfields are used to keep
+	  the overall value small enough to fit in a float accurately,
+	  which makes it possible to transport over the MAVLink
+	  parameter protocol without loss of information.
+	 */
+	struct DeviceStructure {
+		enum DeviceBusType bus_type : 3;
+		uint8_t bus: 5;    // which instance of the bus type
+		uint8_t address;   // address on the bus (eg. I2C address)
+		uint8_t devtype;   // device class specific device type
+	};
+
+	union DeviceId {
+		struct DeviceStructure devid_s;
+		uint32_t devid;
+	};
+
+protected:
+	union DeviceId	_device_id;             /**< device identifier information */
+
+	const char	*_name;			/**< driver name */
+	bool		_debug_enabled{false};		/**< if true, debug messages are printed */
+
+	Device(const char *name) : _name(name)
+	{
+		/* setup a default device ID. When bus_type is UNKNOWN the
+		   other fields are invalid */
+		_device_id.devid = 0;
+		_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
+		_device_id.devid_s.bus = 0;
+		_device_id.devid_s.address = 0;
+		_device_id.devid_s.devtype = 0;
+	}
+
+	Device(DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
+	{
+		_device_id.devid = 0;
+		_device_id.devid_s.bus_type = bus_type;
+		_device_id.devid_s.bus = bus;
+		_device_id.devid_s.address = address;
+		_device_id.devid_s.devtype = devtype;
+	}
+
+	// no copy, assignment, move, move assignment
+	Device(const Device &) = delete;
+	Device &operator=(const Device &) = delete;
+	Device(Device &&) = delete;
+	Device &operator=(Device &&) = delete;
+
+};
+
+} // namespace device
+
+#endif /* _DEVICE_DEVICE_HPP */

src/lib/drivers/device/device.h

@@ -0,0 +1,37 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "Device.hpp"
+#include "CDev.hpp"

src/lib/drivers/device/i2c.h

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#pragma once
+
+#ifdef __PX4_NUTTX
+#include "nuttx/I2C.hpp"
+#else
+#include "posix/I2C.hpp"
+#endif

src/lib/drivers/device/integrator.cpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-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 integrator.cpp
+ *
+ * A resettable integrator
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include "integrator.h"
+#include <drivers/drv_hrt.h>
+
+Integrator::Integrator(uint64_t auto_reset_interval, bool coning_compensation) :
+	_auto_reset_interval(auto_reset_interval),
+	_last_integration_time(0),
+	_last_reset_time(0),
+	_alpha(0.0f, 0.0f, 0.0f),
+	_last_alpha(0.0f, 0.0f, 0.0f),
+	_beta(0.0f, 0.0f, 0.0f),
+	_last_val(0.0f, 0.0f, 0.0f),
+	_last_delta_alpha(0.0f, 0.0f, 0.0f),
+	_coning_comp_on(coning_compensation)
+{
+
+}
+
+Integrator::~Integrator()
+{
+
+}
+
+bool
+Integrator::put(uint64_t timestamp, matrix::Vector3f &val, matrix::Vector3f &integral, uint64_t &integral_dt)
+{
+	if (_last_integration_time == 0) {
+		/* this is the first item in the integrator */
+		_last_integration_time = timestamp;
+		_last_reset_time = timestamp;
+		_last_val = val;
+
+		return false;
+	}
+
+	double dt = 0.0;
+
+	// Integrate:
+	// Leave dt at 0 if the integration time does not make sense.
+	// Without this check the integral is likely to explode.
+	if (timestamp >= _last_integration_time) {
+		dt = (double)(timestamp - _last_integration_time) / 1000000.0;
+	}
+
+	// Use trapezoidal integration to calculate the delta integral
+	matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f;
+	_last_val = val;
+
+	// Calculate coning corrections if required
+	if (_coning_comp_on) {
+		// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
+		// following:
+		// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
+		// Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
+		// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
+		_beta += ((_last_alpha + _last_delta_alpha * (1.0f / 6.0f)) % delta_alpha) * 0.5f;
+		_last_delta_alpha = delta_alpha;
+		_last_alpha = _alpha;
+	}
+
+	// accumulate delta integrals
+	_alpha += delta_alpha;
+
+	_last_integration_time = timestamp;
+
+	// Only do auto reset if auto reset interval is not 0.
+	if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) >= _auto_reset_interval) {
+
+		// apply coning corrections if required
+		if (_coning_comp_on) {
+			integral = _alpha + _beta;
+
+		} else {
+			integral = _alpha;
+		}
+
+		// reset the integrals and coning corrections
+		_reset(integral_dt);
+
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+bool
+Integrator::put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral,
+			      uint64_t &integral_dt)
+{
+	if (_last_integration_time == 0) {
+		/* this is the first item in the integrator */
+		uint64_t now = hrt_absolute_time();
+		_last_integration_time = now;
+		_last_reset_time = now;
+		_last_val = val;
+
+		return false;
+	}
+
+	// Create the timestamp artifically.
+	uint64_t timestamp = _last_integration_time + interval_us;
+
+	return put(timestamp, val, integral, integral_dt);
+}
+
+matrix::Vector3f
+Integrator::get(bool reset, uint64_t &integral_dt)
+{
+	matrix::Vector3f val = _alpha;
+
+	if (reset) {
+		_reset(integral_dt);
+	}
+
+	return val;
+}
+
+matrix::Vector3f
+Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, matrix::Vector3f &filtered_val)
+{
+	// Do the usual get with reset first but don't return yet.
+	matrix::Vector3f ret_integral = get(reset, integral_dt);
+
+	// Because we need both the integral and the integral_dt.
+	filtered_val(0) = ret_integral(0) * 1000000 / integral_dt;
+	filtered_val(1) = ret_integral(1) * 1000000 / integral_dt;
+	filtered_val(2) = ret_integral(2) * 1000000 / integral_dt;
+
+	return ret_integral;
+}
+
+void
+Integrator::_reset(uint64_t &integral_dt)
+{
+	_alpha(0) = 0.0f;
+	_alpha(1) = 0.0f;
+	_alpha(2) = 0.0f;
+	_last_alpha(0) = 0.0f;
+	_last_alpha(1) = 0.0f;
+	_last_alpha(2) = 0.0f;
+	_beta(0) = 0.0f;
+	_beta(1) = 0.0f;
+	_beta(2) = 0.0f;
+
+	integral_dt = (_last_integration_time - _last_reset_time);
+	_last_reset_time = _last_integration_time;
+}

src/lib/drivers/device/integrator.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-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 integrator.h
+ *
+ * A resettable integrator
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#pragma once
+
+#include <mathlib/mathlib.h>
+
+class Integrator
+{
+public:
+	Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
+	virtual ~Integrator();
+
+	/**
+	 * Put an item into the integral.
+	 *
+	 * @param timestamp	Timestamp of the current value.
+	 * @param val		Item to put.
+	 * @param integral	Current integral in case the integrator did reset, else the value will not be modified
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
+	 * @return		true if putting the item triggered an integral reset and the integral should be
+	 *			published.
+	 */
+	bool put(uint64_t timestamp, matrix::Vector3f &val, matrix::Vector3f &integral, uint64_t &integral_dt);
+
+	/**
+	 * Put an item into the integral but provide an interval instead of a timestamp.
+	 *
+	 * @param interval_us	Interval in us since last integration.
+	 * @param val		Item to put.
+	 * @param integral	Current integral in case the integrator did reset, else the value will not be modified
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset). Note that this
+	 *			values might not be accurate vs. hrt_absolute_time because it is just the sum of the
+	 *			supplied intervals.
+	 * @return		true if putting the item triggered an integral reset and the integral should be
+	 *			published.
+	 */
+	bool put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral,
+			       uint64_t &integral_dt);
+
+	/**
+	 * Get the current integral and reset the integrator if needed.
+	 *
+	 * @param reset	    	Reset the integral to zero.
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
+	 * @return		the integral since the last read-reset
+	 */
+	matrix::Vector3f	get(bool reset, uint64_t &integral_dt);
+
+	/**
+	 * Get the current integral and reset the integrator if needed. Additionally give the
+	 * integral over the samples differentiated by the integration time (mean filtered values).
+	 *
+	 * @param reset	    	Reset the integral to zero.
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
+	 * @param filtered_val	The integral differentiated by the integration time.
+	 * @return		the integral since the last read-reset
+	 */
+	matrix::Vector3f	get_and_filtered(bool reset, uint64_t &integral_dt, matrix::Vector3f &filtered_val);
+
+private:
+	uint64_t _auto_reset_interval;			/**< the interval after which the content will be published
+							     and the integrator reset, 0 if no auto-reset */
+	uint64_t _last_integration_time;		/**< timestamp of the last integration step */
+	uint64_t _last_reset_time;			/**< last auto-announcement of integral value */
+	matrix::Vector3f _alpha;			/**< integrated value before coning corrections are applied */
+	matrix::Vector3f _last_alpha;			/**< previous value of _alpha */
+	matrix::Vector3f _beta;				/**< accumulated coning corrections */
+	matrix::Vector3f _last_val;			/**< previous input */
+	matrix::Vector3f _last_delta_alpha;		/**< integral from previous previous sampling interval */
+	bool _coning_comp_on;				/**< true to turn on coning corrections */
+
+	/* we don't want this class to be copied */
+	Integrator(const Integrator &);
+	Integrator operator=(const Integrator &);
+
+	/* Do a reset.
+	 *
+	 * @param integral_dt	Get the dt in us of the current integration.
+	 */
+	void _reset(uint64_t &integral_dt);
+};

src/lib/drivers/device/nuttx/I2C.cpp

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c.cpp
+ *
+ * Base class for devices attached via the I2C bus.
+ *
+ * @todo Bus frequency changes; currently we do nothing with the value
+ *       that is supplied.  Should we just depend on the bus knowing?
+ */
+
+#include "I2C.hpp"
+
+namespace device
+{
+/*
+ *  N.B. By defaulting the value of _bus_clocks to non Zero
+ *  All calls to init() will NOT set the buss frequency
+ */
+
+unsigned int I2C::_bus_clocks[BOARD_NUMBER_I2C_BUSES] = BOARD_I2C_BUS_CLOCK_INIT;
+
+I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) :
+	CDev(name, devname),
+	_frequency(frequency)
+{
+	DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname);
+	// fill in _device_id fields for a I2C device
+	_device_id.devid_s.bus_type = DeviceBusType_I2C;
+	_device_id.devid_s.bus = bus;
+	_device_id.devid_s.address = address;
+	// devtype needs to be filled in by the driver
+	_device_id.devid_s.devtype = 0;
+}
+
+I2C::~I2C()
+{
+	if (_dev) {
+		px4_i2cbus_uninitialize(_dev);
+		_dev = nullptr;
+	}
+}
+
+int
+I2C::set_bus_clock(unsigned bus, unsigned clock_hz)
+{
+	int index = bus - 1;
+
+	if (index < 0 || index >= static_cast<int>(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) {
+		return -EINVAL;
+	}
+
+	if (_bus_clocks[index] > 0) {
+		// DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz);
+	}
+
+	_bus_clocks[index] = clock_hz;
+
+	return OK;
+}
+
+int
+I2C::init()
+{
+	int ret = PX4_ERROR;
+	unsigned bus_index;
+
+	// attach to the i2c bus
+	_dev = px4_i2cbus_initialize(get_device_bus());
+
+	if (_dev == nullptr) {
+		DEVICE_DEBUG("failed to init I2C");
+		ret = -ENOENT;
+		goto out;
+	}
+
+	// the above call fails for a non-existing bus index,
+	// so the index math here is safe.
+	bus_index = get_device_bus() - 1;
+
+	// abort if the max frequency we allow (the frequency we ask)
+	// is smaller than the bus frequency
+	if (_bus_clocks[bus_index] > _frequency) {
+		(void)px4_i2cbus_uninitialize(_dev);
+		_dev = nullptr;
+		DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)",
+			   get_device_bus(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	// set frequency for this instance once to the bus speed
+	// the bus speed is the maximum supported by all devices on the bus,
+	// as we have to prioritize performance over compatibility.
+	// If a new device requires a lower clock speed, this has to be
+	// manually set via "fmu i2c <bus> <clock>" before starting any
+	// drivers.
+	// This is necessary as automatically lowering the bus speed
+	// for maximum compatibility could induce timing issues on
+	// critical sensors the adopter might be unaware of.
+
+	// set the bus frequency on the first access if it has
+	// not been set yet
+	if (_bus_clocks[bus_index] == 0) {
+		_bus_clocks[bus_index] = _frequency;
+	}
+
+	// call the probe function to check whether the device is present
+	ret = probe();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("probe failed");
+		goto out;
+	}
+
+	// do base class init, which will create device node, etc
+	ret = CDev::init();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("cdev init failed");
+		goto out;
+	}
+
+	// tell the world where we are
+	DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)",
+		   get_device_bus(), get_device_address(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
+
+out:
+
+	if ((ret != OK) && (_dev != nullptr)) {
+		px4_i2cbus_uninitialize(_dev);
+		_dev = nullptr;
+	}
+
+	return ret;
+}
+
+int
+I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	px4_i2c_msg_t msgv[2];
+	unsigned msgs;
+	int ret = PX4_ERROR;
+	unsigned retry_count = 0;
+
+	if (_dev == nullptr) {
+		PX4_ERR("I2C device not opened");
+		return 1;
+	}
+
+	do {
+		DEVICE_DEBUG("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+		msgs = 0;
+
+		if (send_len > 0) {
+			msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
+			msgv[msgs].addr = get_device_address();
+			msgv[msgs].flags = 0;
+			msgv[msgs].buffer = const_cast<uint8_t *>(send);
+			msgv[msgs].length = send_len;
+			msgs++;
+		}
+
+		if (recv_len > 0) {
+			msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
+			msgv[msgs].addr = get_device_address();
+			msgv[msgs].flags = I2C_M_READ;
+			msgv[msgs].buffer = recv;
+			msgv[msgs].length = recv_len;
+			msgs++;
+		}
+
+		if (msgs == 0) {
+			return -EINVAL;
+		}
+
+		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
+
+		/* success */
+		if (ret == PX4_OK) {
+			break;
+		}
+
+		/* if we have already retried once, or we are going to give up, then reset the bus */
+		if ((retry_count >= 1) || (retry_count >= _retries)) {
+			I2C_RESET(_dev);
+		}
+
+	} while (retry_count++ < _retries);
+
+	return ret;
+}
+
+} // namespace device

src/lib/drivers/device/nuttx/I2C.hpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 i2c.h
+ *
+ * Base class for devices connected via I2C.
+ */
+
+#ifndef _DEVICE_I2C_H
+#define _DEVICE_I2C_H
+
+#include "../CDev.hpp"
+
+#include <px4_i2c.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on I2C
+ */
+class __EXPORT I2C : public CDev
+{
+
+public:
+
+	static int	set_bus_clock(unsigned bus, unsigned clock_hz);
+
+	static unsigned	int	_bus_clocks[BOARD_NUMBER_I2C_BUSES];
+
+protected:
+	/**
+	 * The number of times a read or write operation will be retried on
+	 * error.
+	 */
+	uint8_t		_retries{0};
+
+	/**
+	 * @ Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param bus		I2C bus on which the device lives
+	 * @param address	I2C bus address, or zero if set_address will be used
+	 * @param frequency	I2C bus frequency for the device (currently not used)
+	 */
+	I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency);
+	virtual ~I2C();
+
+	virtual int	init();
+
+	/**
+	 * Check for the presence of the device on the bus.
+	 */
+	virtual int	probe() { return PX4_OK; }
+
+	/**
+	 * Perform an I2C transaction to the device.
+	 *
+	 * At least one of send_len and recv_len must be non-zero.
+	 *
+	 * @param send		Pointer to bytes to send.
+	 * @param send_len	Number of bytes to send.
+	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv_len	Number of bytes to receive.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
+	bool		external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }
+
+private:
+	uint32_t		_frequency{0};
+	px4_i2c_dev_t		*_dev{nullptr};
+
+	I2C(const device::I2C &);
+	I2C operator=(const device::I2C &);
+};
+
+} // namespace device
+
+#endif /* _DEVICE_I2C_H */

src/lib/drivers/device/nuttx/SPI.cpp

@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 spi.cpp
+ *
+ * Base class for devices connected via SPI.
+ *
+ * @todo Work out if caching the mode/frequency would save any time.
+ *
+ * @todo A separate bus/device abstraction would allow for mixed interrupt-mode
+ * and non-interrupt-mode clients to arbitrate for the bus.  As things stand,
+ * a bus shared between clients of both kinds is vulnerable to races between
+ * the two, where an interrupt-mode client will ignore the lock held by the
+ * non-interrupt-mode client.
+ */
+
+#include "SPI.hpp"
+
+#include <px4_config.h>
+#include <nuttx/arch.h>
+
+#ifndef CONFIG_SPI_EXCHANGE
+# error This driver requires CONFIG_SPI_EXCHANGE
+#endif
+
+namespace device
+{
+
+SPI::SPI(const char *name,
+	 const char *devname,
+	 int bus,
+	 uint32_t device,
+	 enum spi_mode_e mode,
+	 uint32_t frequency) :
+	// base class
+	CDev(name, devname),
+	// public
+	// protected
+	locking_mode(LOCK_PREEMPTION),
+	// private
+	_device(device),
+	_mode(mode),
+	_frequency(frequency),
+	_dev(nullptr)
+{
+	// fill in _device_id fields for a SPI device
+	_device_id.devid_s.bus_type = DeviceBusType_SPI;
+	_device_id.devid_s.bus = bus;
+	_device_id.devid_s.address = (uint8_t)device;
+	// devtype needs to be filled in by the driver
+	_device_id.devid_s.devtype = 0;
+}
+
+SPI::~SPI()
+{
+	// XXX no way to let go of the bus...
+}
+
+int
+SPI::init()
+{
+	int ret = OK;
+
+	/* attach to the spi bus */
+	if (_dev == nullptr) {
+		_dev = px4_spibus_initialize(get_device_bus());
+	}
+
+	if (_dev == nullptr) {
+		DEVICE_DEBUG("failed to init SPI");
+		ret = -ENOENT;
+		goto out;
+	}
+
+	/* deselect device to ensure high to low transition of pin select */
+	SPI_SELECT(_dev, _device, false);
+
+	/* call the probe function to check whether the device is present */
+	ret = probe();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("probe failed");
+		goto out;
+	}
+
+	/* do base class init, which will create the device node, etc. */
+	ret = CDev::init();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("cdev init failed");
+		goto out;
+	}
+
+	/* tell the workd where we are */
+	DEVICE_LOG("on SPI bus %d at %d (%u KHz)", get_device_bus(), PX4_SPI_DEV_ID(_device), _frequency / 1000);
+
+out:
+	return ret;
+}
+
+int
+SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
+	int result;
+
+	if ((send == nullptr) && (recv == nullptr)) {
+		return -EINVAL;
+	}
+
+	LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode;
+
+	/* lock the bus as required */
+	switch (mode) {
+	default:
+	case LOCK_PREEMPTION: {
+			irqstate_t state = px4_enter_critical_section();
+			result = _transfer(send, recv, len);
+			px4_leave_critical_section(state);
+		}
+		break;
+
+	case LOCK_THREADS:
+		SPI_LOCK(_dev, true);
+		result = _transfer(send, recv, len);
+		SPI_LOCK(_dev, false);
+		break;
+
+	case LOCK_NONE:
+		result = _transfer(send, recv, len);
+		break;
+	}
+
+	return result;
+}
+
+int
+SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
+	SPI_SETFREQUENCY(_dev, _frequency);
+	SPI_SETMODE(_dev, _mode);
+	SPI_SETBITS(_dev, 8);
+	SPI_SELECT(_dev, _device, true);
+
+	/* do the transfer */
+	SPI_EXCHANGE(_dev, send, recv, len);
+
+	/* and clean up */
+	SPI_SELECT(_dev, _device, false);
+
+	return OK;
+}
+
+int
+SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len)
+{
+	int result;
+
+	if ((send == nullptr) && (recv == nullptr)) {
+		return -EINVAL;
+	}
+
+	LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode;
+
+	/* lock the bus as required */
+	switch (mode) {
+	default:
+	case LOCK_PREEMPTION: {
+			irqstate_t state = px4_enter_critical_section();
+			result = _transferhword(send, recv, len);
+			px4_leave_critical_section(state);
+		}
+		break;
+
+	case LOCK_THREADS:
+		SPI_LOCK(_dev, true);
+		result = _transferhword(send, recv, len);
+		SPI_LOCK(_dev, false);
+		break;
+
+	case LOCK_NONE:
+		result = _transferhword(send, recv, len);
+		break;
+	}
+
+	return result;
+}
+
+int
+SPI::_transferhword(uint16_t *send, uint16_t *recv, unsigned len)
+{
+	SPI_SETFREQUENCY(_dev, _frequency);
+	SPI_SETMODE(_dev, _mode);
+	SPI_SETBITS(_dev, 16);							/* 16 bit transfer */
+	SPI_SELECT(_dev, _device, true);
+
+	/* do the transfer */
+	SPI_EXCHANGE(_dev, send, recv, len);
+
+	/* and clean up */
+	SPI_SELECT(_dev, _device, false);
+
+	return OK;
+}
+
+} // namespace device

src/lib/drivers/device/nuttx/SPI.hpp

@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 spi.h
+ *
+ * Base class for devices connected via SPI.
+ */
+
+#ifndef _DEVICE_SPI_H
+#define _DEVICE_SPI_H
+
+#include "../CDev.hpp"
+
+#include <px4_spi.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on SPI
+ */
+class __EXPORT SPI : public CDev
+{
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param bus		SPI bus on which the device lives
+	 * @param device	Device handle (used by SPI_SELECT)
+	 * @param mode		SPI clock/data mode
+	 * @param frequency	SPI clock frequency
+	 */
+	SPI(const char *name,
+	    const char *devname,
+	    int bus,
+	    uint32_t device,
+	    enum spi_mode_e mode,
+	    uint32_t frequency);
+	virtual ~SPI();
+
+	/**
+	 * Locking modes supported by the driver.
+	 */
+	enum LockMode {
+		LOCK_PREEMPTION,	/**< the default; lock against all forms of preemption. */
+		LOCK_THREADS,		/**< lock only against other threads, using SPI_LOCK */
+		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
+	};
+
+	virtual int	init();
+
+	/**
+	 * Check for the presence of the device on the bus.
+	 */
+	virtual int	probe() { return PX4_OK; }
+
+	/**
+	 * Perform a SPI transfer.
+	 *
+	 * If called from interrupt context, this interface does not lock
+	 * the bus and may interfere with non-interrupt-context callers.
+	 *
+	 * Clients in a mixed interrupt/non-interrupt configuration must
+	 * ensure appropriate interlocking.
+	 *
+	 * At least one of send or recv must be non-null.
+	 *
+	 * @param send		Bytes to send to the device, or nullptr if
+	 *			no data is to be sent.
+	 * @param recv		Buffer for receiving bytes from the device,
+	 *			or nullptr if no bytes are to be received.
+	 * @param len		Number of bytes to transfer.
+	 * @return		OK if the exchange was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
+
+	/**
+	 * Perform a SPI 16 bit transfer.
+	 *
+	 * If called from interrupt context, this interface does not lock
+	 * the bus and may interfere with non-interrupt-context callers.
+	 *
+	 * Clients in a mixed interrupt/non-interrupt configuration must
+	 * ensure appropriate interlocking.
+	 *
+	 * At least one of send or recv must be non-null.
+	 *
+	 * @param send		Words to send to the device, or nullptr if
+	 *			no data is to be sent.
+	 * @param recv		Words for receiving bytes from the device,
+	 *			or nullptr if no bytes are to be received.
+	 * @param len		Number of words to transfer.
+	 * @return		OK if the exchange was successful, -errno
+	 *			otherwise.
+	 */
+	int		transferhword(uint16_t *send, uint16_t *recv, unsigned len);
+
+	/**
+	 * Set the SPI bus frequency
+	 * This is used to change frequency on the fly. Some sensors
+	 * (such as the MPU6000) need a lower frequency for setup
+	 * registers and can handle higher frequency for sensor
+	 * value registers
+	 *
+	 * @param frequency	Frequency to set (Hz)
+	 */
+	void		set_frequency(uint32_t frequency) { _frequency = frequency; }
+
+	/**
+	 * Set the SPI bus locking mode
+	 *
+	 * This set the SPI locking mode. For devices competing with NuttX SPI
+	 * drivers on a bus the right lock mode is LOCK_THREADS.
+	 *
+	 * @param mode	Locking mode
+	 */
+	void		set_lockmode(enum LockMode mode) { locking_mode = mode; }
+
+	LockMode	locking_mode;	/**< selected locking mode */
+
+private:
+	uint32_t			_device;
+	enum spi_mode_e		_mode;
+	uint32_t		_frequency;
+	struct spi_dev_s	*_dev;
+
+	/* this class does not allow copying */
+	SPI(const SPI &);
+	SPI operator=(const SPI &);
+
+protected:
+	int	_transfer(uint8_t *send, uint8_t *recv, unsigned len);
+
+	int	_transferhword(uint16_t *send, uint16_t *recv, unsigned len);
+
+	bool	external() { return px4_spi_bus_external(get_device_bus()); }
+
+};
+
+} // namespace device
+
+#endif /* _DEVICE_SPI_H */

src/lib/drivers/device/nuttx/cdev_platform.cpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 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 cdev_platform.cpp
+ *
+ * NuttX Character device functions
+ */
+#include "cdev_platform.hpp"
+#include "../CDev.hpp"
+
+#include "px4_posix.h"
+#include "drivers/drv_device.h"
+#include <sys/ioctl.h>
+
+#ifdef CONFIG_DISABLE_POLL
+# error This driver is not compatible with CONFIG_DISABLE_POLL
+#endif
+
+namespace device
+{
+
+/*
+ * The standard NuttX operation dispatch table can't call C++ member functions
+ * directly, so we have to bounce them through this dispatch table.
+ */
+static int	cdev_open(file_t *filp);
+static int	cdev_close(file_t *filp);
+static ssize_t	cdev_read(file_t *filp, char *buffer, size_t buflen);
+static ssize_t	cdev_write(file_t *filp, const char *buffer, size_t buflen);
+static off_t	cdev_seek(file_t *filp, off_t offset, int whence);
+static int	cdev_ioctl(file_t *filp, int cmd, unsigned long arg);
+static int	cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup);
+
+/**
+ * Character device indirection table.
+ *
+ * Every cdev we register gets the same function table; we use the private data
+ * field in the inode to store the instance pointer.
+ *
+ * Note that we use the GNU extension syntax here because we don't get designated
+ * initialisers in gcc 4.6.
+ */
+const struct file_operations device::CDev::fops = {
+open	: cdev_open,
+close	: cdev_close,
+read	: cdev_read,
+write	: cdev_write,
+seek	: cdev_seek,
+ioctl	: cdev_ioctl,
+poll	: cdev_poll
+};
+
+static int
+cdev_open(file_t *filp)
+{
+	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
+
+	return cdev->open(filp);
+}
+
+static int
+cdev_close(file_t *filp)
+{
+	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
+
+	return cdev->close(filp);
+}
+
+static ssize_t
+cdev_read(file_t *filp, char *buffer, size_t buflen)
+{
+	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
+
+	return cdev->read(filp, buffer, buflen);
+}
+
+static ssize_t
+cdev_write(file_t *filp, const char *buffer, size_t buflen)
+{
+	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
+
+	return cdev->write(filp, buffer, buflen);
+}
+
+static off_t
+cdev_seek(file_t *filp, off_t offset, int whence)
+{
+	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
+
+	return cdev->seek(filp, offset, whence);
+}
+
+static int
+cdev_ioctl(file_t *filp, int cmd, unsigned long arg)
+{
+	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
+
+	return cdev->ioctl(filp, cmd, arg);
+}
+
+static int
+cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup)
+{
+	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
+
+	return cdev->poll(filp, fds, setup);
+}
+
+} // namespace device

src/lib/drivers/device/nuttx/cdev_platform.hpp

@@ -0,0 +1,19 @@
+
+#pragma once
+
+#include <cinttypes>
+
+#include <nuttx/arch.h>
+
+#include <px4_micro_hal.h>
+
+#define ATOMIC_ENTER irqstate_t flags = px4_enter_critical_section()
+#define ATOMIC_LEAVE px4_leave_critical_section(flags)
+
+namespace device
+{
+
+using px4_file_operations_t = struct file_operations;
+using file_t = struct file;
+
+} // namespace device

src/lib/drivers/device/posix/I2C.cpp

@@ -0,0 +1,194 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c.cpp
+ *
+ * Base class for devices attached via the I2C bus.
+ *
+ * @todo Bus frequency changes; currently we do nothing with the value
+ *       that is supplied.  Should we just depend on the bus knowing?
+ */
+
+#include "I2C.hpp"
+
+#ifdef __PX4_LINUX
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#endif
+
+#ifdef __PX4_QURT
+#define PX4_SIMULATE_I2C 1
+#else
+#define PX4_SIMULATE_I2C 0
+#endif
+
+static constexpr const int simulate = PX4_SIMULATE_I2C;
+
+
+namespace device
+{
+
+I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) :
+	CDev(name, devname)
+{
+	DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname);
+	// fill in _device_id fields for a I2C device
+	_device_id.devid_s.bus_type = DeviceBusType_I2C;
+	_device_id.devid_s.bus = bus;
+	_device_id.devid_s.address = address;
+	// devtype needs to be filled in by the driver
+	_device_id.devid_s.devtype = 0;
+}
+
+I2C::~I2C()
+{
+	if (_fd >= 0) {
+#ifndef __PX4_QURT
+		::close(_fd);
+#endif /* !__PX4_QURT */
+		_fd = -1;
+	}
+}
+
+int
+I2C::init()
+{
+	// Assume the driver set the desired bus frequency. There is no standard
+	// way to set it from user space.
+
+	// do base class init, which will create device node, etc
+	int ret = CDev::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("CDev::init failed");
+		return ret;
+	}
+
+	if (simulate) {
+		_fd = 10000;
+
+	} else {
+#ifndef __PX4_QURT
+
+		// Open the actual I2C device
+		char dev_path[16];
+		snprintf(dev_path, sizeof(dev_path), "/dev/i2c-%i", get_device_bus());
+		_fd = ::open(dev_path, O_RDWR);
+
+		if (_fd < 0) {
+			PX4_ERR("could not open %s", dev_path);
+			px4_errno = errno;
+			return PX4_ERROR;
+		}
+
+#endif /* !__PX4_QURT */
+	}
+
+	return ret;
+}
+
+int
+I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+#ifndef __PX4_LINUX
+	return PX4_ERROR;
+#else
+	struct i2c_msg msgv[2];
+	unsigned msgs;
+	int ret = PX4_ERROR;
+	unsigned retry_count = 0;
+
+	if (_fd < 0) {
+		PX4_ERR("I2C device not opened");
+		return 1;
+	}
+
+	do {
+		DEVICE_DEBUG("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+		msgs = 0;
+
+		if (send_len > 0) {
+			msgv[msgs].addr = get_device_address();
+			msgv[msgs].flags = 0;
+			msgv[msgs].buf = const_cast<uint8_t *>(send);
+			msgv[msgs].len = send_len;
+			msgs++;
+		}
+
+		if (recv_len > 0) {
+			msgv[msgs].addr = get_device_address();
+			msgv[msgs].flags = I2C_M_READ;
+			msgv[msgs].buf = recv;
+			msgv[msgs].len = recv_len;
+			msgs++;
+		}
+
+		if (msgs == 0) {
+			return -EINVAL;
+		}
+
+		struct i2c_rdwr_ioctl_data packets;
+
+		packets.msgs  = msgv;
+
+		packets.nmsgs = msgs;
+
+		if (simulate) {
+			DEVICE_DEBUG("I2C SIM: transfer_4 on %s", get_devname());
+			ret = PX4_OK;
+
+		} else {
+			ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets);
+
+			if (ret == -1) {
+				DEVICE_DEBUG("I2C transfer failed");
+				ret = PX4_ERROR;
+
+			} else {
+				ret = PX4_OK;
+			}
+		}
+
+		/* success */
+		if (ret == PX4_OK) {
+			break;
+		}
+
+	} while (retry_count++ < _retries);
+
+	return ret;
+#endif
+}
+
+} // namespace device

src/lib/drivers/device/posix/I2C.hpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 i2c.h
+ *
+ * Base class for devices connected via I2C.
+ */
+
+#ifndef _DEVICE_I2C_H
+#define _DEVICE_I2C_H
+
+#include "../CDev.hpp"
+
+#include <px4_i2c.h>
+
+#ifdef __PX4_LINUX
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#endif
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on I2C
+ */
+class __EXPORT I2C : public CDev
+{
+
+public:
+
+protected:
+	/**
+	 * The number of times a read or write operation will be retried on
+	 * error.
+	 */
+	uint8_t		_retries{0};
+
+	/**
+	 * @ Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param bus		I2C bus on which the device lives
+	 * @param address	I2C bus address, or zero if set_address will be used
+	 * @param frequency	I2C bus frequency for the device (currently not used)
+	 */
+	I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency = 0);
+	virtual ~I2C();
+
+	virtual int	init();
+
+	/**
+	 * Check for the presence of the device on the bus.
+	 */
+	virtual int	probe() { return PX4_OK; }
+
+	/**
+	 * Perform an I2C transaction to the device.
+	 *
+	 * At least one of send_len and recv_len must be non-zero.
+	 *
+	 * @param send		Pointer to bytes to send.
+	 * @param send_len	Number of bytes to send.
+	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv_len	Number of bytes to receive.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
+	bool		external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }
+
+private:
+	int 			_fd{-1};
+
+	I2C(const device::I2C &);
+	I2C operator=(const device::I2C &);
+};
+
+} // namespace device
+
+#endif /* _DEVICE_I2C_H */

src/lib/drivers/device/posix/SPI.hpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2017 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 spi.h
+ *
+ * Base class for devices connected via SPI.
+ */
+
+#ifndef _DEVICE_SPI_H
+#define _DEVICE_SPI_H
+
+#include "CDev.hpp"
+
+#include <px4_spi.h>
+
+namespace device __EXPORT
+{
+
+// TODO: implement posix spi
+
+} // namespace device
+
+#endif /* _DEVICE_SPI_H */

src/lib/drivers/device/posix/cdev_platform.cpp

@@ -0,0 +1,551 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vdev_posix.cpp
+ *
+ * POSIX-like API for virtual character device
+ */
+
+#include "cdev_platform.hpp"
+
+#include <string>
+#include <map>
+
+#include "vfile.h"
+#include "../CDev.hpp"
+
+#include <px4_log.h>
+#include <px4_posix.h>
+#include <px4_time.h>
+
+#include "DevMgr.hpp"
+
+using namespace std;
+using namespace DriverFramework;
+
+const device::px4_file_operations_t device::CDev::fops = {};
+
+pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER;
+
+px4_sem_t lockstep_sem;
+bool sim_lockstep = false;
+volatile bool sim_delay = false;
+
+#define PX4_MAX_FD 350
+static map<string, void *> devmap;
+static device::file_t filemap[PX4_MAX_FD] = {};
+
+extern "C" {
+
+	int px4_errno;
+
+	static device::CDev *getDev(const char *path)
+	{
+		PX4_DEBUG("CDev::getDev");
+
+		pthread_mutex_lock(&devmutex);
+
+		auto item = devmap.find(path);
+
+		if (item != devmap.end()) {
+			pthread_mutex_unlock(&devmutex);
+			return (device::CDev *)item->second;
+		}
+
+		pthread_mutex_unlock(&devmutex);
+
+		return nullptr;
+	}
+
+	static device::CDev *get_vdev(int fd)
+	{
+		pthread_mutex_lock(&filemutex);
+		bool valid = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd].vdev);
+		device::CDev *dev;
+
+		if (valid) {
+			dev = (device::CDev *)(filemap[fd].vdev);
+
+		} else {
+			dev = nullptr;
+		}
+
+		pthread_mutex_unlock(&filemutex);
+		return dev;
+	}
+
+	int register_driver(const char *name, const device::px4_file_operations_t *fops, device::mode_t mode, void *data)
+	{
+		PX4_DEBUG("CDev::register_driver %s", name);
+		int ret = 0;
+
+		if (name == nullptr || data == nullptr) {
+			return -EINVAL;
+		}
+
+		pthread_mutex_lock(&devmutex);
+
+		// Make sure the device does not already exist
+		auto item = devmap.find(name);
+
+		if (item != devmap.end()) {
+			pthread_mutex_unlock(&devmutex);
+			return -EEXIST;
+		}
+
+		devmap[name] = (void *)data;
+		PX4_DEBUG("Registered DEV %s", name);
+
+		pthread_mutex_unlock(&devmutex);
+
+		return ret;
+	}
+
+	int unregister_driver(const char *name)
+	{
+		PX4_DEBUG("CDev::unregister_driver %s", name);
+		int ret = -EINVAL;
+
+		if (name == nullptr) {
+			return -EINVAL;
+		}
+
+		pthread_mutex_lock(&devmutex);
+
+		if (devmap.erase(name) > 0) {
+			PX4_DEBUG("Unregistered DEV %s", name);
+			ret = 0;
+		}
+
+		pthread_mutex_unlock(&devmutex);
+
+		return ret;
+	}
+
+	int px4_open(const char *path, int flags, ...)
+	{
+		PX4_DEBUG("px4_open");
+		device::CDev *dev = getDev(path);
+		int ret = 0;
+		int i;
+		mode_t mode;
+
+		if (!dev && (flags & (PX4_F_WRONLY | PX4_F_CREAT)) != 0 &&
+		    strncmp(path, "/obj/", 5) != 0 &&
+		    strncmp(path, "/dev/", 5) != 0) {
+			va_list p;
+			va_start(p, flags);
+			mode = va_arg(p, int);
+			va_end(p);
+
+			// Create the file
+			PX4_DEBUG("Creating virtual file %s", path);
+			dev = device::VFile::createFile(path, mode);
+		}
+
+		if (dev) {
+
+			pthread_mutex_lock(&filemutex);
+
+			for (i = 0; i < PX4_MAX_FD; ++i) {
+				if (filemap[i].vdev == nullptr) {
+					filemap[i] = device::file_t(flags, dev);
+					break;
+				}
+			}
+
+			pthread_mutex_unlock(&filemutex);
+
+			if (i < PX4_MAX_FD) {
+				ret = dev->open(&filemap[i]);
+
+			} else {
+
+				const unsigned NAMELEN = 32;
+				char thread_name[NAMELEN] = {};
+
+#ifndef __PX4_QURT
+				int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN);
+
+				if (nret || thread_name[0] == 0) {
+					PX4_WARN("failed getting thread name");
+				}
+
+				PX4_BACKTRACE();
+#endif
+
+				PX4_WARN("%s: exceeded maximum number of file descriptors, accessing %s",
+					 thread_name, path);
+				ret = -ENOENT;
+			}
+
+		} else {
+			ret = -EINVAL;
+		}
+
+		if (ret < 0) {
+			return -1;
+		}
+
+		PX4_DEBUG("px4_open fd = %d", i);
+		return i;
+	}
+
+	int px4_close(int fd)
+	{
+		int ret;
+
+		device::CDev *dev = get_vdev(fd);
+
+		if (dev) {
+			pthread_mutex_lock(&filemutex);
+			ret = dev->close(&filemap[fd]);
+
+			filemap[fd].vdev = nullptr;
+
+			pthread_mutex_unlock(&filemutex);
+			PX4_DEBUG("px4_close fd = %d", fd);
+
+		} else {
+			ret = -EINVAL;
+		}
+
+		if (ret < 0) {
+			px4_errno = -ret;
+			ret = PX4_ERROR;
+		}
+
+		return ret;
+	}
+
+	ssize_t px4_read(int fd, void *buffer, size_t buflen)
+	{
+		int ret;
+
+		device::CDev *dev = get_vdev(fd);
+
+		if (dev) {
+			PX4_DEBUG("px4_read fd = %d", fd);
+			ret = dev->read(&filemap[fd], (char *)buffer, buflen);
+
+		} else {
+			ret = -EINVAL;
+		}
+
+		if (ret < 0) {
+			px4_errno = -ret;
+			ret = PX4_ERROR;
+		}
+
+		return ret;
+	}
+
+	ssize_t px4_write(int fd, const void *buffer, size_t buflen)
+	{
+		int ret;
+
+		device::CDev *dev = get_vdev(fd);
+
+		if (dev) {
+			PX4_DEBUG("px4_write fd = %d", fd);
+			ret = dev->write(&filemap[fd], (const char *)buffer, buflen);
+
+		} else {
+			ret = -EINVAL;
+		}
+
+		if (ret < 0) {
+			px4_errno = -ret;
+			ret = PX4_ERROR;
+		}
+
+		return ret;
+	}
+
+	int px4_ioctl(int fd, int cmd, unsigned long arg)
+	{
+		PX4_DEBUG("px4_ioctl fd = %d", fd);
+		int ret = 0;
+
+		device::CDev *dev = get_vdev(fd);
+
+		if (dev) {
+			ret = dev->ioctl(&filemap[fd], cmd, arg);
+
+		} else {
+			ret = -EINVAL;
+		}
+
+		if (ret < 0) {
+			px4_errno = -ret;
+		}
+
+		return ret;
+	}
+
+	int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
+	{
+		if (nfds == 0) {
+			PX4_WARN("px4_poll with no fds");
+			return -1;
+		}
+
+		px4_sem_t sem;
+		int count = 0;
+		int ret = -1;
+		unsigned int i;
+
+		const unsigned NAMELEN = 32;
+		char thread_name[NAMELEN] = {};
+
+#ifndef __PX4_QURT
+		int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN);
+
+		if (nret || thread_name[0] == 0) {
+			PX4_WARN("failed getting thread name");
+		}
+
+#endif
+
+		while (sim_delay) {
+			usleep(100);
+		}
+
+		PX4_DEBUG("Called px4_poll timeout = %d", timeout);
+
+		px4_sem_init(&sem, 0, 0);
+
+		// sem use case is a signal
+		px4_sem_setprotocol(&sem, SEM_PRIO_NONE);
+
+		// Go through all fds and check them for a pollable state
+		bool fd_pollable = false;
+
+		for (i = 0; i < nfds; ++i) {
+			fds[i].sem     = &sem;
+			fds[i].revents = 0;
+			fds[i].priv    = nullptr;
+
+			device::CDev *dev = get_vdev(fds[i].fd);
+
+			// If fd is valid
+			if (dev) {
+				PX4_DEBUG("%s: px4_poll: CDev->poll(setup) %d", thread_name, fds[i].fd);
+				ret = dev->poll(&filemap[fds[i].fd], &fds[i], true);
+
+				if (ret < 0) {
+					PX4_WARN("%s: px4_poll() error: %s",
+						 thread_name, strerror(errno));
+					break;
+				}
+
+				if (ret >= 0) {
+					fd_pollable = true;
+				}
+			}
+		}
+
+		// If any FD can be polled, lock the semaphore and
+		// check for new data
+		if (fd_pollable) {
+			if (timeout > 0) {
+
+				// Get the current time
+				struct timespec ts;
+				// FIXME: check if QURT should probably be using CLOCK_MONOTONIC
+				px4_clock_gettime(CLOCK_REALTIME, &ts);
+
+				// Calculate an absolute time in the future
+				const unsigned billion = (1000 * 1000 * 1000);
+				unsigned tdiff = timeout;
+				uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000);
+				ts.tv_sec += nsecs / billion;
+				nsecs -= (nsecs / billion) * billion;
+				ts.tv_nsec = nsecs;
+
+				// Execute a blocking wait for that time in the future
+				errno = 0;
+				ret = px4_sem_timedwait(&sem, &ts);
+#ifndef __PX4_DARWIN
+				ret = errno;
+#endif
+
+				// Ensure ret is negative on failure
+				if (ret > 0) {
+					ret = -ret;
+				}
+
+				if (ret && ret != -ETIMEDOUT) {
+					PX4_WARN("%s: px4_poll() sem error", thread_name);
+				}
+
+			} else if (timeout < 0) {
+				px4_sem_wait(&sem);
+			}
+
+			// We have waited now (or not, depending on timeout),
+			// go through all fds and count how many have data
+			for (i = 0; i < nfds; ++i) {
+
+				device::CDev *dev = get_vdev(fds[i].fd);
+
+				// If fd is valid
+				if (dev) {
+					PX4_DEBUG("%s: px4_poll: CDev->poll(teardown) %d", thread_name, fds[i].fd);
+					ret = dev->poll(&filemap[fds[i].fd], &fds[i], false);
+
+					if (ret < 0) {
+						PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name);
+						break;
+					}
+
+					if (fds[i].revents) {
+						count += 1;
+					}
+				}
+			}
+		}
+
+		px4_sem_destroy(&sem);
+
+		// Return the positive count if present,
+		// return the negative error number if failed
+		return (count) ? count : ret;
+	}
+
+	int px4_fsync(int fd)
+	{
+		return 0;
+	}
+
+	int px4_access(const char *pathname, int mode)
+	{
+		if (mode != F_OK) {
+			errno = EINVAL;
+			return -1;
+		}
+
+		device::CDev *dev = getDev(pathname);
+		return (dev != nullptr) ? 0 : -1;
+	}
+
+	void px4_show_devices()
+	{
+		int i = 0;
+		PX4_INFO("PX4 Devices:");
+
+		pthread_mutex_lock(&devmutex);
+
+		for (const auto &dev : devmap) {
+			if (strncmp(dev.first.c_str(), "/dev/", 5) == 0) {
+				PX4_INFO("   %s", dev.first.c_str());
+			}
+		}
+
+		pthread_mutex_unlock(&devmutex);
+
+		PX4_INFO("DF Devices:");
+		const char *dev_path;
+		unsigned int index = 0;
+		i = 0;
+
+		do {
+			// Each look increments index and returns -1 if end reached
+			i = DevMgr::getNextDeviceName(index, &dev_path);
+
+			if (i == 0) {
+				PX4_INFO("   %s", dev_path);
+			}
+		} while (i == 0);
+	}
+
+	void px4_show_topics()
+	{
+		PX4_INFO("Devices:");
+
+		pthread_mutex_lock(&devmutex);
+
+		for (const auto &dev : devmap) {
+			if (strncmp(dev.first.c_str(), "/obj/", 5) == 0) {
+				PX4_INFO("   %s", dev.first.c_str());
+			}
+		}
+
+		pthread_mutex_unlock(&devmutex);
+	}
+
+	void px4_show_files()
+	{
+		PX4_INFO("Files:");
+
+		pthread_mutex_lock(&devmutex);
+
+		for (const auto &dev : devmap) {
+			if (strncmp(dev.first.c_str(), "/obj/", 5) != 0 &&
+			    strncmp(dev.first.c_str(), "/dev/", 5) != 0) {
+				PX4_INFO("   %s", dev.first.c_str());
+			}
+		}
+
+		pthread_mutex_unlock(&devmutex);
+	}
+
+	void px4_enable_sim_lockstep()
+	{
+		px4_sem_init(&lockstep_sem, 0, 0);
+
+		// lockstep_sem use case is a signal
+		px4_sem_setprotocol(&lockstep_sem, SEM_PRIO_NONE);
+
+		sim_lockstep = true;
+		sim_delay = false;
+	}
+
+	void px4_sim_start_delay()
+	{
+		sim_delay = true;
+	}
+
+	void px4_sim_stop_delay()
+	{
+		sim_delay = false;
+	}
+
+	bool px4_sim_delay_enabled()
+	{
+		return sim_delay;
+	}
+
+} // extern "C"

src/lib/drivers/device/posix/cdev_platform.hpp

@@ -0,0 +1,32 @@
+
+#pragma once
+
+#include <cinttypes>
+
+#define ATOMIC_ENTER lock()
+#define ATOMIC_LEAVE unlock()
+
+namespace device
+{
+
+struct file_operations {
+	void *op;
+};
+
+using px4_file_operations_t = struct file_operations;
+using mode_t = uint32_t;
+
+struct file_t {
+	int flags;
+	void *priv;
+	void *vdev;
+
+	file_t() : flags(0), priv(nullptr), vdev(nullptr) {}
+	file_t(int f, void *c) : flags(f), priv(nullptr), vdev(c) {}
+};
+
+} // namespace device
+
+extern "C" __EXPORT int register_driver(const char *name, const device::px4_file_operations_t *fops,
+					device::mode_t mode, void *data);
+extern "C" __EXPORT int unregister_driver(const char *path);

src/lib/drivers/device/posix/vfile.cpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vdev_file.cpp
+ * Virtual file
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ */
+
+#include "vfile.h"
+
+namespace device
+{
+
+VFile::VFile(const char *fname, mode_t mode) :
+	CDev("vfile", fname)
+{
+}
+
+VFile *VFile::createFile(const char *fname, mode_t mode)
+{
+	VFile *me = new VFile(fname, mode);
+	px4_file_operations_t *fops = nullptr;
+	register_driver(fname, fops, 0666, (void *)me);
+	return me;
+}
+
+ssize_t VFile::write(file_t *handlep, const char *buffer, size_t buflen)
+{
+	// ignore what was written, but let pollers know something was written
+	poll_notify(POLLIN);
+
+	return buflen;
+}
+
+} // namespace device

src/lib/drivers/device/posix/vfile.h

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vfile.cpp
+ * Virtual file
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ */
+
+#pragma once
+
+#include "../CDev.hpp"
+
+namespace device
+{
+
+class VFile : public CDev
+{
+public:
+
+	static VFile *createFile(const char *fname, mode_t mode);
+	~VFile() {}
+
+	ssize_t write(file_t *handlep, const char *buffer, size_t buflen) override;
+
+private:
+	VFile(const char *fname, mode_t mode);
+	VFile(const VFile &);
+};
+
+} // namespace device

src/lib/drivers/device/ringbuffer.cpp

@@ -0,0 +1,420 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ringbuffer.cpp
+ *
+ * A flexible ringbuffer class.
+ */
+
+#include "ringbuffer.h"
+#include <string.h>
+
+namespace ringbuffer
+{
+
+RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
+	_num_items(num_items),
+	_item_size(item_size),
+	_buf(new char[(_num_items + 1) * item_size]),
+	_head(_num_items),
+	_tail(_num_items)
+{}
+
+RingBuffer::~RingBuffer()
+{
+	if (_buf != nullptr) {
+		delete[] _buf;
+	}
+}
+
+unsigned
+RingBuffer::_next(unsigned index)
+{
+	return (0 == index) ? _num_items : (index - 1);
+}
+
+bool
+RingBuffer::empty()
+{
+	return _tail == _head;
+}
+
+bool
+RingBuffer::full()
+{
+	return _next(_head) == _tail;
+}
+
+unsigned
+RingBuffer::size()
+{
+	return (_buf != nullptr) ? _num_items : 0;
+}
+
+void
+RingBuffer::flush()
+{
+	while (!empty()) {
+		get(nullptr);
+	}
+}
+
+bool
+RingBuffer::put(const void *val, size_t val_size)
+{
+	unsigned next = _next(_head);
+
+	if (next != _tail) {
+		if ((val_size == 0) || (val_size > _item_size)) {
+			val_size = _item_size;
+		}
+
+		memcpy(&_buf[_head * _item_size], val, val_size);
+		_head = next;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+bool
+RingBuffer::put(int8_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint8_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(float val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(double val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(const void *val, size_t val_size)
+{
+	bool overwrote = false;
+
+	for (;;) {
+		if (put(val, val_size)) {
+			break;
+		}
+
+		get(nullptr);
+		overwrote = true;
+	}
+
+	return overwrote;
+}
+
+bool
+RingBuffer::force(int8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(float val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(double val)
+{
+	return force(&val, sizeof(val));
+}
+
+// FIXME - clang crashes on this get() call
+#ifdef __PX4_QURT
+#define __PX4_SBCAP my_sync_bool_compare_and_swap
+static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c)
+{
+	if (*a == b) {
+		*a = c;
+		return true;
+	}
+
+	return false;
+}
+
+#else
+#define __PX4_SBCAP __sync_bool_compare_and_swap
+#endif
+bool
+RingBuffer::get(void *val, size_t val_size)
+{
+	if (_tail != _head) {
+		unsigned candidate;
+		unsigned next;
+
+		if ((val_size == 0) || (val_size > _item_size)) {
+			val_size = _item_size;
+		}
+
+		do {
+			/* decide which element we think we're going to read */
+			candidate = _tail;
+
+			/* and what the corresponding next index will be */
+			next = _next(candidate);
+
+			/* go ahead and read from this index */
+			if (val != nullptr) {
+				memcpy(val, &_buf[candidate * _item_size], val_size);
+			}
+
+			/* if the tail pointer didn't change, we got our item */
+		} while (!__PX4_SBCAP(&_tail, candidate, next));
+
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+bool
+RingBuffer::get(int8_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint8_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int16_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint16_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(float &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(double &val)
+{
+	return get(&val, sizeof(val));
+}
+
+unsigned
+RingBuffer::space()
+{
+	unsigned tail, head;
+
+	/*
+	 * Make a copy of the head/tail pointers in a fashion that
+	 * may err on the side of under-estimating the free space
+	 * in the buffer in the case that the buffer is being updated
+	 * asynchronously with our check.
+	 * If the head pointer changes (reducing space) while copying,
+	 * re-try the copy.
+	 */
+	do {
+		head = _head;
+		tail = _tail;
+	} while (head != _head);
+
+	return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
+}
+
+unsigned
+RingBuffer::count()
+{
+	/*
+	 * Note that due to the conservative nature of space(), this may
+	 * over-estimate the number of items in the buffer.
+	 */
+	return _num_items - space();
+}
+
+bool
+RingBuffer::resize(unsigned new_size)
+{
+	char *old_buffer;
+	char *new_buffer = new char [(new_size + 1) * _item_size];
+
+	if (new_buffer == nullptr) {
+		return false;
+	}
+
+	old_buffer = _buf;
+	_buf = new_buffer;
+	_num_items = new_size;
+	_head = new_size;
+	_tail = new_size;
+	delete[] old_buffer;
+	return true;
+}
+
+void
+RingBuffer::print_info(const char *name)
+{
+	printf("%s	%u/%lu (%u/%u @ %p)\n",
+	       name,
+	       _num_items,
+	       (unsigned long)_num_items * _item_size,
+	       _head,
+	       _tail,
+	       _buf);
+}
+
+} // namespace ringbuffer

src/lib/drivers/device/ringbuffer.h

@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ringbuffer.h
+ *
+ * A flexible ringbuffer class.
+ */
+
+#pragma once
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+
+namespace ringbuffer __EXPORT
+{
+
+class RingBuffer
+{
+public:
+	RingBuffer(unsigned num_items, size_t item_size);
+	virtual ~RingBuffer();
+
+	/**
+	 * Put an item into the buffer.
+	 *
+	 * @param val		Item to put
+	 * @return		true if the item was put, false if the buffer is full
+	 */
+	bool			put(const void *val, size_t val_size = 0);
+
+	bool			put(int8_t val);
+	bool			put(uint8_t val);
+	bool			put(int16_t val);
+	bool			put(uint16_t val);
+	bool			put(int32_t val);
+	bool			put(uint32_t val);
+	bool			put(int64_t val);
+	bool			put(uint64_t val);
+	bool			put(float val);
+	bool			put(double val);
+
+	/**
+	 * Force an item into the buffer, discarding an older item if there is not space.
+	 *
+	 * @param val		Item to put
+	 * @return		true if an item was discarded to make space
+	 */
+	bool			force(const void *val, size_t val_size = 0);
+
+	bool			force(int8_t val);
+	bool			force(uint8_t val);
+	bool			force(int16_t val);
+	bool			force(uint16_t val);
+	bool			force(int32_t val);
+	bool			force(uint32_t val);
+	bool			force(int64_t val);
+	bool			force(uint64_t val);
+	bool			force(float val);
+	bool			force(double val);
+
+	/**
+	 * Get an item from the buffer.
+	 *
+	 * @param val		Item that was gotten
+	 * @return		true if an item was got, false if the buffer was empty.
+	 */
+	bool			get(void *val, size_t val_size = 0);
+
+	bool			get(int8_t &val);
+	bool			get(uint8_t &val);
+	bool			get(int16_t &val);
+	bool			get(uint16_t &val);
+	bool			get(int32_t &val);
+	bool			get(uint32_t &val);
+	bool			get(int64_t &val);
+	bool			get(uint64_t &val);
+	bool			get(float &val);
+	bool			get(double &val);
+
+	/*
+	 * Get the number of slots free in the buffer.
+	 *
+	 * @return		The number of items that can be put into the buffer before
+	 *			it becomes full.
+	 */
+	unsigned		space(void);
+
+	/*
+	 * Get the number of items in the buffer.
+	 *
+	 * @return		The number of items that can be got from the buffer before
+	 *			it becomes empty.
+	 */
+	unsigned		count(void);
+
+	/*
+	 * Returns true if the buffer is empty.
+	 */
+	bool			empty();
+
+	/*
+	 * Returns true if the buffer is full.
+	 */
+	bool			full();
+
+	/*
+	 * Returns the capacity of the buffer, or zero if the buffer could
+	 * not be allocated.
+	 */
+	unsigned		size();
+
+	/*
+	 * Empties the buffer.
+	 */
+	void			flush();
+
+	/*
+	 * resize the buffer. This is unsafe to be called while
+	 * a producer or consuming is running. Caller is responsible
+	 * for any locking needed
+	 *
+	 * @param new_size	new size for buffer
+	 * @return		true if the resize succeeds, false if
+	 * 			not (allocation error)
+	 */
+	bool			resize(unsigned new_size);
+
+	/*
+	 * printf() some info on the buffer
+	 */
+	void			print_info(const char *name);
+
+private:
+	unsigned		_num_items;
+	const size_t		_item_size;
+	char			*_buf;
+	volatile unsigned	_head;	/**< insertion point in _item_size units */
+	volatile unsigned	_tail;	/**< removal point in _item_size units */
+
+	unsigned		_next(unsigned index);
+
+	/* we don't want this class to be copied */
+	RingBuffer(const RingBuffer &);
+	RingBuffer operator=(const RingBuffer &);
+};
+
+} // namespace ringbuffer

src/lib/drivers/device/spi.h

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2017 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.
+ *
+ ****************************************************************************/
+#pragma once
+
+#ifdef __PX4_NUTTX
+#include "nuttx/SPI.hpp"
+#else
+#include "posix/SPI.hpp"
+#endif

src/lib/drivers/led/CMakeLists.txt

@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(drivers__led led.cpp)
+target_link_libraries(drivers__led PRIVATE drivers__device)

src/lib/drivers/led/led.cpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 led.cpp
+ *
+ * LED driver to control the onboard LED(s) via ioctl interface.
+ */
+
+#include <px4_config.h>
+#include <drivers/device/device.h>
+#include <drivers/drv_board_led.h>
+#include <stdio.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+class LED : device::CDev
+{
+public:
+	LED();
+	virtual ~LED();
+
+	virtual int		init();
+	virtual int		ioctl(device::file_t *filp, int cmd, unsigned long arg);
+};
+
+LED::LED() : CDev("led", LED0_DEVICE_PATH)
+{
+	// force immediate init/device registration
+	init();
+}
+
+LED::~LED()
+{
+}
+
+int
+LED::init()
+{
+	DEVICE_DEBUG("LED::init");
+	CDev::init();
+	led_init();
+
+	return 0;
+}
+
+int
+LED::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+{
+	int result = OK;
+
+	switch (cmd) {
+	case LED_ON:
+		led_on(arg);
+		break;
+
+	case LED_OFF:
+		led_off(arg);
+		break;
+
+	case LED_TOGGLE:
+		led_toggle(arg);
+		break;
+
+	default:
+		result = CDev::ioctl(filp, cmd, arg);
+	}
+
+	return result;
+}
+
+namespace
+{
+LED	*gLED;
+}
+
+void
+drv_led_start(void)
+{
+	if (gLED == nullptr) {
+		gLED = new LED;
+	}
+}

src/lib/led/CMakeLists.txt

@@ -30,11 +30,5 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__led
-	SRCS
-		led.cpp
-	DEPENDS
-		platforms__common
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+
+px4_add_library(led led.cpp)

src/lib/mathlib/CMakeLists.txt

@@ -30,11 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__mathlib
-	COMPILE_FLAGS
-	SRCS
-		math/test/test.cpp
-		math/matrix_alg.cpp
-		math/filter/LowPassFilter2p.cpp
-	)
+
+px4_add_library(mathlib
+	math/test/test.cpp
+	math/matrix_alg.cpp
+	math/filter/LowPassFilter2p.cpp
+)

src/lib/mixer/CMakeLists.txt

@@ -83,20 +83,14 @@ add_custom_command(
 	)
 add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h)
 
+add_library(mixer
+	mixer.cpp
+	mixer_group.cpp
+	mixer_helicopter.cpp
+	mixer_load.c
+	mixer_multirotor.cpp
+	mixer_simple.cpp
+)
+target_include_directories(mixer PRIVATE ${PX4_BINARY_DIR}/src/lib/mixer)
 
-px4_add_module(
-	MODULE lib__mixer
-	INCLUDES
-		${CMAKE_CURRENT_BINARY_DIR}
-	SRCS
-		mixer.cpp
-		mixer_group.cpp
-		mixer_helicopter.cpp
-		mixer_load.c
-		mixer_multirotor.cpp
-		mixer_simple.cpp
-	DEPENDS
-		mixer_gen
-		mixer_gen_6dof
-		platforms__common
-	)
+add_dependencies(mixer mixer_gen mixer_gen_6dof prebuild_targets)

src/lib/parameters/CMakeLists.txt

@@ -0,0 +1,139 @@
+############################################################################
+#
+#   Copyright (c) 2017 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.
+#
+############################################################################
+
+add_subdirectory(tinybson)
+
+if (NOT PARAM_DEFAULT_OVERRIDES)
+	set(PARAM_DEFAULT_OVERRIDES "{}")
+endif()
+
+# get full path for each module
+get_property(module_list GLOBAL PROPERTY PX4_MODULE_PATHS)
+if (DISABLE_PARAMS_MODULE_SCOPING)
+	# search all directories with .c files (potentially containing parameters)
+	file(GLOB_RECURSE new_list
+		${PX4_SOURCE_DIR}/src/*.c
+		${external_module_paths}
+		)
+	foreach(file_path ${new_list})
+		get_filename_component(dir_path ${file_path} PATH)
+		list(APPEND module_list "${dir_path}")
+	endforeach()
+	list(REMOVE_DUPLICATES module_list)
+else()
+	foreach(module ${config_module_list})
+		# include all subdirectories as well
+		file(GLOB children RELATIVE ${PX4_SOURCE_DIR}/src/${module} ${PX4_SOURCE_DIR}/src/${module}/*)
+		foreach(child ${children})
+			if(IS_DIRECTORY ${PX4_SOURCE_DIR}/src/${module}/${child})
+				list(APPEND module_list ${PX4_SOURCE_DIR}/src/${module}/${child})
+			endif()
+		endforeach()
+
+		list(APPEND module_list ${PX4_SOURCE_DIR}/src/${module})
+	endforeach()
+
+	list(APPEND module_list ${external_module_paths})
+endif()
+
+list(REMOVE_DUPLICATES module_list)
+
+set(parameters_xml ${PX4_BINARY_DIR}/parameters.xml)
+file(GLOB_RECURSE param_src_files ${PX4_SOURCE_DIR}/src/*params.c)
+add_custom_command(OUTPUT ${parameters_xml}
+	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_process_params.py
+		--src-path ${module_list}
+		--xml ${parameters_xml}
+		--inject-xml ${CMAKE_CURRENT_SOURCE_DIR}/parameters_injected.xml
+		--overrides ${PARAM_DEFAULT_OVERRIDES}
+		--board ${BOARD}
+		#--verbose
+	DEPENDS
+		${param_src_files}
+		parameters_injected.xml
+		px4params/srcparser.py
+		px4params/srcscanner.py
+		px4params/xmlout.py
+		px_process_params.py
+		parameters_injected.xml
+	COMMENT "Generating parameters.xml"
+)
+add_custom_target(parameters_xml DEPENDS ${parameters_xml})
+
+# generate px4_parameters.c and px4_parameters{,_public}.h
+add_custom_command(OUTPUT px4_parameters.c px4_parameters.h px4_parameters_public.h
+	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_generate_params.py
+		--xml ${parameters_xml} --dest ${CMAKE_CURRENT_BINARY_DIR}
+	DEPENDS
+		${PX4_BINARY_DIR}/parameters.xml
+		px_generate_params.py
+		templates/px4_parameters.c.jinja
+		templates/px4_parameters.h.jinja
+		templates/px4_parameters_public.h.jinja
+	)
+
+set(SRCS)
+if ("${CONFIG_SHMEM}" STREQUAL "1")
+	message(STATUS "parameters shared memory enabled")
+	list(APPEND SRCS param_shmem.c)
+else()
+	list(APPEND SRCS param.c)
+endif()
+
+if(${OS} STREQUAL "nuttx")
+	add_subdirectory(flashparams)
+endif()
+
+# TODO: find a better way to do this
+if (NOT "${CONFIG}" MATCHES "px4io")
+	add_library(parameters
+		${SRCS}
+		px4_parameters.c
+		px4_parameters.h
+		px4_parameters_public.h
+		)
+	add_dependencies(parameters prebuild_targets)
+
+	if ("${CONFIG_SHMEM}" STREQUAL "1")
+		target_link_libraries(parameters PRIVATE px4_layer)
+	endif()
+
+	target_link_libraries(parameters PRIVATE perf tinybson)
+	target_compile_options(parameters PRIVATE -Wno-sign-compare) # TODO: fix this
+else()
+	add_library(parameters ${PX4_SOURCE_DIR}/src/platforms/empty.c)
+endif()
+
+if(${OS} STREQUAL "nuttx")
+	target_link_libraries(parameters PRIVATE flashparams)
+endif()

src/lib/parameters/flashparams/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2017 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.
+#
+############################################################################
+
+add_library(flashparams
+	flashfs.c
+	flashparams.c
+)
+add_dependencies(flashparams prebuild_targets)
+target_compile_options(flashparams PRIVATE -Wno-sign-compare) # TODO: fix this
+target_link_libraries(flashparams PRIVATE nuttx_arch)

src/lib/parameters/flashparams/flashfs.h

@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file param.h
+ *
+ * Global flash based parameter store.
+ *
+ * This provides the mechanisms to interface to the PX4
+ * parameter system but replace the IO with non file based flash
+ * i/o routines. So that the code my be implemented on a SMALL memory
+ * foot print device.
+ *
+ */
+
+#ifndef _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H
+#define _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+/*
+ *  PARAMETER_BUFFER_SIZE must be defined larger then the maximum parameter
+ * memory needed to commit the recored + ~20 bytes. For the syslib's parameter
+ * this would be the size of the bson representations of the data
+ */
+#if !defined(PARAMETER_BUFFER_SIZE)
+#define PARAMETER_BUFFER_SIZE 512
+#endif
+
+__BEGIN_DECLS
+
+/*
+ *  Define the interface data a flash_file_token_t
+ *  is like a file name
+ *
+ */
+typedef uint32_t flash_file_tokens_t;
+
+typedef struct flash_file_token_t {
+	union {
+		flash_file_tokens_t t;
+		uint8_t n[sizeof(flash_file_tokens_t)];
+	};
+} flash_file_token_t;
+
+/*
+ * Define the parameter "file name" Currently there is only
+ * and it is hard coded. If more are added the
+ * parameter_flashfs_write would need to support a backing buffer
+ * for when a sector is erased.
+ */
+__EXPORT extern const flash_file_token_t parameters_token;
+
+/* Define the elements of the array passed to the
+ * parameter_flashfs_init function
+ *
+ * For example
+ * static sector_descriptor_t  sector_map[] = {
+ *      {1, 16 * 1024, 0x08004000},
+ *      {2, 16 * 1024, 0x08008000},
+ *       {0, 0, 0},
+ *
+ */
+typedef struct sector_descriptor_t {
+	uint8_t       page;
+	uint32_t      size;
+	uint32_t      address;
+} sector_descriptor_t;
+
+
+/****************************************************************************
+ * Name: parameter_flashfs_init
+ *
+ * Description:
+ *   This helper function advances the flash entry header pointer to the
+ *   locations of the next entry.
+ *
+ * Input Parameters:
+ *   fconfig      - A pointer to an null entry terminated array of
+ *                  flash_file_sector_t
+ *    buffer      - A pointer to a memory to make available to callers
+ *                  for write operations. When allocated to the caller
+ *                  space is reserved in the front for the
+ *                  flash_entry_header_t.
+ *                  If this is passes as NULL. The buffer will be
+ *                  allocated from the heap on calls to
+ *                  parameter_flashfs_alloc and fread on calls
+ *                  to parameter_flashfs_free
+ *
+ *   size         - The size of the buffer in bytes. Should be be 0 if buffer
+ *                  is NULL
+ *
+ * Returned value:
+ *                - A pointer to the next file header location
+ *
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16_t size);
+
+/****************************************************************************
+ * Name: parameter_flashfs_read
+ *
+ * Description:
+ *   This function returns a pointer to the locations of the data associated
+ *   with the file token. On successful return *buffer will be set to Flash
+ *   location and *buf_size the length of the user data.
+ *
+ * Input Parameters:
+ *   token       - File Token File to read
+ *   buffer      - A pointer to a pointer that will receive the address
+ *                 in flash of the data of this "files" data
+ *   buf_size    - A pointer to receive the number of bytes in the "file"
+ *
+ * Returned value:
+ *   On success number of bytes read or a negative errno value,
+ *
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_read(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size);
+
+/****************************************************************************
+ * Name: parameter_flashfs_write
+ *
+ * Description:
+ *   This function writes user data from the buffer allocated with a previous call
+ *   to parameter_flashfs_alloc. flash starting at the given address
+ *
+ * Input Parameters:
+ *   token      - File Token File to read
+ *   buffer      - A pointer to a buffer with buf_size bytes to be written
+ *                 to the flash. This buffer must be allocated
+ *                 with a previous call to parameter_flashfs_alloc
+ *   buf_size    - Number of bytes to write
+ *
+ * Returned value:
+ *   On success the number of bytes written On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_write(flash_file_token_t ft, uint8_t *buffer, size_t buf_size);
+
+/****************************************************************************
+ * Name: parameter_flashfs_erase
+ *
+ * Description:
+ *   This function erases the sectors that were passed to parameter_flashfs_init
+ *
+ * Input Parameters:
+ *
+ * Returned value:
+ *   On success the number of bytes erased
+ *   On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_erase(void);
+
+/****************************************************************************
+ * Name: parameter_flashfs_alloc
+ *
+ * Description:
+ *   This function is called to get a buffer to use in a subsequent call
+ *   to parameter_flashfs_write. The address returned is advanced into the
+ *   buffer to reserve space for the flash entry header.
+ *
+ * Input Parameters:
+ *   token      - File Token File to read (not used)
+ *   buffer     - Memory of buf_size length suitable for calling
+ *                parameter_flashfs_write
+ *   buf_size   - The maximum number of bytes that can be written to
+ *                the buffer
+ *
+ * Returned value:
+ *   On success the number of bytes written On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_alloc(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size);
+
+
+/****************************************************************************
+ * Name: parameter_flashfs_free
+ *
+ * Description:
+ *   Frees  dynamically allocated memory
+ *
+ *
+ ****************************************************************************/
+
+__EXPORT void parameter_flashfs_free(void);
+
+__END_DECLS
+#endif /* _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H */

src/lib/parameters/flashparams/flashparams.c

@@ -0,0 +1,370 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file flashparam.c
+ *
+ * Global flash based parameter store.
+ *
+ * This provides the mechanisms to interface to the PX4
+ * parameter system but replace the IO with non file based flash
+ * i/o routines. So that the code my be implemented on a SMALL memory
+ * foot print device.
+ */
+
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <px4_shutdown.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <parameters/param.h>
+
+#include "systemlib/uthash/utarray.h"
+#include <parameters/tinybson/tinybson.h>
+#include "flashparams.h"
+#include "flashfs.h"
+
+#if 0
+# define debug(fmt, args...)            do { warnx(fmt, ##args); } while(0)
+#else
+# define debug(fmt, args...)            do { } while(0)
+#endif
+
+
+/**
+ * Storage for modified parameters.
+ */
+struct param_wbuf_s {
+	union param_value_u     val;
+	param_t                 param;
+	bool                    unsaved;
+};
+
+static int
+param_export_internal(bool only_unsaved)
+{
+	struct param_wbuf_s *s = NULL;
+	struct bson_encoder_s encoder;
+	int     result = -1;
+
+	/* Use realloc */
+
+	bson_encoder_init_buf(&encoder, NULL, 0);
+
+	/* no modified parameters -> we are done */
+	if (param_values == NULL) {
+		result = 0;
+		goto out;
+	}
+
+	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+
+		int32_t i;
+		float   f;
+
+		/*
+		 * If we are only saving values changed since last save, and this
+		 * one hasn't, then skip it
+		 */
+		if (only_unsaved && !s->unsaved) {
+			continue;
+		}
+
+		s->unsaved = false;
+
+		/* append the appropriate BSON type object */
+
+		switch (param_type(s->param)) {
+
+		case PARAM_TYPE_INT32:
+			i = s->val.i;
+
+			if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			f = s->val.f;
+
+			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			if (bson_encoder_append_binary(&encoder,
+						       param_name(s->param),
+						       BSON_BIN_BINARY,
+						       param_size(s->param),
+						       param_get_value_ptr_external(s->param))) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		default:
+			debug("unrecognized parameter type");
+			goto out;
+		}
+	}
+
+	result = 0;
+
+out:
+
+	if (result == 0) {
+
+		/* Finalize the bison encoding*/
+
+		bson_encoder_fini(&encoder);
+
+		/* Get requiered space */
+
+		size_t buf_size = bson_encoder_buf_size(&encoder);
+
+		int shutdown_lock_ret = px4_shutdown_lock();
+
+		if (shutdown_lock_ret) {
+			PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret);
+		}
+
+		/* Get a buffer from the flash driver with enough space */
+
+		uint8_t *buffer;
+		result = parameter_flashfs_alloc(parameters_token, &buffer, &buf_size);
+
+		if (result == OK) {
+
+			/* Check for a write that has no changes */
+
+			uint8_t *was_buffer;
+			size_t was_buf_size;
+			int was_result = parameter_flashfs_read(parameters_token, &was_buffer, &was_buf_size);
+
+			void *enc_buff = bson_encoder_buf_data(&encoder);
+
+			bool commit = was_result < OK || was_buf_size != buf_size || 0 != memcmp(was_buffer, enc_buff, was_buf_size);
+
+			if (commit) {
+
+				memcpy(buffer, enc_buff, buf_size);
+				result = parameter_flashfs_write(parameters_token, buffer, buf_size);
+				result = result == buf_size ? OK : -EFBIG;
+
+			}
+
+			free(enc_buff);
+			parameter_flashfs_free();
+		}
+
+		if (shutdown_lock_ret == 0) {
+			px4_shutdown_unlock();
+		}
+
+	}
+
+	return result;
+}
+
+struct param_import_state {
+	bool mark_saved;
+};
+
+static int
+param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	float f;
+	int32_t i;
+	void *v, *tmp = NULL;
+	int result = -1;
+	struct param_import_state *state = (struct param_import_state *)private;
+
+	/*
+	 * EOO means the end of the parameter object. (Currently not supporting
+	 * nested BSON objects).
+	 */
+	if (node->type == BSON_EOO) {
+		debug("end of parameters");
+		return 0;
+	}
+
+	/*
+	 * Find the parameter this node represents.  If we don't know it,
+	 * ignore the node.
+	 */
+	param_t param = param_find_no_notification(node->name);
+
+	if (param == PARAM_INVALID) {
+		debug("ignoring unrecognised parameter '%s'", node->name);
+		return 1;
+	}
+
+	/*
+	 * Handle setting the parameter from the node
+	 */
+
+	switch (node->type) {
+	case BSON_INT32:
+		if (param_type(param) != PARAM_TYPE_INT32) {
+			PX4_WARN("unexpected type for %s", node->name);
+			result = 1; // just skip this entry
+			goto out;
+		}
+
+		i = node->i;
+		v = &i;
+		break;
+
+	case BSON_DOUBLE:
+		if (param_type(param) != PARAM_TYPE_FLOAT) {
+			PX4_WARN("unexpected type for %s", node->name);
+			result = 1; // just skip this entry
+			goto out;
+		}
+
+		f = node->d;
+		v = &f;
+		break;
+
+	case BSON_BINDATA:
+		if (node->subtype != BSON_BIN_BINARY) {
+			PX4_WARN("unexpected type for %s", node->name);
+			result = 1; // just skip this entry
+			goto out;
+		}
+
+		if (bson_decoder_data_pending(decoder) != param_size(param)) {
+			PX4_WARN("bad size for '%s'", node->name);
+			result = 1; // just skip this entry
+			goto out;
+		}
+
+		/* XXX check actual file data size? */
+		tmp = malloc(param_size(param));
+
+		if (tmp == NULL) {
+			debug("failed allocating for '%s'", node->name);
+			goto out;
+		}
+
+		if (bson_decoder_copy_data(decoder, tmp)) {
+			debug("failed copying data for '%s'", node->name);
+			goto out;
+		}
+
+		v = tmp;
+		break;
+
+	default:
+		debug("unrecognised node type");
+		goto out;
+	}
+
+	if (param_set_external(param, v, state->mark_saved, true)) {
+
+		debug("error setting value for '%s'", node->name);
+		goto out;
+	}
+
+	if (tmp != NULL) {
+		free(tmp);
+		tmp = NULL;
+	}
+
+	/* don't return zero, that means EOF */
+	result = 1;
+
+out:
+
+	if (tmp != NULL) {
+		free(tmp);
+	}
+
+	return result;
+}
+
+static int
+param_import_internal(bool mark_saved)
+{
+	struct bson_decoder_s decoder;
+	int result = -1;
+	struct param_import_state state;
+
+	uint8_t *buffer = 0;
+	size_t buf_size;
+	parameter_flashfs_read(parameters_token, &buffer, &buf_size);
+
+	if (bson_decoder_init_buf(&decoder, buffer, buf_size, param_import_callback, &state)) {
+		debug("decoder init failed");
+		goto out;
+	}
+
+	state.mark_saved = mark_saved;
+
+	do {
+		result = bson_decoder_next(&decoder);
+
+	} while (result > 0);
+
+out:
+
+	if (result < 0) {
+		debug("BSON error decoding parameters");
+	}
+
+	return result;
+}
+
+int flash_param_save(void)
+{
+	return param_export_internal(false);
+}
+
+int flash_param_load(void)
+{
+	param_reset_all();
+	return param_import_internal(true);
+}
+
+int flash_param_import(void)
+{
+	return -1;
+}

src/lib/parameters/flashparams/flashparams.h

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file param.h
+ *
+ * Global flash based parameter store.
+ *
+ * This provides the mechanisms to interface to the PX4
+ * parameter system but replace the IO with non file based flash
+ * i/o routines. So that the code my be implemented on a SMALL memory
+ * foot print device.
+ *
+ */
+
+#ifndef _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H
+#define _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sys/types.h>
+#include "systemlib/uthash/utarray.h"
+
+__BEGIN_DECLS
+
+/*
+ * When using the flash based parameter store we have to force
+ * the param_values and 2 functions to be global
+ */
+
+#define FLASH_PARAMS_EXPOSE __EXPORT
+
+__EXPORT extern UT_array        *param_values;
+__EXPORT int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes);
+__EXPORT const void *param_get_value_ptr_external(param_t param);
+
+/* The interface hooks to the Flash based storage. The caller is responsible for locking */
+__EXPORT int flash_param_save(void);
+__EXPORT int flash_param_load(void);
+__EXPORT int flash_param_import(void);
+__END_DECLS
+#endif /* _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H */

src/lib/parameters/param.h

@@ -0,0 +1,502 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file param.h
+ *
+ * Global parameter store.
+ *
+ * Note that a number of API members are marked const or pure; these
+ * assume that the set of parameters cannot change, or that a parameter
+ * cannot change type or size over its lifetime.  If any of these assumptions
+ * are invalidated, the attributes should be re-evaluated.
+ */
+
+#ifndef _SYSTEMLIB_PARAM_PARAM_H
+#define _SYSTEMLIB_PARAM_PARAM_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sys/types.h>
+
+/** Maximum size of the parameter backing file */
+#define PARAM_FILE_MAXSIZE		4096
+
+__BEGIN_DECLS
+
+/**
+ * Parameter types.
+ */
+#define PARAM_TYPE_INT32		0
+#define PARAM_TYPE_FLOAT		1
+#define PARAM_TYPE_STRUCT		100
+#define PARAM_TYPE_STRUCT_MAX	(16384 + PARAM_TYPE_STRUCT)
+#define PARAM_TYPE_UNKNOWN		(0xffff)
+
+typedef uint16_t param_type_t;
+
+
+#ifdef __PX4_NUTTX // on NuttX use 16 bits to save RAM
+/**
+ * Parameter handle.
+ *
+ * Parameters are represented by parameter handles, which can
+ * be obtained by looking up parameters. They are an offset into a global
+ * constant parameter array.
+ */
+typedef uint16_t	param_t;
+
+/**
+ * Handle returned when a parameter cannot be found.
+ */
+#define PARAM_INVALID	((uint16_t)0xffff)
+
+/**
+ * Magic handle for hash check param
+ */
+#define PARAM_HASH      ((uint16_t)INT16_MAX)
+
+#else // on other platforms use 32 bits for better performance
+
+/**
+ * Parameter handle.
+ *
+ * Parameters are represented by parameter handles, which can
+ * be obtained by looking up parameters. They are an offset into a global
+ * constant parameter array.
+ */
+typedef uint32_t	param_t;
+
+/**
+ * Handle returned when a parameter cannot be found.
+ */
+#define PARAM_INVALID	((uint32_t)0xffffffff)
+
+/**
+ * Magic handle for hash check param
+ */
+#define PARAM_HASH      ((uint32_t)INT32_MAX)
+
+#endif /* __PX4_NUTTX */
+
+
+/**
+ * Initialize the param backend. Call this on startup before calling any other methods.
+ */
+__EXPORT void		param_init(void);
+
+/**
+ * Look up a parameter by name.
+ *
+ * @param name		The canonical name of the parameter being looked up.
+ * @return		A handle to the parameter, or PARAM_INVALID if the parameter does not exist.
+ *			This call will also set the parameter as "used" in the system, which is used
+ *			to e.g. show the parameter via the RC interface
+ */
+__EXPORT param_t	param_find(const char *name);
+
+/**
+ * Look up a parameter by name.
+ *
+ * @param name		The canonical name of the parameter being looked up.
+ * @return		A handle to the parameter, or PARAM_INVALID if the parameter does not exist.
+ */
+__EXPORT param_t	param_find_no_notification(const char *name);
+
+/**
+ * Return the total number of parameters.
+ *
+ * @return		The number of parameters.
+ */
+__EXPORT unsigned	param_count(void);
+
+/**
+ * Return the actually used number of parameters.
+ *
+ * @return		The number of parameters.
+ */
+__EXPORT unsigned	param_count_used(void);
+
+/**
+ * Wether a parameter is in use in the system.
+ *
+ * @return		True if it has been written or read
+ */
+__EXPORT bool		param_used(param_t param);
+
+/**
+ * Look up a parameter by index.
+ *
+ * @param index		An index from 0 to n, where n is param_count()-1.
+ * @return		A handle to the parameter, or PARAM_INVALID if the index is out of range.
+ */
+__EXPORT param_t	param_for_index(unsigned index);
+
+/**
+ * Look up an used parameter by index.
+ *
+ * @param index		The parameter to obtain the index for.
+ * @return		The index of the parameter in use, or -1 if the parameter does not exist.
+ */
+__EXPORT param_t	param_for_used_index(unsigned index);
+
+/**
+ * Look up the index of a parameter.
+ *
+ * @param param		The parameter to obtain the index for.
+ * @return		The index, or -1 if the parameter does not exist.
+ */
+__EXPORT int		param_get_index(param_t param);
+
+/**
+ * Look up the index of an used parameter.
+ *
+ * @param param		The parameter to obtain the index for.
+ * @return		The index of the parameter in use, or -1 if the parameter does not exist.
+ */
+__EXPORT int		param_get_used_index(param_t param);
+
+/**
+ * Obtain the name of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The name assigned to the parameter, or NULL if the handle is invalid.
+ */
+__EXPORT const char	*param_name(param_t param);
+
+/**
+ * Obtain the volatile state of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return			true if the parameter is volatile
+ */
+__EXPORT bool		param_is_volatile(param_t param);
+
+/**
+ * Test whether a parameter's value has changed from the default.
+ *
+ * @return		If true, the parameter's value has not been changed from the default.
+ */
+__EXPORT bool		param_value_is_default(param_t param);
+
+/**
+ * Test whether a parameter's value has been changed but not saved.
+ *
+ * @return		If true, the parameter's value has not been saved.
+ */
+__EXPORT bool		param_value_unsaved(param_t param);
+
+/**
+ * Obtain the type of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The type assigned to the parameter.
+ */
+__EXPORT param_type_t	param_type(param_t param);
+
+/**
+ * Determine the size of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The size of the parameter's value.
+ */
+__EXPORT size_t		param_size(param_t param);
+
+/**
+ * Copy the value of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @param val		Where to return the value, assumed to point to suitable storage for the parameter type.
+ *			For structures, a bitwise copy of the structure is performed to this address.
+ * @return		Zero if the parameter's value could be returned, nonzero otherwise.
+ */
+__EXPORT int		param_get(param_t param, void *val);
+
+/**
+ * Set the value of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @param val		The value to set; assumed to point to a variable of the parameter type.
+ *			For structures, the pointer is assumed to point to a structure to be copied.
+ * @return		Zero if the parameter's value could be set from a scalar, nonzero otherwise.
+ */
+__EXPORT int		param_set(param_t param, const void *val);
+
+/**
+ * Mark a parameter as used. Only marked parameters will be sent to a GCS.
+ * A call to param_find() will mark a param as used as well.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ */
+__EXPORT void		param_set_used(param_t param);
+
+/**
+ * Set the value of a parameter, but do not notify the system about the change.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @param val		The value to set; assumed to point to a variable of the parameter type.
+ *			For structures, the pointer is assumed to point to a structure to be copied.
+ * @return		Zero if the parameter's value could be set from a scalar, nonzero otherwise.
+ */
+__EXPORT int		param_set_no_notification(param_t param, const void *val);
+
+/**
+ * Notify the system about parameter changes. Can be used for example after several calls to
+ * param_set_no_notification() to avoid unnecessary system notifications.
+ */
+__EXPORT void		param_notify_changes(void);
+
+/**
+ * Reset a parameter to its default value.
+ *
+ * This function frees any storage used by struct parameters, and returns the parameter
+ * to its default value.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		Zero on success, nonzero on failure
+ */
+__EXPORT int		param_reset(param_t param);
+
+/**
+ * Reset all parameters to their default values.
+ *
+ * This function also releases the storage used by struct parameters.
+ */
+__EXPORT void		param_reset_all(void);
+
+/**
+ * Reset all parameters to their default values except for excluded parameters.
+ *
+ * This function also releases the storage used by struct parameters.
+ *
+ * @param excludes			Array of param names to exclude from resetting. Use a wildcard
+ *							at the end to exclude parameters with a certain prefix.
+ * @param num_excludes		The number of excludes provided.
+ */
+__EXPORT void		param_reset_excludes(const char *excludes[], int num_excludes);
+
+/**
+ * Export changed parameters to a file.
+ * Note: this method requires a large amount of stack size!
+ *
+ * @param fd		File descriptor to export to.
+ * @param only_unsaved	Only export changed parameters that have not yet been exported.
+ * @return		Zero on success, nonzero on failure.
+ */
+__EXPORT int		param_export(int fd, bool only_unsaved);
+
+/**
+ * Import parameters from a file, discarding any unrecognized parameters.
+ *
+ * This function merges the imported parameters with the current parameter set.
+ *
+ * @param fd		File descriptor to import from.  (Currently expected to be a file.)
+ * @return		Zero on success, nonzero if an error occurred during import.
+ *			Note that in the failure case, parameters may be inconsistent.
+ */
+__EXPORT int		param_import(int fd);
+
+/**
+ * Load parameters from a file.
+ *
+ * This function resets all parameters to their default values, then loads new
+ * values from a file.
+ *
+ * @param fd		File descriptor to import from.  (Currently expected to be a file.)
+ * @return		Zero on success, nonzero if an error occurred during import.
+ *			Note that in the failure case, parameters may be inconsistent.
+ */
+__EXPORT int		param_load(int fd);
+
+/**
+ * Apply a function to each parameter.
+ *
+ * Note that the parameter set is not locked during the traversal. It also does
+ * not hold an internal state, so the callback function can block or sleep between
+ * parameter callbacks.
+ *
+ * @param func		The function to invoke for each parameter.
+ * @param arg		Argument passed to the function.
+ * @param only_changed	If true, the function is only called for parameters whose values have
+ *			been changed from the default.
+ * @param only_changed	If true, the function is only called for parameters which have been
+ *			used in one of the running applications.
+ */
+__EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used);
+
+/**
+ * Set the default parameter file name.
+ *
+ * @param filename	Path to the default parameter file.  The file is not require to
+ *			exist.
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_set_default_file(const char *filename);
+
+/**
+ * Get the default parameter file name.
+ *
+ * @return		The path to the current default parameter file; either as
+ *			a result of a call to param_set_default_file, or the
+ *			built-in default.
+ */
+__EXPORT const char	*param_get_default_file(void);
+
+/**
+ * Save parameters to the default file.
+ * Note: this method requires a large amount of stack size!
+ *
+ * This function saves all parameters with non-default values.
+ *
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_save_default(void);
+
+/**
+ * Load parameters from the default parameter file.
+ *
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_load_default(void);
+
+/**
+ * Generate the hash of all parameters and their values
+ *
+ * @return		CRC32 hash of all param_ids and values
+ */
+__EXPORT uint32_t	param_hash_check(void);
+
+
+/**
+ * Enable/disable the param autosaving.
+ * Re-enabling with changed params will not cause an autosave.
+ * @param enable true: enable autosaving, false: disable autosaving
+ */
+__EXPORT void	param_control_autosave(bool enable);
+
+/*
+ * Macros creating static parameter definitions.
+ *
+ * Note that these structures are not known by name; they are
+ * collected into a section that is iterated by the parameter
+ * code.
+ *
+ * Note that these macros cannot be used in C++ code due to
+ * their use of designated initializers.  They should probably
+ * be refactored to avoid the use of a union for param_value_u.
+ */
+
+/** define an int32 parameter */
+#define PARAM_DEFINE_INT32(_name, _default)
+
+/** define a float parameter */
+#define PARAM_DEFINE_FLOAT(_name, _default)
+
+/** define a parameter that points to a structure */
+#define PARAM_DEFINE_STRUCT(_name, _default)
+
+/**
+ * Parameter value union.
+ */
+union param_value_u {
+	void		*p;
+	int32_t		i;
+	float		f;
+};
+
+/**
+ * Static parameter definition structure.
+ *
+ * This is normally not used by user code; see the PARAM_DEFINE macros
+ * instead.
+ */
+struct param_info_s {
+	const char	*name
+
+// GCC 4.8 and higher don't implement proper alignment of static data on
+// 64-bit. This means that the 24-byte param_info_s variables are
+// 16 byte aligned by GCC and that messes up the assumption that
+// sequential items in the __param segment can be addressed as an array.
+// The assumption is that the address of the second parameter is at
+// &param[0]+sizeof(param[0]). When compiled with clang it is
+// true, with gcc is is not true.
+// See https://llvm.org/bugs/show_bug.cgi?format=multiple&id=18006
+// The following hack is for GCC >=4.8 only. Clang works fine without
+// this.
+#ifdef __PX4_POSIX
+	__attribute__((aligned(16)));
+#else
+	;
+#endif
+	param_type_t	type;
+	uint16_t		volatile_param: 1;
+	union param_value_u val;
+};
+
+__END_DECLS
+
+
+
+#ifdef	__cplusplus
+#if 0 // set to 1 to debug param type mismatches
+#include <cstdio>
+#define CHECK_PARAM_TYPE(param, type) \
+	if (param_type(param) != type) { \
+		/* use printf() to avoid having to use more includes */ \
+		printf("wrong type passed to param_get() for param %s\n", param_name(param)); \
+	}
+#else
+#define CHECK_PARAM_TYPE(param, type)
+#endif
+
+// param is a C-interface. This means there is no overloading, and thus no type-safety for param_get().
+// So for C++ code we redefine param_get() to inlined overloaded versions, which gives us type-safety
+// w/o having to use a different interface
+static inline int param_get_cplusplus(param_t param, float *val)
+{
+	CHECK_PARAM_TYPE(param, PARAM_TYPE_FLOAT);
+	return param_get(param, val);
+}
+static inline int param_get_cplusplus(param_t param, int32_t *val)
+{
+	CHECK_PARAM_TYPE(param, PARAM_TYPE_INT32);
+	return param_get(param, val);
+}
+#undef CHECK_PARAM_TYPE
+
+#define param_get(param, val) param_get_cplusplus(param, val)
+
+
+#endif /* __cplusplus */
+
+#endif

src/lib/parameters/parameters_injected.xml

@@ -0,0 +1,138 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<parameters>
+  <version>3</version>
+  <group name="UAVCAN Motor Parameters" no_code_generation="true">
+    <parameter default="75" name="ctl_bw" type="INT32">
+      <short_desc>Speed controller bandwidth</short_desc>
+      <long_desc>Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness.</long_desc>
+      <unit>Hertz</unit>
+      <min>10</min>
+      <max>250</max>
+    </parameter>
+    <parameter default="1" name="ctl_dir" type="INT32">
+      <short_desc>Reverse direction</short_desc>
+      <long_desc>Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction.</long_desc>
+      <min>0</min>
+      <max>1</max>
+    </parameter>
+    <parameter default="1" name="ctl_gain" type="FLOAT">
+      <short_desc>Speed (RPM) controller gain</short_desc>
+      <long_desc>Speed (RPM) controller gain. Determines controller
+            aggressiveness; units are amp-seconds per radian. Systems with
+            higher rotational inertia (large props) will need gain increased;
+            systems with low rotational inertia (small props) may need gain
+            decreased. Higher values result in faster response, but may result
+            in oscillation and excessive overshoot. Lower values result in a
+            slower, smoother response.</long_desc>
+      <unit>amp-seconds per radian</unit>
+      <decimal>3</decimal>
+      <min>0.00</min>
+      <max>1.00</max>
+    </parameter>
+    <parameter default="3.5" name="ctl_hz_idle" type="FLOAT">
+      <short_desc>Idle speed (e Hz)</short_desc>
+      <long_desc>Idle speed (e Hz)</long_desc>
+      <unit>Hertz</unit>
+      <decimal>3</decimal>
+      <min>0.0</min>
+      <max>100.0</max>
+    </parameter>
+    <parameter default="25" name="ctl_start_rate" type="INT32">
+      <short_desc>Spin-up rate (e Hz/s)</short_desc>
+      <long_desc>Spin-up rate (e Hz/s)</long_desc>
+      <unit>Hz/s</unit>
+      <min>5</min>
+      <max>1000</max>
+    </parameter>
+    <parameter default="0" name="esc_index" type="INT32">
+      <short_desc>Index of this ESC in throttle command messages.</short_desc>
+      <long_desc>Index of this ESC in throttle command messages.</long_desc>
+      <unit>Index</unit>
+      <min>0</min>
+      <max>15</max>
+    </parameter>
+    <parameter default="20034" name="id_ext_status" type="INT32">
+      <short_desc>Extended status ID</short_desc>
+      <long_desc>Extended status ID</long_desc>
+      <min>1</min>
+      <max>1000000</max>
+    </parameter>
+    <parameter default="50000" name="int_ext_status" type="INT32">
+      <short_desc>Extended status interval (µs)</short_desc>
+      <long_desc>Extended status interval (µs)</long_desc>
+      <unit>µs</unit>
+      <min>0</min>
+      <max>1000000</max>
+    </parameter>
+    <parameter default="50000" name="int_status" type="INT32">
+      <short_desc>ESC status interval (µs)</short_desc>
+      <long_desc>ESC status interval (µs)</long_desc>
+      <unit>µs</unit>
+      <max>1000000</max>
+    </parameter>
+    <parameter default="12" name="mot_i_max" type="FLOAT">
+      <short_desc>Motor current limit in amps</short_desc>
+      <long_desc>Motor current limit in amps. This determines the maximum
+            current controller setpoint, as well as the maximum allowable
+            current setpoint slew rate. This value should generally be set to
+            the continuous current rating listed in the motor’s specification
+            sheet, or set equal to the motor’s specified continuous power
+            divided by the motor voltage limit.</long_desc>
+      <unit>Amps</unit>
+      <decimal>3</decimal>
+      <min>1</min>
+      <max>80</max>
+    </parameter>
+    <parameter default="2300" name="mot_kv" type="INT32">
+      <short_desc>Motor Kv in RPM per volt</short_desc>
+      <long_desc>Motor Kv in RPM per volt. This can be taken from the motor’s
+            specification sheet; accuracy will help control performance but
+            some deviation from the specified value is acceptable.</long_desc>
+      <unit>RPM/v</unit>
+      <min>0</min>
+      <max>4000</max>
+    </parameter>
+      <parameter default="0.0" name="mot_ls" type="FLOAT">
+      <short_desc>READ ONLY: Motor inductance in henries.</short_desc>
+      <long_desc>READ ONLY: Motor inductance in henries. This is measured on start-up.</long_desc>
+      <unit>henries</unit>
+      <decimal>3</decimal>
+    </parameter>
+    <parameter default="14" name="mot_num_poles" type="INT32">
+      <short_desc>Number of motor poles.</short_desc>
+      <long_desc>Number of motor poles. Used to convert mechanical speeds to
+            electrical speeds. This number should be taken from the motor’s
+            specification sheet.</long_desc>
+      <unit>Poles</unit>
+      <min>2</min>
+      <max>40</max>
+    </parameter>
+    <parameter default="0.0" name="mot_rs" type="FLOAT">
+      <short_desc>READ ONLY: Motor resistance in ohms</short_desc>
+      <long_desc>READ ONLY: Motor resistance in ohms. This is measured on start-up. When
+            tuning a new motor, check that this value is approximately equal
+            to the value shown in the motor’s specification sheet.</long_desc>
+      <unit>Ohms</unit>
+      <decimal>3</decimal>
+    </parameter>
+    <parameter default="0.5" name="mot_v_accel" type="FLOAT">
+      <short_desc>Acceleration limit (V)</short_desc>
+      <long_desc>Acceleration limit (V)</long_desc>
+      <unit>Volts</unit>
+      <decimal>3</decimal>
+      <min>0.01</min>
+      <max>1.00</max>
+    </parameter>
+    <parameter default="14.8" name="mot_v_max" type="FLOAT">
+      <short_desc>Motor voltage limit in volts</short_desc>
+      <long_desc>Motor voltage limit in volts. The current controller’s
+            commanded voltage will never exceed this value. Note that this may
+            safely be above the nominal voltage of the motor; to determine the
+            actual motor voltage limit, divide the motor’s rated power by the
+            motor current limit.</long_desc>
+      <unit>Volts</unit>
+      <decimal>3</decimal>
+      <min>0</min>
+    </parameter>
+  </group>
+</parameters>

src/lib/parameters/px4params/README.md

@@ -0,0 +1 @@
+This folder contains a python library used by px_process_params.py

src/lib/parameters/px4params/__init__.py

@@ -0,0 +1 @@
+__all__ = ["srcscanner", "srcparser", "xmlout", "dokuwikiout", "dokuwikirpc", "scope"]

src/lib/parameters/px4params/dokuwikiout.py

@@ -0,0 +1,44 @@
+from xml.sax.saxutils import escape
+import codecs
+
+class DokuWikiTablesOutput():
+    def __init__(self, groups):
+        result = ("====== Parameter Reference ======\n"
+                  "<note>**This list is auto-generated from the source code** and contains the most recent parameter documentation.</note>\n"
+                  "\n")
+        for group in groups:
+            result += "==== %s ====\n\n" % group.GetName()
+            result += "|< 100% 25% 45% 10% 10% 10% >|\n"
+            result += "^ Name ^ Description ^  Min ^  Max ^  Default ^\n"
+            result += "^ :::  ^ Comment ^^^^\n"
+            for param in group.GetParams():
+                code = param.GetName()
+                def_val = param.GetDefault()
+                name = param.GetFieldValue("short_desc")
+                min_val = param.GetFieldValue("min")
+                max_val = param.GetFieldValue("max")
+                long_desc = param.GetFieldValue("long_desc")
+
+                if name == code:
+                    name = ""
+                else:
+                    name = name.replace("\n", " ")
+                    name = name.replace("|", "%%|%%")
+                    name = name.replace("^", "%%^%%")
+
+                result += "| **%s** |" % code
+                result += " %s |" % name
+                result += "  %s |" % (min_val or "")
+                result += "  %s |" % (max_val or "")
+                result += "  %s |" % (def_val or "")
+                result += "\n"
+
+                if long_desc is not None:
+                    result += "| ::: | <div>%s</div> ||||\n" % long_desc
+
+            result += "\n"
+        self.output = result;
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            f.write(self.output)

src/lib/parameters/px4params/dokuwikirpc.py

@@ -0,0 +1,16 @@
+try:
+    import xmlrpclib
+except ImportError:
+    import xmlrpc.client as xmlrpclib 
+
+# See https://www.dokuwiki.org/devel:xmlrpc for a list of available functions!
+# Usage example:
+#     xmlrpc = dokuwikirpc.get_xmlrpc(url, username, password)
+#     print(xmlrpc.dokuwiki.getVersion())
+
+def get_xmlrpc(url, username, password):
+    #proto, url = url.split("://")
+    #url = proto + "://" + username + ":" + password + "@" + url + "/lib/exe/xmlrpc.php"
+    url += "/lib/exe/xmlrpc.php?u=" + username + "&p=" + password
+    
+    return xmlrpclib.ServerProxy(url)

src/lib/parameters/px4params/markdownout.py

@@ -0,0 +1,105 @@
+from xml.sax.saxutils import escape
+import codecs
+
+class MarkdownTablesOutput():
+    def __init__(self, groups):
+        result = ("# Parameter Reference\n"
+                  "> **Note** **This list is auto-generated from the source code** and contains the most recent parameter documentation.\n"
+                  "\n")
+        for group in groups:
+            result += '## %s\n\n' % group.GetName()
+            
+            #Check if scope (module where parameter is defined) is the same for all parameters in the group. 
+            # If so then display just once about the table. 
+            scope_set = set()
+            for param in group.GetParams():
+                scope_set.add(param.GetFieldValue("scope"))
+            if len(scope_set)==1:
+                result+='\nThe module where these parameters are defined is: *%s*.\n\n' %  list(scope_set)[0]
+            
+            
+            result += '<table style="width: 100%; table-layout:fixed; font-size:1.5rem; overflow: auto; display:block;">\n'
+            result += ' <colgroup><col style="width: 23%"><col style="width: 46%"><col style="width: 11%"><col style="width: 11%"><col style="width: 9%"></colgroup>\n'
+            result += ' <thead>\n'
+            result += '   <tr><th>Name</th><th>Description</th><th>Min > Max (Incr.)</th><th>Default</th><th>Units</th></tr>\n'
+            result += ' </thead>\n'
+            result += '<tbody>\n'
+            
+            for param in group.GetParams():
+                code = param.GetName()
+                name = param.GetFieldValue("short_desc") or ''
+                long_desc = param.GetFieldValue("long_desc") or ''
+                min_val = param.GetFieldValue("min") or ''
+                max_val = param.GetFieldValue("max") or ''
+                increment = param.GetFieldValue("increment") or ''
+                def_val = param.GetDefault() or ''
+                unit = param.GetFieldValue("unit") or ''
+                type = param.GetType()
+                reboot_required = param.GetFieldValue("reboot_required") or ''
+                #board = param.GetFieldValue("board") or '' ## Disabled as no board values are defined in any parameters!
+                #decimal = param.GetFieldValue("decimal") or '' #Disabled as is intended for GCS not people
+                #field_codes = param.GetFieldCodes() ## Disabled as not needed for display. 
+                #boolean = param.GetFieldValue("boolean") # or '' # Disabled - does not appear useful.
+
+
+                # Format values for display.
+                # Display min/max/increment value based on what values are defined.
+                max_min_combined = ''
+                if min_val or max_val:
+                    if not min_val:
+                        min_val='?'
+                    if not max_val:
+                        max_val='?'
+                    max_min_combined+='%s > %s ' % (min_val, max_val)
+                if increment:
+                    max_min_combined+='(%s)' % increment
+
+                if long_desc is not '':
+                    long_desc = '<p><strong>Comment:</strong> %s</p>' % long_desc
+
+                if name == code:
+                    name = ""
+                code='<strong id="%s">%s</strong>' % (code, code)
+
+                if reboot_required:
+                    reboot_required='<p><b>Reboot required:</b> %s</p>\n' % reboot_required
+                
+                scope=''
+                if not len(scope_set)==1 or len(scope_set)==0:
+                    scope = param.GetFieldValue("scope") or ''
+                    if scope:
+                        scope='<p><b>Module:</b> %s</p>\n' % scope
+
+
+                enum_codes=param.GetEnumCodes() or '' # Gets numerical values for parameter.
+                enum_output=''
+                # Format codes and their descriptions for display. 
+                if enum_codes:
+                    enum_output+='<strong>Values:</strong><ul>'
+                    enum_codes=sorted(enum_codes,key=float)
+                    for item in enum_codes:
+                        enum_output+='\n<li><strong>%s:</strong> %s</li> \n' % (item, param.GetEnumValue(item))
+                    enum_output+='</ul>\n'
+                    
+
+                bitmask_list=param.GetBitmaskList() #Gets bitmask values for parameter
+                bitmask_output=''
+                #Format bitmask values
+                if bitmask_list:
+                    bitmask_output+='<strong>Bitmask:</strong><ul>'
+                    for bit in bitmask_list:
+                        bit_text = param.GetBitmaskBit(bit)
+                        bitmask_output+='  <li><strong>%s:</strong> %s</li> \n' % (bit, bit_text)
+                    bitmask_output+='</ul>\n'
+
+                    
+                result += '<tr>\n <td style="vertical-align: top;">%s (%s)</td>\n <td style="vertical-align: top;"><p>%s</p>%s %s %s %s %s</td>\n <td style="vertical-align: top;">%s</td>\n <td style="vertical-align: top;">%s </td>\n <td style="vertical-align: top;">%s</td>\n</tr>\n' % (code,type,name, long_desc, enum_output, bitmask_output, reboot_required, scope, max_min_combined,def_val,unit)
+
+            #Close the table.
+            result += '</tbody></table>\n\n'
+
+        self.output = result
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            f.write(self.output)

src/lib/parameters/px4params/scope.py

@@ -0,0 +1,32 @@
+import os
+import re
+
+class Scope(object):
+    """
+    Single parameter group
+    """
+    re_deep_lines = re.compile(r'.*\/.*\/')
+    def __init__(self,):
+        self.scope = set()
+
+
+    def __str__(self):
+        return self.scope.__str__()
+
+    def Add(self, scope):
+        """
+        Add Scope to set
+        """
+        self.scope.add(scope)
+
+    def Has(self, scope):
+        """
+        Check for existance
+        """
+        if len(self.scope) == 0:
+            return True
+        # Anything in the form xxxxx/yyyyy/zzzzz....
+        # is treated as xxxxx/yyyyy
+        while (self.re_deep_lines.match(scope)):
+            scope = os.path.dirname(scope)
+        return scope in self.scope

src/lib/parameters/px4params/srcparser.py

@@ -0,0 +1,404 @@
+import sys
+import re
+import math
+
+global default_var
+default_var = {}
+
+class ParameterGroup(object):
+    """
+    Single parameter group
+    """
+    def __init__(self, name):
+        self.name = name
+        self.params = []
+
+    def AddParameter(self, param):
+        """
+        Add parameter to the group
+        """
+        self.params.append(param)
+
+    def GetName(self):
+        """
+        Get parameter group name
+        """
+        return self.name
+
+    def GetParams(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
+        """
+        return sorted(self.params, key=lambda param: param.name)
+
+class Parameter(object):
+    """
+    Single parameter
+    """
+
+    # Define sorting order of the fields
+    priority = {
+        "board": 9,
+        "short_desc": 8,
+        "long_desc": 7,
+        "min": 5,
+        "max": 4,
+        "unit": 3,
+        "decimal": 2,
+        # all others == 0 (sorted alphabetically)
+    }
+
+    def __init__(self, name, type, default = ""):
+        self.fields = {}
+        self.values = {}
+        self.bitmask = {}
+        self.name = name
+        self.type = type
+        self.default = default
+        self.volatile = "false"
+        self.category = ""
+
+    def GetName(self):
+        return self.name
+
+    def GetType(self):
+        return self.type
+
+    def GetDefault(self):
+        return self.default
+
+    def GetCategory(self):
+        return self.category.title()
+
+    def GetVolatile(self):
+        return self.volatile
+
+    def SetField(self, code, value):
+        """
+        Set named field value
+        """
+        self.fields[code] = value
+
+    def SetEnumValue(self, code, value):
+        """
+        Set named enum value
+        """
+        self.values[code] = value
+
+    def SetBitmaskBit(self, index, bit):
+        """
+        Set named enum value
+        """
+        self.bitmask[index] = bit
+
+    def SetVolatile(self):
+        """
+        Set volatile flag
+        """
+        self.volatile = "true"
+
+    def SetCategory(self, category):
+        """
+        Set param category
+        """
+        self.category = category
+
+    def GetFieldCodes(self):
+        """
+        Return list of existing field codes in convenient order
+        """
+        keys = self.fields.keys()
+        keys = sorted(keys)
+        keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
+        return keys
+
+    def GetFieldValue(self, code):
+        """
+        Return value of the given field code or None if not found.
+        """
+        fv =  self.fields.get(code)
+        if not fv:
+                # required because python 3 sorted does not accept None
+                return ""
+        return fv
+
+    def GetEnumCodes(self):
+        """
+        Return list of existing value codes in convenient order
+        """
+        return sorted(self.values.keys(), key=float)
+
+    def GetEnumValue(self, code):
+        """
+        Return value of the given enum code or None if not found.
+        """
+        fv =  self.values.get(code)
+        if not fv:
+                # required because python 3 sorted does not accept None
+                return ""
+        return fv
+
+    def GetBitmaskList(self):
+        """
+        Return list of existing bitmask codes in convenient order
+        """
+        keys = self.bitmask.keys()
+        return sorted(keys, key=float)
+
+    def GetBitmaskBit(self, index):
+        """
+        Return value of the given bitmask code or None if not found.
+        """
+        fv =  self.bitmask.get(index)
+        if not fv:
+                # required because python 3 sorted does not accept None
+                return ""
+        return fv
+
+class SourceParser(object):
+    """
+    Parses provided data and stores all found parameters internally.
+    """
+
+    re_split_lines = re.compile(r'[\r\n]+')
+    re_comment_start = re.compile(r'^\/\*\*')
+    re_comment_content = re.compile(r'^\*\s*(.*)')
+    re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
+    re_comment_end = re.compile(r'(.*?)\s*\*\/')
+    re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
+    re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;')
+    re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*')
+    re_cut_type_specifier = re.compile(r'[a-z]+$')
+    re_is_a_number = re.compile(r'^-?[0-9\.]')
+    re_remove_dots = re.compile(r'\.+$')
+    re_remove_carriage_return = re.compile('\n+')
+
+    valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "increment", "reboot_required", "value", "boolean", "bit", "category", "volatile"])
+
+    # Order of parameter groups
+    priority = {
+        # All other groups = 0 (sort alphabetically)
+        "Miscellaneous": -10
+    }
+
+    def __init__(self):
+        self.param_groups = {}
+
+    def Parse(self, scope, contents):
+        """
+        Incrementally parse program contents and append all found parameters
+        to the list.
+        """
+        # This code is essentially a comment-parsing grammar. "state"
+        # represents parser state. It contains human-readable state
+        # names.
+        state = None
+        for line in self.re_split_lines.split(contents):
+            line = line.strip()
+            # Ignore empty lines
+            if line == "":
+                continue
+            if self.re_comment_start.match(line):
+                state = "wait-short"
+                short_desc = None
+                long_desc = None
+                tags = {}
+                def_values = {}
+                def_bitmask = {}
+            elif state is not None and state != "comment-processed":
+                m = self.re_comment_end.search(line)
+                if m:
+                    line = m.group(1)
+                    last_comment_line = True
+                else:
+                    last_comment_line = False
+                m = self.re_comment_content.match(line)
+                if m:
+                    comment_content = m.group(1)
+                    if comment_content == "":
+                        # When short comment ends with empty comment line,
+                        # start waiting for the next part - long comment.
+                        if state == "wait-short-end":
+                            state = "wait-long"
+                    else:
+                        m = self.re_comment_tag.match(comment_content)
+                        if m:
+                            tag, desc = m.group(1, 2)
+                            if (tag == "value"):
+                                # Take the meta info string and split the code and description
+                                metainfo = desc.split(" ",  1)
+                                def_values[metainfo[0]] = metainfo[1]
+                            elif (tag == "bit"):
+                                # Take the meta info string and split the code and description
+                                metainfo = desc.split(" ",  1)
+                                def_bitmask[metainfo[0]] = metainfo[1]
+                            else:
+                                tags[tag] = desc
+                            current_tag = tag
+                            state = "wait-tag-end"
+                        elif state == "wait-short":
+                            # Store first line of the short description
+                            short_desc = comment_content
+                            state = "wait-short-end"
+                        elif state == "wait-short-end":
+                            # Append comment line to the short description
+                            short_desc += "\n" + comment_content
+                        elif state == "wait-long":
+                            # Store first line of the long description
+                            long_desc = comment_content
+                            state = "wait-long-end"
+                        elif state == "wait-long-end":
+                            # Append comment line to the long description
+                            long_desc += "\n" + comment_content
+                        elif state == "wait-tag-end":
+                            # Append comment line to the tag text
+                            tags[current_tag] += "\n" + comment_content
+                        else:
+                            raise AssertionError(
+                                    "Invalid parser state: %s" % state)
+                elif not last_comment_line:
+                    # Invalid comment line (inside comment, but not starting with
+                    # "*" or "*/". Reset parsed content.
+                    state = None
+                if last_comment_line:
+                    state = "comment-processed"
+            else:
+                tp = None
+                name = None
+                defval = ""
+                # Non-empty line outside the comment
+                m = self.re_px4_param_default_definition.match(line)
+                # Default value handling
+                if m:
+                    name_m, defval_m = m.group(1,2)
+                    default_var[name_m] = defval_m
+                m = self.re_parameter_definition.match(line)
+                if m:
+                    tp, name, defval = m.group(1, 2, 3)
+                else:
+                    m = self.re_px4_parameter_definition.match(line)
+                    if m:
+                        tp, name = m.group(1, 2)
+                        if (name+'_DEFAULT') in default_var:
+                            defval = default_var[name+'_DEFAULT']
+                if tp is not None:
+                    # Remove trailing type specifier from numbers: 0.1f => 0.1
+                    if defval != "" and self.re_is_a_number.match(defval):
+                        defval = self.re_cut_type_specifier.sub('', defval)
+                    param = Parameter(name, tp, defval)
+                    param.SetField("scope", scope)
+                    param.SetField("short_desc", name)
+                    # If comment was found before the parameter declaration,
+                    # inject its data into the newly created parameter.
+                    group = "Miscellaneous"
+                    if state == "comment-processed":
+                        if short_desc is not None:
+                            param.SetField("short_desc", self.re_remove_dots.sub('', short_desc))
+                        if long_desc is not None:
+                            long_desc = self.re_remove_carriage_return.sub(' ', long_desc)
+                            param.SetField("long_desc", long_desc)
+                        for tag in tags:
+                            if tag == "group":
+                                group = tags[tag]
+                            elif tag == "volatile":
+                                param.SetVolatile()
+                            elif tag == "category":
+                                param.SetCategory(tags[tag])
+                            elif tag not in self.valid_tags:
+                                sys.stderr.write("Skipping invalid documentation tag: '%s'\n" % tag)
+                                return False
+                            else:
+                                param.SetField(tag, tags[tag])
+                        for def_value in def_values:
+                            param.SetEnumValue(def_value, def_values[def_value])
+                        for def_bit in def_bitmask:
+                            param.SetBitmaskBit(def_bit, def_bitmask[def_bit])
+                    # Store the parameter
+                    if group not in self.param_groups:
+                        self.param_groups[group] = ParameterGroup(group)
+                    self.param_groups[group].AddParameter(param)
+                state = None
+        return True
+    
+    def IsNumber(self, numberString):
+        try:
+            float(numberString)
+            return True
+        except ValueError:
+            return False
+
+    def Validate(self):
+        """
+        Validates the parameter meta data.
+        """
+        seenParamNames = []
+        for group in self.GetParamGroups():
+            for param in group.GetParams():
+                name  = param.GetName()
+                if len(name) > 16:
+                    sys.stderr.write("Parameter Name {0} is too long (Limit is 16)\n".format(name))
+                    return False
+                board = param.GetFieldValue("board")
+                # Check for duplicates
+                name_plus_board = name + "+" + board
+                for seenParamName in seenParamNames:
+                    if seenParamName == name_plus_board:
+                        sys.stderr.write("Duplicate parameter definition: {0}\n".format(name_plus_board))
+                        return False
+                seenParamNames.append(name_plus_board)
+                # Validate values
+                default = param.GetDefault()
+                min = param.GetFieldValue("min")
+                max = param.GetFieldValue("max")
+                #sys.stderr.write("{0} default:{1} min:{2} max:{3}\n".format(name, default, min, max))
+                if default != "" and not self.IsNumber(default):
+                    sys.stderr.write("Default value not number: {0} {1}\n".format(name, default))
+                    return False
+                # if default != "" and "." not in default:
+                #     sys.stderr.write("Default value does not contain dot (e.g. 10 needs to be written as 10.0): {0} {1}\n".format(name, default))
+                #     return False
+                if min != "":
+                    if not self.IsNumber(min):
+                        sys.stderr.write("Min value not number: {0} {1}\n".format(name, min))
+                        return False
+                    if default != "" and float(default) < float(min):
+                        sys.stderr.write("Default value is smaller than min: {0} default:{1} min:{2}\n".format(name, default, min))
+                        return False
+                if max != "":
+                    if not self.IsNumber(max):
+                        sys.stderr.write("Max value not number: {0} {1}\n".format(name, max))
+                        return False
+                    if default != "" and float(default) > float(max):
+                        sys.stderr.write("Default value is larger than max: {0} default:{1} max:{2}\n".format(name, default, max))
+                        return False
+                for code in param.GetEnumCodes():
+                        if not self.IsNumber(code):
+                            sys.stderr.write("Min value not number: {0} {1}\n".format(name, code))
+                            return False
+                        if param.GetEnumValue(code) == "":
+                            sys.stderr.write("Description for enum value is empty: {0} {1}\n".format(name, code))
+                            return False
+                for index in param.GetBitmaskList():
+                        if not self.IsNumber(index):
+                            sys.stderr.write("bit value not number: {0} {1}\n".format(name, index))
+                            return False
+                        if not int(min) <= math.pow(2, int(index)) <= int(max):
+                            sys.stderr.write("Bitmask bit must be between {0} and {1}: {2} {3}\n".format(min, max, name, math.pow(2, int(index))))
+                            return False
+                        if param.GetBitmaskBit(index) == "":
+                            sys.stderr.write("Description for bitmask bit is empty: {0} {1}\n".format(name, index))
+                            return False
+        return True
+
+    def GetParamGroups(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
+        """
+        groups = self.param_groups.values()
+        groups = sorted(groups, key=lambda x: x.GetName())
+        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
+        return groups

src/lib/parameters/px4params/srcscanner.py

@@ -0,0 +1,50 @@
+import os
+import re
+import codecs
+import sys
+
+class SourceScanner(object):
+    """
+    Traverses directory tree, reads all source files, and passes their contents
+    to the Parser.
+    """
+
+    def ScanDir(self, srcdirs, parser):
+        """
+        Scans provided path and passes all found contents to the parser using
+        parser.Parse method.
+        """
+        extensions1 = tuple([".h"])
+        extensions2 = tuple([".c"])
+        for srcdir in srcdirs:
+            for filename in os.listdir(srcdir):
+                if filename.endswith(extensions1):
+                    path = os.path.join(srcdir, filename)
+                    if not self.ScanFile(path, parser):
+                        return False
+            for filename in os.listdir(srcdir):
+                if filename.endswith(extensions2):
+                    path = os.path.join(srcdir, filename)
+                    if not self.ScanFile(path, parser):
+                        return False
+        return True
+
+    def ScanFile(self, path, parser):
+        """
+        Scans provided file and passes its contents to the parser using
+        parser.Parse method.
+        """
+        # Extract the scope: it is the directory within the repo. Either it
+        # starts directly with 'src/module/abc', or it has the form 'x/y/z/src/module/abc'.
+        # The output is 'module/abc' in both cases.
+        prefix = "^(|.*" + os.path.sep + ")src" + os.path.sep
+        scope = re.sub(prefix.replace("\\", "/"), "", os.path.dirname(os.path.relpath(path)).replace("\\", "/"))
+
+        with codecs.open(path, 'r', 'utf-8') as f:
+            try:
+                contents = f.read()
+            except:
+                contents = ''
+                print('Failed reading file: %s, skipping content.' % path)
+                pass
+        return parser.Parse(scope, contents)

src/lib/parameters/px4params/xmlout.py

@@ -0,0 +1,82 @@
+import xml.etree.ElementTree as ET
+import codecs
+
+def indent(elem, level=0):
+    i = "\n" + level*"  "
+    if len(elem):
+        if not elem.text or not elem.text.strip():
+            elem.text = i + "  "
+        if not elem.tail or not elem.tail.strip():
+            elem.tail = i
+        for elem in elem:
+            indent(elem, level+1)
+        if not elem.tail or not elem.tail.strip():
+            elem.tail = i
+    else:
+        if level and (not elem.tail or not elem.tail.strip()):
+            elem.tail = i
+
+class XMLOutput():
+
+    def __init__(self, groups, board, inject_xml_file_name):
+        xml_parameters = ET.Element("parameters")
+        xml_version = ET.SubElement(xml_parameters, "version")
+        xml_version.text = "3"
+        xml_version = ET.SubElement(xml_parameters, "parameter_version_major")
+        xml_version.text = "1"
+        xml_version = ET.SubElement(xml_parameters, "parameter_version_minor")
+        xml_version.text = "15"
+        importtree = ET.parse(inject_xml_file_name)
+        injectgroups = importtree.getroot().findall("group")
+        for igroup in injectgroups:
+            xml_parameters.append(igroup)
+        last_param_name = ""
+        board_specific_param_set = False
+        for group in groups:
+            xml_group = ET.SubElement(xml_parameters, "group")
+            xml_group.attrib["name"] = group.GetName()
+            for param in group.GetParams():
+                if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName():
+                    xml_param = ET.SubElement(xml_group, "parameter")
+                    xml_param.attrib["name"] = param.GetName()
+                    xml_param.attrib["default"] = param.GetDefault()
+                    xml_param.attrib["type"] = param.GetType()
+                    if (param.GetVolatile() == "true"):
+                        xml_param.attrib["volatile"] = param.GetVolatile()
+                    if (param.GetCategory()):
+                        xml_param.attrib["category"] = param.GetCategory()
+                    last_param_name = param.GetName()
+                    for code in param.GetFieldCodes():
+                        value = param.GetFieldValue(code)
+                        if code == "board":
+                            if value == board:
+                                board_specific_param_set = True
+                                xml_field = ET.SubElement(xml_param, code)
+                                xml_field.text = value
+                            else:
+                                xml_group.remove(xml_param)
+                        else:
+                            xml_field = ET.SubElement(xml_param, code)
+                            xml_field.text = value
+                if last_param_name != param.GetName():
+                    board_specific_param_set = False
+                
+                if len(param.GetEnumCodes()) > 0:
+                    xml_values = ET.SubElement(xml_param, "values")
+                    for code in param.GetEnumCodes():
+                        xml_value = ET.SubElement(xml_values, "value")
+                        xml_value.attrib["code"] = code;
+                        xml_value.text = param.GetEnumValue(code)
+
+                if len(param.GetBitmaskList()) > 0:
+                    xml_values = ET.SubElement(xml_param, "bitmask")
+                    for index in param.GetBitmaskList():
+                        xml_value = ET.SubElement(xml_values, "bit")
+                        xml_value.attrib["index"] = index;
+                        xml_value.text = param.GetBitmaskBit(index)
+
+        indent(xml_parameters)
+        self.xml_document = ET.ElementTree(xml_parameters)
+
+    def Save(self, filename):
+        self.xml_document.write(filename, encoding="UTF-8")

src/lib/parameters/px_generate_params.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+"""
+Param source code generation script.
+"""
+from __future__ import print_function
+import xml.etree.ElementTree as ET
+import codecs
+import argparse
+from jinja2 import Environment, FileSystemLoader
+import os
+
+def generate(xml_file, dest='.'):
+    """
+    Generate px4 param source from xml.
+
+    @param xml_file: input parameter xml file
+    @param dest: Destination directory for generated files
+        None means to scan everything.
+    """
+    # pylint: disable=broad-except
+    tree = ET.parse(xml_file)
+    root = tree.getroot()
+
+    params = []
+    for group in root:
+        if group.tag == "group" and "no_code_generation" not in group.attrib:
+            for param in group:
+                params.append(param)
+
+    params = sorted(params, key=lambda name: name.attrib["name"])
+
+    script_path = os.path.dirname(os.path.realpath(__file__))
+
+    # for jinja docs see: http://jinja.pocoo.org/docs/2.9/api/
+    env = Environment(
+        loader=FileSystemLoader(os.path.join(script_path, 'templates')))
+
+    if not os.path.isdir(dest):
+        os.path.mkdir(dest)
+
+    template_files = [
+        'px4_parameters.h.jinja',
+        'px4_parameters_public.h.jinja',
+        'px4_parameters.c.jinja',
+    ]
+    for template_file in template_files:
+        template = env.get_template(template_file)
+        with open(os.path.join(
+                dest, template_file.replace('.jinja','')), 'w') as fid:
+            fid.write(template.render(params=params))
+
+if __name__ == "__main__":
+    arg_parser = argparse.ArgumentParser()
+    arg_parser.add_argument("--xml", help="parameter xml file")
+    arg_parser.add_argument("--dest", help="destination path", default=os.path.curdir)
+    args = arg_parser.parse_args()
+    generate(xml_file=args.xml, dest=args.dest)
+
+#  vim: set et fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :

src/lib/parameters/px_process_params.py

@@ -0,0 +1,200 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2013-2017 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.
+#
+############################################################################
+
+#
+# PX4 paramers processor (main executable file)
+#
+# This tool scans the PX4 source code for declarations of tunable parameters
+# and outputs the list in various formats.
+#
+# Currently supported formats are:
+#   * XML for the parametric UI generator
+#   * Human-readable description in DokuWiki page format
+#   * Human-readable description in Markdown page format for the PX4 dev guide
+#
+# This tool also allows to automatically upload the human-readable version
+# to the DokuWiki installation via XML-RPC.
+#
+
+from __future__ import print_function
+import sys
+import os
+import argparse
+from px4params import srcscanner, srcparser, xmlout, dokuwikiout, dokuwikirpc, markdownout
+
+import re
+import json
+import codecs
+
+def main():
+    # Parse command line arguments
+    parser = argparse.ArgumentParser(description="Process parameter documentation.")
+    parser.add_argument("-s", "--src-path",
+                        default=["../src"],
+                        metavar="PATH",
+                        nargs='*',
+                        help="one or more paths to source files to scan for parameters")
+    parser.add_argument("-x", "--xml",
+                        nargs='?',
+                        const="parameters.xml",
+                        metavar="FILENAME",
+                        help="Create XML file"
+                             " (default FILENAME: parameters.xml)")
+    parser.add_argument("-i", "--inject-xml",
+                        nargs='?',
+                        const="../Tools/parameters_injected.xml",
+                        metavar="FILENAME",
+                        help="Inject additional param XML file"
+                             " (default FILENAME: ../Tools/parameters_injected.xml)")
+    parser.add_argument("-b", "--board",
+                        nargs='?',
+                        const="",
+                        metavar="BOARD",
+                        help="Board to create xml parameter xml for")
+    parser.add_argument("-m", "--markdown",
+                        nargs='?',
+                        const="parameters.md",
+                        metavar="FILENAME",
+                        help="Create Markdown file"
+                             " (default FILENAME: parameters.md)")
+    parser.add_argument("-w", "--wiki",
+                        nargs='?',
+                        const="parameters.wiki",
+                        metavar="FILENAME",
+                        help="Create DokuWiki file"
+                             " (default FILENAME: parameters.wiki)")
+    parser.add_argument("-u", "--wiki-update",
+                        nargs='?',
+                        const="firmware:parameters",
+                        metavar="PAGENAME",
+                        help="Update DokuWiki page"
+                             " (default PAGENAME: firmware:parameters)")
+    parser.add_argument("--wiki-url",
+                        default="https://pixhawk.org",
+                        metavar="URL",
+                        help="DokuWiki URL"
+                             " (default: https://pixhawk.org)")
+    parser.add_argument("--wiki-user",
+                        default=os.environ.get('XMLRPCUSER', None),
+                        metavar="USERNAME",
+                        help="DokuWiki XML-RPC user name"
+                             " (default: $XMLRPCUSER environment variable)")
+    parser.add_argument("--wiki-pass",
+                        default=os.environ.get('XMLRPCPASS', None),
+                        metavar="PASSWORD",
+                        help="DokuWiki XML-RPC user password"
+                             " (default: $XMLRPCUSER environment variable)")
+    parser.add_argument("--wiki-summary",
+                        metavar="SUMMARY",
+                        default="Automagically updated parameter documentation from code.",
+                        help="DokuWiki page edit summary")
+    parser.add_argument('-v', '--verbose',
+                        action='store_true',
+                        help="verbose output")
+    parser.add_argument("-o", "--overrides",
+                        default="{}",
+                        metavar="OVERRIDES",
+                        help="a dict of overrides in the form of a json string")
+
+    args = parser.parse_args()
+
+    # Check for valid command
+    if not (args.xml or args.wiki or args.wiki_update or args.markdown):
+        print("Error: You need to specify at least one output method!\n")
+        parser.print_usage()
+        sys.exit(1)
+
+    # Initialize source scanner and parser
+    scanner = srcscanner.SourceScanner()
+    parser = srcparser.SourceParser()
+
+    # Scan directories, and parse the files
+    if (args.verbose):
+        print("Scanning source path " + str(args.src_path))
+
+    if not scanner.ScanDir(args.src_path, parser):
+        sys.exit(1)
+
+    if not parser.Validate():
+        sys.exit(1)
+    param_groups = parser.GetParamGroups()
+
+    if len(param_groups) == 0:
+        print("Warning: no parameters found")
+
+    override_dict = json.loads(args.overrides)
+    if len(override_dict.keys()) > 0:
+        for group in param_groups:
+            for param in group.GetParams():
+                name = param.GetName()
+                if name in override_dict.keys():
+                    val = str(override_dict[param.GetName()])
+                    param.default = val
+                    print("OVERRIDING {:s} to {:s}!!!!!".format(name, val))
+
+    # Output to XML file
+    if args.xml:
+        if args.verbose:
+            print("Creating XML file " + args.xml)
+        cur_dir = os.path.dirname(os.path.realpath(__file__))
+        out = xmlout.XMLOutput(param_groups, args.board,
+                               os.path.join(cur_dir, args.inject_xml))
+        out.Save(args.xml)
+
+    # Output to DokuWiki tables
+    if args.wiki or args.wiki_update:
+        out = dokuwikiout.DokuWikiTablesOutput(param_groups)
+        if args.wiki:
+            print("Creating wiki file " + args.wiki)
+            out.Save(args.wiki)
+        if args.wiki_update:
+            if args.wiki_user and args.wiki_pass:
+                print("Updating wiki page " + args.wiki_update)
+                xmlrpc = dokuwikirpc.get_xmlrpc(args.wiki_url, args.wiki_user, args.wiki_pass)
+                xmlrpc.wiki.putPage(args.wiki_update, out.output, {'sum': args.wiki_summary})
+            else:
+                print("Error: You need to specify DokuWiki XML-RPC username and password!")
+
+    # Output to Markdown/HTML tables
+    if args.markdown:
+        out = markdownout.MarkdownTablesOutput(param_groups)
+        if args.markdown:
+            print("Creating markdown file " + args.markdown)
+            out.Save(args.markdown)
+
+    #print("All done!")
+
+
+if __name__ == "__main__":
+    main()

src/lib/parameters/templates/px4_parameters.c.jinja

@@ -0,0 +1,38 @@
+{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
+#include "px4_parameters.h"
+
+// DO NOT EDIT
+// This file is autogenerated from paramaters.xml
+
+__BEGIN_DECLS
+
+const
+#ifndef __PX4_DARWIN
+__attribute__((used, section("__param")))
+#endif
+
+struct px4_parameters_t px4_parameters = {
+{% for param in params %}
+	{
+		"{{ param.attrib["name"] }}",
+		PARAM_TYPE_{{ param.attrib["type"] }},
+	{%- if param.attrib["volatile"] == "true" %}
+		.volatile_param = 1,
+	{%- else %}
+		.volatile_param = 0,
+	{%- endif %}
+	{%- if param.attrib["type"] == "FLOAT" %}
+		.val.f = {{ param.attrib["default"] }}
+	{%- elif param.attrib["type"] == "INT32" %}
+		.val.i = {{ param.attrib["default"] }}
+	{%- endif %}
+	},
+{% endfor %}
+	{{ params | length }}
+};
+
+//extern const struct px4_parameters_t px4_parameters;
+
+__END_DECLS
+
+{# vim: set noet ft=jinja fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : #}

src/lib/parameters/templates/px4_parameters.h.jinja

@@ -0,0 +1,21 @@
+{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
+#include <stdint.h>
+#include <parameters/param.h>
+
+// DO NOT EDIT
+// This file is autogenerated from parameters.xml
+
+__BEGIN_DECLS
+
+struct px4_parameters_t {
+{%- for param in params %}
+	const struct param_info_s __param__{{ param.attrib["name"] }};
+{%- endfor %}
+	const unsigned int param_count;
+};
+
+extern const struct px4_parameters_t px4_parameters;
+
+__END_DECLS
+
+{# vim: set noet ft=jinja fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : #}

src/lib/parameters/templates/px4_parameters_public.h.jinja

@@ -0,0 +1,37 @@
+{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
+#include <stdint.h>
+#include <parameters/param.h>
+
+// DO NOT EDIT
+// This file is autogenerated from parameters.xml
+
+
+#ifdef	__cplusplus
+
+namespace px4 { {# wrap the enum in a namespace, otherwise we get shadowing errors for MAV_TYPE #}
+
+/// Enum with all parameters
+enum class params {
+	{# enums are guaranteed to start with 0 (if the value for the first is not
+	   specified), and then incremented by 1 (the implementation depends on that!)  #}
+{%- for param in params %}
+	{{ param.attrib["name"] }},
+{%- endfor %}
+	
+	_COUNT
+};
+
+// All parameter types
+{# (px4_parameters is marked as extern, so we cannot use it as constexpr) #}
+static const constexpr int param_types_array[] =  {
+{%- for param in params %}
+	PARAM_TYPE_{{ param.attrib["type"] }}, // {{ param.attrib["name"] }}
+{%- endfor %}
+};
+
+
+} // namespace px4
+
+
+#endif /* __cplusplus */
+

src/lib/parameters/tinybson/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_library(tinybson tinybson.c)
+target_compile_options(tinybson PRIVATE -Wno-sign-compare) # TODO: fix this
+add_dependencies(tinybson prebuild_targets)

src/lib/parameters/tinybson/tinybson.c

@@ -0,0 +1,628 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 tinybson.c
+ *
+ * A simple subset SAX-style BSON parser and generator.
+ */
+
+#include <px4_posix.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <systemlib/err.h>
+
+#include "tinybson.h"
+
+#if 0
+# define debug(fmt, args...)		do { PX4_INFO("BSON: " fmt, ##args); } while(0)
+#else
+# define debug(fmt, args...)		do { } while(0)
+#endif
+
+#define CODER_CHECK(_c)		do { if (_c->dead) { debug("coder dead"); return -1; }} while(0)
+#define CODER_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0)
+
+#define BSON_READ  read
+#define BSON_WRITE write
+#define BSON_FSYNC fsync
+
+static int
+read_x(bson_decoder_t decoder, void *p, size_t s)
+{
+	CODER_CHECK(decoder);
+
+	if (decoder->fd > -1) {
+		return (BSON_READ(decoder->fd, p, s) == s) ? 0 : -1;
+	}
+
+	if (decoder->buf != NULL) {
+		/* staged operations to avoid integer overflow for corrupt data */
+		if (s >= decoder->bufsize) {
+			CODER_KILL(decoder, "buffer too small for read");
+		}
+
+		if ((decoder->bufsize - s) < decoder->bufpos) {
+			CODER_KILL(decoder, "not enough data for read");
+		}
+
+		memcpy(p, (decoder->buf + decoder->bufpos), s);
+		decoder->bufpos += s;
+		return 0;
+	}
+
+	debug("no source");
+	return -1;
+}
+
+static int
+read_int8(bson_decoder_t decoder, int8_t *b)
+{
+	return read_x(decoder, b, sizeof(*b));
+}
+
+static int
+read_int32(bson_decoder_t decoder, int32_t *i)
+{
+	return read_x(decoder, i, sizeof(*i));
+}
+
+static int
+read_int64(bson_decoder_t decoder, int64_t *i)
+{
+	return read_x(decoder, i, sizeof(*i));
+}
+
+static int
+read_double(bson_decoder_t decoder, double *d)
+{
+	return read_x(decoder, d, sizeof(*d));
+}
+
+int
+bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *priv)
+{
+	int32_t	junk;
+
+	decoder->fd = fd;
+	decoder->buf = NULL;
+	decoder->dead = false;
+	decoder->callback = callback;
+	decoder->priv = priv;
+	decoder->nesting = 1;
+	decoder->pending = 0;
+	decoder->node.type = BSON_UNDEFINED;
+
+	/* read and discard document size */
+	if (read_int32(decoder, &junk)) {
+		CODER_KILL(decoder, "failed discarding length");
+	}
+
+	/* ready for decoding */
+	return 0;
+}
+
+int
+bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback,
+		      void *priv)
+{
+	int32_t	len;
+
+	/* argument sanity */
+	if ((buf == NULL) || (callback == NULL)) {
+		return -1;
+	}
+
+	decoder->fd = -1;
+	decoder->buf = (uint8_t *)buf;
+	decoder->dead = false;
+
+	if (bufsize == 0) {
+		decoder->bufsize = *(uint32_t *)buf;
+		debug("auto-detected %u byte object", decoder->bufsize);
+
+	} else {
+		decoder->bufsize = bufsize;
+	}
+
+	decoder->bufpos = 0;
+	decoder->callback = callback;
+	decoder->priv = priv;
+	decoder->nesting = 1;
+	decoder->pending = 0;
+	decoder->node.type = BSON_UNDEFINED;
+
+	/* read and discard document size */
+	if (read_int32(decoder, &len)) {
+		CODER_KILL(decoder, "failed reading length");
+	}
+
+	if ((len > 0) && (len > (int)decoder->bufsize)) {
+		CODER_KILL(decoder, "document length larger than buffer");
+	}
+
+	/* ready for decoding */
+	return 0;
+}
+
+int
+bson_decoder_next(bson_decoder_t decoder)
+{
+	int8_t	tbyte;
+	int32_t tint;
+	unsigned nlen;
+
+	CODER_CHECK(decoder);
+
+	/* if the previous node was EOO, pop a nesting level */
+	if (decoder->node.type == BSON_EOO) {
+		if (decoder->nesting > 0) {
+			decoder->nesting--;
+		}
+
+		/* if the nesting level is now zero, the top-level document is done */
+		if (decoder->nesting == 0) {
+			/* like kill but not an error */
+			debug("nesting is zero, document is done");
+			decoder->fd = -1;
+
+			/* return end-of-file to the caller */
+			return 0;
+		}
+	}
+
+	/* if there are unread bytes pending in the stream, discard them */
+	while (decoder->pending > 0) {
+		if (read_int8(decoder, &tbyte)) {
+			CODER_KILL(decoder, "read error discarding pending bytes");
+		}
+
+		decoder->pending--;
+	}
+
+	/* get the type byte */
+	if (read_int8(decoder, &tbyte)) {
+		CODER_KILL(decoder, "read error on type byte");
+	}
+
+	decoder->node.type = tbyte;
+	decoder->pending = 0;
+
+	debug("got type byte 0x%02x", decoder->node.type);
+
+	/* EOO is special; it has no name/data following */
+	if (decoder->node.type == BSON_EOO) {
+		decoder->node.name[0] = '\0';
+
+	} else {
+
+		/* get the node name */
+		nlen = 0;
+
+		for (;;) {
+			if (nlen >= BSON_MAXNAME) {
+				CODER_KILL(decoder, "node name overflow");
+			}
+
+			if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) {
+				CODER_KILL(decoder, "read error on node name");
+			}
+
+			if (decoder->node.name[nlen] == '\0') {
+				break;
+			}
+
+			nlen++;
+		}
+
+		debug("got name '%s'", decoder->node.name);
+
+		switch (decoder->node.type) {
+		case BSON_BOOL:
+			if (read_int8(decoder, &tbyte)) {
+				CODER_KILL(decoder, "read error on BSON_BOOL");
+			}
+
+			decoder->node.b = (tbyte != 0);
+			break;
+
+		case BSON_INT32:
+			if (read_int32(decoder, &tint)) {
+				CODER_KILL(decoder, "read error on BSON_INT");
+			}
+
+			decoder->node.i = tint;
+			break;
+
+		case BSON_INT64:
+			if (read_int64(decoder, &decoder->node.i)) {
+				CODER_KILL(decoder, "read error on BSON_INT");
+			}
+
+			break;
+
+		case BSON_DOUBLE:
+			if (read_double(decoder, &decoder->node.d)) {
+				CODER_KILL(decoder, "read error on BSON_DOUBLE");
+			}
+
+			break;
+
+		case BSON_STRING:
+			if (read_int32(decoder, &decoder->pending)) {
+				CODER_KILL(decoder, "read error on BSON_STRING length");
+			}
+
+			break;
+
+		case BSON_BINDATA:
+			if (read_int32(decoder, &decoder->pending)) {
+				CODER_KILL(decoder, "read error on BSON_BINDATA size");
+			}
+
+			if (read_int8(decoder, &tbyte)) {
+				CODER_KILL(decoder, "read error on BSON_BINDATA subtype");
+			}
+
+			decoder->node.subtype = tbyte;
+			break;
+
+		/* XXX currently not supporting other types */
+		default:
+			CODER_KILL(decoder, "unsupported node type");
+		}
+	}
+
+	/* call the callback and pass its results back */
+	return decoder->callback(decoder, decoder->priv, &decoder->node);
+}
+
+int
+bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
+{
+	int result;
+
+	CODER_CHECK(decoder);
+
+	/* copy data */
+	result = read_x(decoder, buf, decoder->pending);
+
+	if (result != 0) {
+		CODER_KILL(decoder, "read error on copy_data");
+	}
+
+	/* pending count is discharged */
+	decoder->pending = 0;
+	return 0;
+}
+
+size_t
+bson_decoder_data_pending(bson_decoder_t decoder)
+{
+	return decoder->pending;
+}
+
+static int
+write_x(bson_encoder_t encoder, const void *p, size_t s)
+{
+	CODER_CHECK(encoder);
+
+	/* bson file encoder (non-buffered) */
+	if (encoder->fd > -1 && encoder->buf == NULL) {
+		return (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1;
+	}
+
+	/* do we need to extend the buffer? */
+	while ((encoder->bufpos + s) > encoder->bufsize) {
+
+		/* bson buffered file encoder */
+		if (encoder->fd > -1) {
+			// write to disk
+			debug("writing buffer (%d) to disk", encoder->bufpos);
+			int ret = BSON_WRITE(encoder->fd, encoder->buf, encoder->bufpos);
+
+			if (ret == (int)encoder->bufpos) {
+				// reset buffer to beginning and continue
+				encoder->bufpos = 0;
+
+				if ((encoder->bufpos + s) > encoder->bufsize) {
+					CODER_KILL(encoder, "fixed-size buffer overflow");
+				}
+
+				break;
+
+			} else {
+				CODER_KILL(encoder, "file write error");
+			}
+		}
+
+		if (!encoder->realloc_ok) {
+			CODER_KILL(encoder, "fixed-size buffer overflow");
+		}
+
+		uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT);
+
+		if (newbuf == NULL) {
+			CODER_KILL(encoder, "could not grow buffer");
+		}
+
+		encoder->buf = newbuf;
+		encoder->bufsize += BSON_BUF_INCREMENT;
+		debug("allocated %d bytes", BSON_BUF_INCREMENT);
+	}
+
+	memcpy(encoder->buf + encoder->bufpos, p, s);
+	encoder->bufpos += s;
+	debug("appended %d bytes", s);
+
+	return 0;
+}
+
+static int
+write_int8(bson_encoder_t encoder, int8_t b)
+{
+	return write_x(encoder, &b, sizeof(b));
+}
+
+static int
+write_int32(bson_encoder_t encoder, int32_t i)
+{
+	return write_x(encoder, &i, sizeof(i));
+}
+
+static int
+write_int64(bson_encoder_t encoder, int64_t i)
+{
+	return write_x(encoder, &i, sizeof(i));
+}
+
+static int
+write_double(bson_encoder_t encoder, double d)
+{
+	return write_x(encoder, &d, sizeof(d));
+}
+
+static int
+write_name(bson_encoder_t encoder, const char *name)
+{
+	size_t len = strlen(name);
+
+	if (len > BSON_MAXNAME) {
+		CODER_KILL(encoder, "node name too long");
+	}
+
+	return write_x(encoder, name, len + 1);
+}
+
+int
+bson_encoder_init_file(bson_encoder_t encoder, int fd)
+{
+	encoder->fd = fd;
+	encoder->buf = NULL;
+	encoder->dead = false;
+
+	if (write_int32(encoder, 0)) {
+		CODER_KILL(encoder, "write error on document length");
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_init_buf_file(bson_encoder_t encoder, int fd, void *buf, unsigned bufsize)
+{
+	encoder->fd = fd;
+	encoder->buf = (uint8_t *)buf;
+	encoder->bufpos = 0;
+	encoder->bufsize = bufsize;
+	encoder->dead = false;
+	encoder->realloc_ok = false;
+
+	if (write_int32(encoder, 0)) {
+		CODER_KILL(encoder, "write error on document length");
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize)
+{
+	encoder->fd = -1;
+	encoder->buf = (uint8_t *)buf;
+	encoder->bufpos = 0;
+	encoder->dead = false;
+
+	if (encoder->buf == NULL) {
+		encoder->bufsize = 0;
+		encoder->realloc_ok = true;
+
+	} else {
+		encoder->bufsize = bufsize;
+		encoder->realloc_ok = false;
+	}
+
+	if (write_int32(encoder, 0)) {
+		CODER_KILL(encoder, "write error on document length");
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_fini(bson_encoder_t encoder)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_EOO)) {
+		CODER_KILL(encoder, "write error on document terminator");
+	}
+
+	if (encoder->fd > -1 && encoder->buf != NULL) {
+		/* write final buffer to disk */
+		int ret = BSON_WRITE(encoder->fd, encoder->buf, encoder->bufpos);
+
+		if (ret != (int)encoder->bufpos) {
+			CODER_KILL(encoder, "write error");
+		}
+
+	} else if (encoder->buf != NULL) {
+		/* update buffer length */
+		int32_t len = bson_encoder_buf_size(encoder);
+		memcpy(encoder->buf, &len, sizeof(len));
+	}
+
+	/* sync file */
+	if (encoder->fd > -1) {
+		BSON_FSYNC(encoder->fd);
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_buf_size(bson_encoder_t encoder)
+{
+	CODER_CHECK(encoder);
+
+	if (encoder->fd > -1) {
+		return -1;
+	}
+
+	return encoder->bufpos;
+}
+
+void *
+bson_encoder_buf_data(bson_encoder_t encoder)
+{
+	/* note, no CODER_CHECK here as the caller has to clean up dead buffers */
+
+	if (encoder->fd > -1) {
+		return NULL;
+	}
+
+	return encoder->buf;
+}
+
+int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_BOOL) ||
+	    write_name(encoder, name) ||
+	    write_int8(encoder, value ? 1 : 0)) {
+		CODER_KILL(encoder, "write error on BSON_BOOL");
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value)
+{
+	bool result;
+
+	CODER_CHECK(encoder);
+
+	/* use the smallest encoding that will hold the value */
+	if (value == (int32_t)value) {
+		debug("encoding %lld as int32", value);
+		result = write_int8(encoder, BSON_INT32) ||
+			 write_name(encoder, name) ||
+			 write_int32(encoder, value);
+
+	} else {
+		debug("encoding %lld as int64", value);
+		result = write_int8(encoder, BSON_INT64) ||
+			 write_name(encoder, name) ||
+			 write_int64(encoder, value);
+	}
+
+	if (result) {
+		CODER_KILL(encoder, "write error on BSON_INT");
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_DOUBLE) ||
+	    write_name(encoder, name) ||
+	    write_double(encoder, value)) {
+		CODER_KILL(encoder, "write error on BSON_DOUBLE");
+	}
+
+
+	return 0;
+}
+
+int
+bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string)
+{
+	size_t len;
+
+	CODER_CHECK(encoder);
+
+	len = strlen(string) + 1;	/* include trailing nul */
+
+	if (write_int8(encoder, BSON_STRING) ||
+	    write_name(encoder, name) ||
+	    write_int32(encoder, len) ||
+	    write_x(encoder, string, len)) {
+		CODER_KILL(encoder, "write error on BSON_STRING");
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size,
+			   const void *data)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_BINDATA) ||
+	    write_name(encoder, name) ||
+	    write_int32(encoder, size) ||
+	    write_int8(encoder, subtype) ||
+	    write_x(encoder, data, size)) {
+		CODER_KILL(encoder, "write error on BSON_BINDATA");
+	}
+
+	return 0;
+}

src/lib/parameters/tinybson/tinybson.h

@@ -0,0 +1,288 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 tinybson.h
+*
+* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
+*
+* Some types and defines taken from the standalone BSON parser/generator
+* in the Mongo C connector.
+*/
+
+#ifndef _TINYBSON_H
+#define _TINYBSON_H
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+/** subset of the BSON node types we might care about */
+typedef enum {
+	BSON_EOO = 0,
+	BSON_DOUBLE = 1,
+	BSON_STRING = 2,
+	BSON_OBJECT = 3,
+	BSON_ARRAY = 4,
+	BSON_BINDATA = 5,
+	BSON_UNDEFINED = 6,
+	BSON_BOOL = 8,
+	BSON_DATE = 9,
+	BSON_NULL = 10,
+	BSON_INT32 = 16,
+	BSON_INT64 = 18
+} bson_type_t;
+
+typedef enum bson_binary_subtype {
+	BSON_BIN_BINARY = 0,
+	BSON_BIN_USER = 128
+} bson_binary_subtype_t;
+
+/**
+ * Maximum node name length.
+ */
+#define BSON_MAXNAME		32
+
+/**
+ * Buffer growth increment when writing to a buffer.
+ */
+#define BSON_BUF_INCREMENT	128
+
+/**
+ * Node structure passed to the callback.
+ */
+typedef struct bson_node_s {
+	char			name[BSON_MAXNAME];
+	bson_type_t		type;
+	bson_binary_subtype_t	subtype;
+	union {
+		int64_t		i;
+		double		d;
+		bool		b;
+	};
+} *bson_node_t;
+
+typedef struct bson_decoder_s *bson_decoder_t;
+
+/**
+ * Node callback.
+ *
+ * The node callback function's return value is returned by bson_decoder_next.
+ */
+typedef int	(* bson_decoder_callback)(bson_decoder_t decoder, void *priv, bson_node_t node);
+
+struct bson_decoder_s {
+	/* file reader state */
+	int			fd;
+
+	/* buffer reader state */
+	uint8_t			*buf;
+	size_t			bufsize;
+	unsigned		bufpos;
+
+	bool			dead;
+	bson_decoder_callback	callback;
+	void			*priv;
+	unsigned		nesting;
+	struct bson_node_s	node;
+	int32_t			pending;
+};
+
+/**
+ * Initialise the decoder to read from a file.
+ *
+ * @param decoder		Decoder state structure to be initialised.
+ * @param fd			File to read BSON data from.
+ * @param callback		Callback to be invoked by bson_decoder_next
+ * @param priv          Callback private data, stored in node.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *priv);
+
+/**
+ * Initialise the decoder to read from a buffer in memory.
+ *
+ * @param decoder		Decoder state structure to be initialised.
+ * @param buf			Buffer to read from.
+ * @param bufsize		Size of the buffer (BSON object may be smaller).  May be
+ *				passed as zero if the buffer size should be extracted from the
+ *				BSON header only.
+ * @param callback		Callback to be invoked by bson_decoder_next
+ * @param priv		Callback private data, stored in node.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback,
+				   void *priv);
+
+/**
+ * Process the next node from the stream and invoke the callback.
+ *
+ * @param decoder		Decoder state, must have been initialised with bson_decoder_init.
+ * @return			-1 if parsing encountered an error, 0 if the BSON stream has ended,
+ *				otherwise the return value from the callback.
+ */
+__EXPORT int bson_decoder_next(bson_decoder_t decoder);
+
+/**
+ * Copy node data.
+ *
+ * @param decoder		Decoder state, must have been initialised with bson_decoder_init.
+ */
+__EXPORT int bson_decoder_copy_data(bson_decoder_t decoder, void *buf);
+
+/**
+ * Report copyable data size.
+ *
+ * @param decoder		Decoder state, must have been initialised with bson_decoder_init.
+ */
+__EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder);
+
+/**
+ * Encoder state structure.
+ */
+typedef struct bson_encoder_s {
+	/* file writer state */
+	int		fd;
+
+	/* buffer writer state */
+	uint8_t		*buf;
+	unsigned	bufsize;
+	unsigned	bufpos;
+
+	bool		realloc_ok;
+	bool		dead;
+
+} *bson_encoder_t;
+
+/**
+ * Initialze the encoder for writing to a file.
+ *
+ * @param encoder		Encoder state structure to be initialised.
+ * @param fd			File to write to.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd);
+
+/**
+ * Initialze the encoder for writing to a file.
+ *
+ * @param encoder		Encoder state structure to be initialised.
+ * @param fd			File to write to.
+ * @param buf			Buffer pointer to use, can't be NULL
+ * @param bufsize		Supplied buffer size
+ * @return			Zero on success.
+ */
+__EXPORT int bson_encoder_init_buf_file(bson_encoder_t encoder, int fd, void *buf, unsigned bufsize);
+
+/**
+ * Initialze the encoder for writing to a buffer.
+ *
+ * @param encoder		Encoder state structure to be initialised.
+ * @param buf			Buffer pointer to use, or NULL if the buffer
+ *				should be allocated by the encoder.
+ * @param bufsize		Maximum buffer size, or zero for no limit. If
+ *				the buffer is supplied, the size of the supplied buffer.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize);
+
+/**
+ * Finalise the encoded stream.
+ *
+ * @param encoder		The encoder to finalise.
+ */
+__EXPORT int bson_encoder_fini(bson_encoder_t encoder);
+
+/**
+ * Fetch the size of the encoded object; only valid for buffer operations.
+ */
+__EXPORT int bson_encoder_buf_size(bson_encoder_t encoder);
+
+/**
+ * Get a pointer to the encoded object buffer.
+ *
+ * Note that if the buffer was allocated by the encoder, it is the caller's responsibility
+ * to free this buffer.
+ */
+__EXPORT void *bson_encoder_buf_data(bson_encoder_t encoder);
+
+/**
+ * Append a boolean to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded.
+ */
+__EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value);
+
+/**
+ * Append an integer to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded.
+ */
+__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value);
+
+/**
+ * Append a double to the encoded stream
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded.
+ */
+__EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value);
+
+/**
+ * Append a string to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param string		Nul-terminated C string.
+ */
+__EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string);
+
+/**
+ * Append a binary blob to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param subtype		Binary data subtype.
+ * @param size			Data size.
+ * @param data			Buffer containing data to be encoded.
+ */
+__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype,
+					size_t size, const void *data);
+
+
+#endif

src/lib/perf/CMakeLists.txt

@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 2017 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.
+#
+############################################################################
+
+add_library(perf perf_counter.c)
+add_dependencies(perf prebuild_targets)

src/lib/perf/perf_counter.c

@@ -0,0 +1,634 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 perf_counter.c
+ *
+ * @brief Performance measuring tools.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/queue.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+#include <pthread.h>
+#include <systemlib/err.h>
+
+#include "perf_counter.h"
+
+
+#ifdef __PX4_QURT
+// There is presumably no dprintf on QURT. Therefore use the usual output to mini-dm.
+#define dprintf(_fd, _text, ...) ((_fd) == 1 ? PX4_INFO((_text), ##__VA_ARGS__) : (void)(_fd))
+#endif
+
+/**
+ * Header common to all counters.
+ */
+struct perf_ctr_header {
+	sq_entry_t		link;	/**< list linkage */
+	enum perf_counter_type	type;	/**< counter type */
+	const char		*name;	/**< counter name */
+};
+
+/**
+ * PC_EVENT counter.
+ */
+struct perf_ctr_count {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+};
+
+/**
+ * PC_ELAPSED counter.
+ */
+struct perf_ctr_elapsed {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_start;
+	uint64_t		time_total;
+	uint32_t		time_least;
+	uint32_t		time_most;
+	float			mean;
+	float			M2;
+};
+
+/**
+ * PC_INTERVAL counter.
+ */
+struct perf_ctr_interval {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_event;
+	uint64_t		time_first;
+	uint64_t		time_last;
+	uint32_t		time_least;
+	uint32_t		time_most;
+	float			mean;
+	float			M2;
+};
+
+/**
+ * List of all known counters.
+ */
+static sq_queue_t	perf_counters = { NULL, NULL };
+
+/**
+ * mutex protecting access to the perf_counters linked list (which is read from & written to by different threads)
+ */
+pthread_mutex_t perf_counters_mutex = PTHREAD_MUTEX_INITIALIZER;
+// FIXME: the mutex does **not** protect against access to/from the perf
+// counter's data. It can still happen that a counter is updated while it is
+// printed. This can lead to inconsistent output, or completely bogus values
+// (especially the 64bit values which are in general not atomically updated).
+// The same holds for shared perf counters (perf_alloc_once), that can be updated
+// concurrently (this affects the 'ctrl_latency' counter).
+
+
+perf_counter_t
+perf_alloc(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t ctr = NULL;
+
+	switch (type) {
+	case PC_COUNT:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1);
+		break;
+
+	case PC_ELAPSED:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1);
+		break;
+
+	case PC_INTERVAL:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+
+		break;
+
+	default:
+		break;
+	}
+
+	if (ctr != NULL) {
+		ctr->type = type;
+		ctr->name = name;
+		pthread_mutex_lock(&perf_counters_mutex);
+		sq_addfirst(&ctr->link, &perf_counters);
+		pthread_mutex_unlock(&perf_counters_mutex);
+	}
+
+	return ctr;
+}
+
+perf_counter_t
+perf_alloc_once(enum perf_counter_type type, const char *name)
+{
+	pthread_mutex_lock(&perf_counters_mutex);
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		if (!strcmp(handle->name, name)) {
+			if (type == handle->type) {
+				/* they are the same counter */
+				pthread_mutex_unlock(&perf_counters_mutex);
+				return handle;
+
+			} else {
+				/* same name but different type, assuming this is an error and not intended */
+				pthread_mutex_unlock(&perf_counters_mutex);
+				return NULL;
+			}
+		}
+
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	pthread_mutex_unlock(&perf_counters_mutex);
+
+	/* if the execution reaches here, no existing counter of that name was found */
+	return perf_alloc(type, name);
+}
+
+void
+perf_free(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	pthread_mutex_lock(&perf_counters_mutex);
+	sq_rem(&handle->link, &perf_counters);
+	pthread_mutex_unlock(&perf_counters_mutex);
+	free(handle);
+}
+
+void
+perf_count(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count++;
+		break;
+
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+			hrt_abstime now = hrt_absolute_time();
+
+			switch (pci->event_count) {
+			case 0:
+				pci->time_first = now;
+				break;
+
+			case 1:
+				pci->time_least = (uint32_t)(now - pci->time_last);
+				pci->time_most = (uint32_t)(now - pci->time_last);
+				pci->mean = pci->time_least / 1e6f;
+				pci->M2 = 0;
+				break;
+
+			default: {
+					hrt_abstime interval = now - pci->time_last;
+
+					if ((uint32_t)interval < pci->time_least) {
+						pci->time_least = (uint32_t)interval;
+					}
+
+					if ((uint32_t)interval > pci->time_most) {
+						pci->time_most = (uint32_t)interval;
+					}
+
+					// maintain mean and variance of interval in seconds
+					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+					float dt = interval / 1e6f;
+					float delta_intvl = dt - pci->mean;
+					pci->mean += delta_intvl / pci->event_count;
+					pci->M2 += delta_intvl * (dt - pci->mean);
+					break;
+				}
+			}
+
+			pci->time_last = now;
+			pci->event_count++;
+			break;
+		}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_begin(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED:
+		((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time();
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_end(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			if (pce->time_start != 0) {
+				int64_t elapsed = hrt_absolute_time() - pce->time_start;
+
+				if (elapsed >= 0) {
+
+					pce->event_count++;
+					pce->time_total += elapsed;
+
+					if ((pce->time_least > (uint32_t)elapsed) || (pce->time_least == 0)) {
+						pce->time_least = elapsed;
+					}
+
+					if (pce->time_most < (uint32_t)elapsed) {
+						pce->time_most = elapsed;
+					}
+
+					// maintain mean and variance of the elapsed time in seconds
+					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+					float dt = elapsed / 1e6f;
+					float delta_intvl = dt - pce->mean;
+					pce->mean += delta_intvl / pce->event_count;
+					pce->M2 += delta_intvl * (dt - pce->mean);
+
+					pce->time_start = 0;
+				}
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_set_elapsed(perf_counter_t handle, int64_t elapsed)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			if (elapsed >= 0) {
+
+				pce->event_count++;
+				pce->time_total += elapsed;
+
+				if ((pce->time_least > (uint32_t)elapsed) || (pce->time_least == 0)) {
+					pce->time_least = elapsed;
+				}
+
+				if (pce->time_most < (uint32_t)elapsed) {
+					pce->time_most = elapsed;
+				}
+
+				// maintain mean and variance of the elapsed time in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = elapsed / 1e6f;
+				float delta_intvl = dt - pce->mean;
+				pce->mean += delta_intvl / pce->event_count;
+				pce->M2 += delta_intvl * (dt - pce->mean);
+
+				pce->time_start = 0;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_set_count(perf_counter_t handle, uint64_t count)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_COUNT: {
+			((struct perf_ctr_count *)handle)->event_count = count;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+}
+
+void
+perf_cancel(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			pce->time_start = 0;
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+
+void
+perf_reset(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count = 0;
+		break;
+
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			pce->event_count = 0;
+			pce->time_start = 0;
+			pce->time_total = 0;
+			pce->time_least = 0;
+			pce->time_most = 0;
+			break;
+		}
+
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+			pci->event_count = 0;
+			pci->time_event = 0;
+			pci->time_first = 0;
+			pci->time_last = 0;
+			pci->time_least = 0;
+			pci->time_most = 0;
+			break;
+		}
+	}
+}
+
+void
+perf_print_counter(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	perf_print_counter_fd(1, handle);
+}
+
+void
+perf_print_counter_fd(int fd, perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_COUNT:
+		dprintf(fd, "%s: %llu events\n",
+			handle->name,
+			(unsigned long long)((struct perf_ctr_count *)handle)->event_count);
+		break;
+
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			float rms = sqrtf(pce->M2 / (pce->event_count - 1));
+			dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+				handle->name,
+				(unsigned long long)pce->event_count,
+				(unsigned long long)pce->time_total,
+				(pce->event_count == 0) ? 0 : (unsigned long long)pce->time_total / pce->event_count,
+				(unsigned long long)pce->time_least,
+				(unsigned long long)pce->time_most,
+				(double)(1e6f * rms));
+			break;
+		}
+
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+			float rms = sqrtf(pci->M2 / (pci->event_count - 1));
+
+			dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+				handle->name,
+				(unsigned long long)pci->event_count,
+				(pci->event_count == 0) ? 0 : (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count,
+				(unsigned long long)pci->time_least,
+				(unsigned long long)pci->time_most,
+				(double)(1e6f * rms));
+			break;
+		}
+
+	default:
+		break;
+	}
+}
+
+
+int
+perf_print_counter_buffer(char *buffer, int length, perf_counter_t handle)
+{
+	int num_written = 0;
+
+	if (handle == NULL) {
+		return 0;
+	}
+
+	switch (handle->type) {
+	case PC_COUNT:
+		num_written = snprintf(buffer, length, "%s: %llu events",
+				       handle->name,
+				       (unsigned long long)((struct perf_ctr_count *)handle)->event_count);
+		break;
+
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			float rms = sqrtf(pce->M2 / (pce->event_count - 1));
+			num_written = snprintf(buffer, length, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms",
+					       handle->name,
+					       (unsigned long long)pce->event_count,
+					       (unsigned long long)pce->time_total,
+					       (pce->event_count == 0) ? 0 : (unsigned long long)pce->time_total / pce->event_count,
+					       (unsigned long long)pce->time_least,
+					       (unsigned long long)pce->time_most,
+					       (double)(1e6f * rms));
+			break;
+		}
+
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+			float rms = sqrtf(pci->M2 / (pci->event_count - 1));
+
+			num_written = snprintf(buffer, length, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms",
+					       handle->name,
+					       (unsigned long long)pci->event_count,
+					       (pci->event_count == 0) ? 0 : (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count,
+					       (unsigned long long)pci->time_least,
+					       (unsigned long long)pci->time_most,
+					       (double)(1e6f * rms));
+			break;
+		}
+
+	default:
+		break;
+	}
+
+	buffer[length - 1] = 0; // ensure 0-termination
+	return num_written;
+}
+
+uint64_t
+perf_event_count(perf_counter_t handle)
+{
+	if (handle == NULL) {
+		return 0;
+	}
+
+	switch (handle->type) {
+	case PC_COUNT:
+		return ((struct perf_ctr_count *)handle)->event_count;
+
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			return pce->event_count;
+		}
+
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+			return pci->event_count;
+		}
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+void
+perf_iterate_all(perf_callback cb, void *user)
+{
+	pthread_mutex_lock(&perf_counters_mutex);
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		cb(handle, user);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	pthread_mutex_unlock(&perf_counters_mutex);
+}
+
+void
+perf_print_all(int fd)
+{
+	pthread_mutex_lock(&perf_counters_mutex);
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_print_counter_fd(fd, handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	pthread_mutex_unlock(&perf_counters_mutex);
+}
+
+// these are defined in drv_hrt.c
+extern const uint16_t latency_bucket_count;
+extern uint32_t latency_counters[];
+extern const uint16_t latency_buckets[];
+
+void
+perf_print_latency(int fd)
+{
+	dprintf(fd, "bucket [us] : events\n");
+
+	for (int i = 0; i < latency_bucket_count; i++) {
+		printf("       %4i : %li\n", latency_buckets[i], (long int)latency_counters[i]);
+	}
+
+	// print the overflow bucket value
+	dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count - 1], latency_counters[latency_bucket_count]);
+}
+
+void
+perf_reset_all(void)
+{
+	pthread_mutex_lock(&perf_counters_mutex);
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_reset(handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	pthread_mutex_unlock(&perf_counters_mutex);
+
+	for (int i = 0; i <= latency_bucket_count; i++) {
+		latency_counters[i] = 0;
+	}
+}

src/lib/perf/perf_counter.h

@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-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 perf_counter.h
+ * Performance measuring tools.
+ */
+
+#ifndef _SYSTEMLIB_PERF_COUNTER_H
+#define _SYSTEMLIB_PERF_COUNTER_H value
+
+#include <stdint.h>
+#include <px4_defines.h>
+
+/**
+ * Counter types.
+ */
+enum perf_counter_type {
+	PC_COUNT,		/**< count the number of times an event occurs */
+	PC_ELAPSED,		/**< measure the time elapsed performing an event */
+	PC_INTERVAL		/**< measure the interval between instances of an event */
+};
+
+struct perf_ctr_header;
+typedef struct perf_ctr_header	*perf_counter_t;
+
+__BEGIN_DECLS
+
+/**
+ * Create a new local counter.
+ *
+ * @param type			The type of the new counter.
+ * @param name			The counter name.
+ * @return			Handle for the new counter, or NULL if a counter
+ *				could not be allocated.
+ */
+#ifndef perf_alloc	// perf_alloc might be defined to be NULL in src/modules/px4iofirmware/px4io.h
+__EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
+#endif
+
+/**
+ * Get the reference to an existing counter or create a new one if it does not exist.
+ *
+ * @param type			The type of the counter.
+ * @param name			The counter name.
+ * @return			Handle for the counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc_once(enum perf_counter_type type, const char *name);
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+__EXPORT extern void		perf_free(perf_counter_t handle);
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_count(perf_counter_t handle);
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_begin(perf_counter_t handle);
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresponding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_end(perf_counter_t handle);
+
+/**
+ * Register a measurement
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresponding perf_begin call. It sets the
+ * value provided as argument as a new measurement.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param elapsed		The time elapsed. Negative values lead to incrementing the overrun counter.
+ */
+__EXPORT extern void		perf_set_elapsed(perf_counter_t handle, int64_t elapsed);
+
+/**
+ * Set a counter
+ *
+ * This call applies to counters of type PC_COUNT. It (re-)sets the count.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param count			The counter value to be set.
+ */
+__EXPORT extern void		perf_set_count(perf_counter_t handle, uint64_t count);
+
+/**
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_cancel(perf_counter_t handle);
+
+/**
+ * Reset a performance counter.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_reset(perf_counter_t handle);
+
+/**
+ * Print one performance counter to stdout
+ *
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter(perf_counter_t handle);
+
+/**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
+
+/**
+ * Print one performance counter to a buffer.
+ *
+ * @param buffer			buffer to write to
+ * @param length			buffer length
+ * @param handle			The counter to print.
+ * @param return			number of bytes written
+ */
+__EXPORT extern int		perf_print_counter_buffer(char *buffer, int length, perf_counter_t handle);
+
+/**
+ * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+__EXPORT extern void		perf_print_all(int fd);
+
+
+typedef void (*perf_callback)(perf_counter_t handle, void *user);
+
+/**
+ * Iterate over all performance counters using a callback.
+ *
+ * Caution: This will aquire the mutex, so do not call any other perf_* method
+ * that aquire the mutex as well from the callback (If this is needed, configure
+ * the mutex to be reentrant).
+ *
+ * @param cb callback method
+ * @param user custom argument for the callback
+ */
+__EXPORT extern void	perf_iterate_all(perf_callback cb, void *user);
+
+/**
+ * Print hrt latency counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+__EXPORT extern void		perf_print_latency(int fd);
+
+/**
+ * Reset all of the performance counters.
+ */
+__EXPORT extern void		perf_reset_all(void);
+
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+__EXPORT extern uint64_t	perf_event_count(perf_counter_t handle);
+
+__END_DECLS
+
+#endif

src/lib/rc/CMakeLists.txt

@@ -30,17 +30,13 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__rc
-	COMPILE_FLAGS
-		-Wno-unused-result
-	SRCS
-		st24.cpp
-		sumd.cpp
-		sbus.cpp
-		dsm.cpp
-		common_rc.cpp
-	DEPENDS
-		platforms__common
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+
+add_library(rc
+	st24.cpp
+	sumd.cpp
+	sbus.cpp
+	dsm.cpp
+	common_rc.cpp
+)
+target_compile_options(rc PRIVATE -Wno-unused-result)
+target_link_libraries(rc PRIVATE prebuild_targets)

src/lib/rc/rc_tests/CMakeLists.txt

@@ -30,6 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
 px4_add_module(
 	MODULE lib__rc__rc_tests
 	MAIN rc_tests
@@ -38,7 +39,5 @@ px4_add_module(
 	SRCS
 		RCTest.cpp
 	DEPENDS
-		platforms__common
+		rc
 	)
-
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/lib/terrain_estimation/CMakeLists.txt

@@ -30,13 +30,5 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__terrain_estimation
-	STACK_MAIN 1024
-	COMPILE_FLAGS
-	SRCS
-		terrain_estimator.cpp
-	DEPENDS
-		platforms__common
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+
+px4_add_library(terrain_estimation terrain_estimator.cpp)

src/lib/tunes/CMakeLists.txt

@@ -30,13 +30,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE lib__tunes
-	SRCS
-		tunes.cpp
-		default_tunes.cpp
-	DEPENDS
-		platforms__common
-		modules__uORB
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+
+px4_add_library(tunes
+	tunes.cpp
+	default_tunes.cpp
+)

src/lib/version/CMakeLists.txt

@@ -64,13 +64,13 @@ add_custom_command(OUTPUT ${px4_git_ver_header}
 set_source_files_properties(${px4_git_ver_header} PROPERTIES GENERATED TRUE)
 add_custom_target(ver_gen ALL DEPENDS ${px4_git_ver_header})
 
-px4_add_module(
-	MODULE lib__version
-	COMPILE_FLAGS
-	SRCS
-		version.c
-	DEPENDS
-		ver_gen
-		platforms__common
-	)
+# The URL for the elf file for crash logging
+if (DEFINED ENV{BUILD_URI})
+	set(BUILD_URI $ENV{BUILD_URI})
+else()
+	set(BUILD_URI "localhost")
+endif()
 
+add_definitions(-DBUILD_URI=${BUILD_URI})
+
+px4_add_library(version version.c ${px4_git_ver_header})