Initial import

CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2015 ECL 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 ECL 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_module(
+	MODULE lib__ecl
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		attitude_fw/ecl_controller.cpp
+		attitude_fw/ecl_pitch_controller.cpp
+		attitude_fw/ecl_roll_controller.cpp
+		attitude_fw/ecl_yaw_controller.cpp
+		l1/ecl_l1_pos_controller.cpp
+		validation/data_validator.cpp
+		validation/data_validator_group.cpp
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

attitude_fw/ecl_controller.cpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.cpp
+ * Definition of base class for other controllers
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#include "ecl_controller.h"
+
+#include <stdio.h>
+#include <mathlib/mathlib.h>
+
+ECL_Controller::ECL_Controller(const char *name) :
+	_last_run(0),
+	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f),
+	_perf_name()
+{
+	/* Init performance counter */
+	snprintf(_perf_name, sizeof(_perf_name), "fw att control %s nonfinite input", name);
+	_nonfinite_input_perf = perf_alloc(PC_COUNT, _perf_name);
+}
+
+ECL_Controller::~ECL_Controller()
+{
+	perf_free(_nonfinite_input_perf);
+}
+
+void ECL_Controller::reset_integrator()
+{
+	_integrator = 0.0f;
+}
+
+void ECL_Controller::set_time_constant(float time_constant)
+{
+	if (time_constant > 0.1f && time_constant < 3.0f) {
+		_tc = time_constant;
+	}
+}
+
+void ECL_Controller::set_k_p(float k_p)
+{
+	_k_p = k_p;
+}
+
+void ECL_Controller::set_k_i(float k_i)
+{
+	_k_i = k_i;
+}
+
+void ECL_Controller::set_k_ff(float k_ff)
+{
+	_k_ff = k_ff;
+}
+
+void ECL_Controller::set_integrator_max(float max)
+{
+	_integrator_max = max;
+}
+
+void ECL_Controller::set_max_rate(float max_rate)
+{
+	_max_rate = max_rate;
+}
+
+float ECL_Controller::get_rate_error()
+{
+	return _rate_error;
+}
+
+float ECL_Controller::get_desired_rate()
+{
+	return _rate_setpoint;
+}
+
+float ECL_Controller::get_desired_bodyrate()
+{
+	return _bodyrate_setpoint;
+}
+
+float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) {
+	float airspeed_result = airspeed;
+	if (!PX4_ISFINITE(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed_result = 0.5f * (minspeed + maxspeed);
+	} else if (airspeed < minspeed) {
+		airspeed_result = minspeed;
+	}
+	return airspeed_result;
+}

attitude_fw/ecl_controller.h

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.h
+ * Definition of base class for other controllers
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <systemlib/perf_counter.h>
+
+struct ECL_ControlData {
+	float roll;
+	float pitch;
+	float yaw;
+	float roll_rate;
+	float pitch_rate;
+	float yaw_rate;
+	float speed_body_u;
+	float speed_body_v;
+	float speed_body_w;
+	float acc_body_x;
+	float acc_body_y;
+	float acc_body_z;
+	float roll_setpoint;
+	float pitch_setpoint;
+	float yaw_setpoint;
+	float roll_rate_setpoint;
+	float pitch_rate_setpoint;
+	float yaw_rate_setpoint;
+	float airspeed_min;
+	float airspeed_max;
+	float airspeed;
+	float scaler;
+	bool lock_integrator;
+};
+
+class __EXPORT ECL_Controller
+{
+public:
+	ECL_Controller(const char *name);
+
+	~ECL_Controller();
+
+	virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
+	virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0;
+
+	/* Setters */
+	void set_time_constant(float time_constant);
+	void set_k_p(float k_p);
+	void set_k_i(float k_i);
+	void set_k_ff(float k_ff);
+	void set_integrator_max(float max);
+	void set_max_rate(float max_rate);
+
+	/* Getters */
+	float get_rate_error();
+	float get_desired_rate();
+	float get_desired_bodyrate();
+
+	void reset_integrator();
+
+protected:
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_ff;
+	float _integrator_max;
+	float _max_rate;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _bodyrate_setpoint;
+	perf_counter_t _nonfinite_input_perf;
+	static const uint8_t _perf_name_max = 40;
+	char _perf_name[_perf_name_max];
+	float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
+};

attitude_fw/ecl_pitch_controller.cpp

@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp
+ * Implementation of a simple orthogonal pitch PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_pitch_controller.h"
+#include <math.h>
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+
+ECL_PitchController::ECL_PitchController() :
+	ECL_Controller("pitch"),
+	_max_rate_neg(0.0f),
+	_roll_ff(0.0f)
+{
+}
+
+ECL_PitchController::~ECL_PitchController()
+{
+}
+
+float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
+{
+
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
+	      PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.airspeed))) {
+		perf_count(_nonfinite_input_perf);
+		warnx("not controlling pitch");
+		return _rate_setpoint;
+	}
+
+	/* Calculate the error */
+	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
+
+	/*  Apply P controller: rate setpoint from current error and time constant */
+	_rate_setpoint =  pitch_error / _tc;
+
+	/* limit the rate */
+	if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
+		if (_rate_setpoint > 0.0f) {
+			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+
+		} else {
+			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
+		}
+
+	}
+
+	return _rate_setpoint;
+}
+
+float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.pitch_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) &&
+	      PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
+
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	bool lock_integrator = ctl_data.lock_integrator;
+
+	if (dt_micros > 500000) {
+		lock_integrator = true;
+	}
+
+	/* Transform setpoint to body angular rates (jacobian) */
+	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
+			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
+
+	/* apply turning offset to desired bodyrate setpoint*/
+	/* flying inverted (wings upside down)*/
+	bool inverted = false;
+	float constrained_roll;
+	/* roll is used as feedforward term and inverted flight needs to be considered */
+	if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
+		/* not inverted, but numerically still potentially close to infinity */
+		constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
+
+	} else {
+		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
+		inverted = true;
+		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
+		if (ctl_data.roll > 0.0f) {
+			/* right hemisphere */
+			constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
+
+		} else {
+			/* left hemisphere */
+			constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
+		}
+	}
+
+	/* input conditioning */
+	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
+
+	/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
+	   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
+	   Availible on google books 8/11/2015: 
+	   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
+	float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
+				  		tanf(constrained_roll) * sinf(constrained_roll)));
+
+	if (inverted) {
+		body_fixed_turn_offset = -body_fixed_turn_offset;
+	}
+
+	/* Finally add the turn offset to your bodyrate setpoint*/
+	_bodyrate_setpoint += body_fixed_turn_offset;
+
+
+	_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
+
+	if (!lock_integrator && _k_i > 0.0f) {
+
+		float id = _rate_error * dt * ctl_data.scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
+		 */
+		if (_last_output < -1.0f) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+
+		} else if (_last_output > 1.0f) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
+		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+		       + integrator_constrained;  //scaler is proportional to 1/airspeed
+//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+//	warnx("roll: _last_output %.4f", (double)_last_output);
+	return math::constrain(_last_output, -1.0f, 1.0f);
+}

attitude_fw/ecl_pitch_controller.h

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h
+ * Definition of a simple orthogonal pitch PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#ifndef ECL_PITCH_CONTROLLER_H
+#define ECL_PITCH_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "ecl_controller.h"
+
+class __EXPORT ECL_PitchController :
+	public ECL_Controller
+{
+public:
+	ECL_PitchController();
+
+	~ECL_PitchController();
+
+	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_bodyrate(const struct ECL_ControlData &ctl_data);
+
+	/* Additional Setters */
+	void set_max_rate_pos(float max_rate_pos)
+	{
+		_max_rate = max_rate_pos;
+	}
+
+	void set_max_rate_neg(float max_rate_neg)
+	{
+		_max_rate_neg = max_rate_neg;
+	}
+
+	void set_roll_ff(float roll_ff)
+	{
+		_roll_ff = roll_ff;
+	}
+
+protected:
+	float _max_rate_neg;
+	float _roll_ff;
+};
+
+#endif // ECL_PITCH_CONTROLLER_H

attitude_fw/ecl_roll_controller.cpp

@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp
+ * Implementation of a simple orthogonal roll PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include <ecl/ecl.h>
+#include "ecl_roll_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+
+ECL_RollController::ECL_RollController() :
+	ECL_Controller("roll")
+{
+}
+
+ECL_RollController::~ECL_RollController()
+{
+}
+
+float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) {
+		perf_count(_nonfinite_input_perf);
+		return _rate_setpoint;
+	}
+
+	/* Calculate error */
+	float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
+
+	/* Apply P controller */
+	_rate_setpoint = roll_error / _tc;
+
+	/* limit the rate */ //XXX: move to body angluar rates
+
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
+	return _rate_setpoint;
+}
+
+float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.roll_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) &&
+	      PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
+
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	bool lock_integrator = ctl_data.lock_integrator;
+
+	if (dt_micros > 500000) {
+		lock_integrator = true;
+	}
+
+	/* Transform setpoint to body angular rates (jacobian) */
+	_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
+
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error
+
+	if (!lock_integrator && _k_i > 0.0f) {
+
+		float id = _rate_error * dt * ctl_data.scaler;
+
+		/*
+		* anti-windup: do not allow integrator to increase if actuator is at limit
+		*/
+		if (_last_output < -1.0f) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+
+		} else if (_last_output > 1.0f) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
+		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+		       + integrator_constrained;  //scaler is proportional to 1/airspeed
+
+	return math::constrain(_last_output, -1.0f, 1.0f);
+}
+

attitude_fw/ecl_roll_controller.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.h
+ * Definition of a simple orthogonal roll PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#ifndef ECL_ROLL_CONTROLLER_H
+#define ECL_ROLL_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "ecl_controller.h"
+
+class __EXPORT ECL_RollController :
+	public ECL_Controller
+{
+public:
+	ECL_RollController();
+
+	~ECL_RollController();
+
+	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_bodyrate(const struct ECL_ControlData &ctl_data);
+};
+
+#endif // ECL_ROLL_CONTROLLER_H

attitude_fw/ecl_yaw_controller.cpp

@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp
+ * Implementation of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_yaw_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <ecl/ecl.h>
+
+ECL_YawController::ECL_YawController() :
+	ECL_Controller("yaw"),
+	_coordinated_min_speed(1.0f),
+	_coordinated_method(0)
+{
+}
+
+ECL_YawController::~ECL_YawController()
+{
+}
+
+float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
+{
+	switch (_coordinated_method) {
+	case COORD_METHOD_OPEN:
+		return control_attitude_impl_openloop(ctl_data);
+
+	case COORD_METHOD_CLOSEACC:
+		return control_attitude_impl_accclosedloop(ctl_data);
+
+	default:
+		static hrt_abstime last_print = 0;
+
+		if (ecl_elapsed_time(&last_print) > 5e6) {
+			warnx("invalid param setting FW_YCO_METHOD");
+			last_print = ecl_absolute_time();
+		}
+	}
+
+	return _rate_setpoint;
+}
+
+float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+{
+	switch (_coordinated_method) {
+	case COORD_METHOD_OPEN:
+	case COORD_METHOD_CLOSEACC:
+		return control_bodyrate_impl(ctl_data);
+
+	default:
+		static hrt_abstime last_print = 0;
+
+		if (ecl_elapsed_time(&last_print) > 5e6) {
+			warnx("invalid param setting FW_YCO_METHOD");
+			last_print = ecl_absolute_time();
+		}
+	}
+
+	return math::constrain(_last_output, -1.0f, 1.0f);
+}
+
+float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.speed_body_u) &&
+	      PX4_ISFINITE(ctl_data.speed_body_v) &&
+	      PX4_ISFINITE(ctl_data.speed_body_w) &&
+	      PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
+		perf_count(_nonfinite_input_perf);
+		return _rate_setpoint;
+	}
+
+//	static int counter = 0;
+	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
+	_rate_setpoint = 0.0f;
+
+	if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v +
+		  ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) {
+		float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) +
+				     ctl_data.speed_body_w * sinf(ctl_data.pitch));
+
+		if (fabsf(denumerator) > FLT_EPSILON) {
+			_rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint +
+					  9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) +
+					  ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) /
+					 denumerator;
+
+//			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
+		}
+
+//		if(counter % 20 == 0) {
+//			warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
+//		}
+	}
+
+	/* limit the rate */ //XXX: move to body angluar rates
+
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
+
+//	counter++;
+
+	if (!PX4_ISFINITE(_rate_setpoint)) {
+		warnx("yaw rate sepoint not finite");
+		_rate_setpoint = 0.0f;
+	}
+
+	return _rate_setpoint;
+}
+
+float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
+
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	bool lock_integrator = ctl_data.lock_integrator;
+
+	if (dt_micros > 500000) {
+		lock_integrator = true;
+	}
+
+	/* input conditioning */
+	float airspeed = ctl_data.airspeed;
+
+	if (!PX4_ISFINITE(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
+
+	} else if (airspeed < ctl_data.airspeed_min) {
+		airspeed = ctl_data.airspeed_min;
+	}
+
+
+	/* Transform setpoint to body angular rates (jacobian) */
+	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
+			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
+
+	/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
+	if (_coordinated_method == COORD_METHOD_CLOSEACC) {
+		//XXX: filtering of acceleration?
+		_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
+	}
+
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
+
+		float id = _rate_error * dt;
+
+		/*
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
+		 */
+		if (_last_output < -1.0f) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+
+		} else if (_last_output > 1.0f) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
+		       ctl_data.scaler;  //scaler is proportional to 1/airspeed
+	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+
+
+	return math::constrain(_last_output, -1.0f, 1.0f);
+}
+
+float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
+{
+	/* dont set a rate setpoint */
+	return 0.0f;
+}

attitude_fw/ecl_yaw_controller.h

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h
+ * Definition of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+#ifndef ECL_YAW_CONTROLLER_H
+#define ECL_YAW_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "ecl_controller.h"
+
+class __EXPORT ECL_YawController :
+	public ECL_Controller
+{
+public:
+	ECL_YawController();
+
+	~ECL_YawController();
+
+	float control_attitude(const struct ECL_ControlData &ctl_data);
+
+	float control_bodyrate(const struct ECL_ControlData &ctl_data);
+
+	/* Additional setters */
+	void set_coordinated_min_speed(float coordinated_min_speed)
+	{
+		_coordinated_min_speed = coordinated_min_speed;
+	}
+
+	void set_coordinated_method(int32_t coordinated_method)
+	{
+		_coordinated_method = coordinated_method;
+	}
+
+protected:
+	float _coordinated_min_speed;
+
+	enum {
+		COORD_METHOD_OPEN = 0,
+		COORD_METHOD_CLOSEACC = 1,
+	};
+
+	int32_t _coordinated_method;
+
+	float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
+
+	float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
+
+	float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
+
+};
+
+#endif // ECL_YAW_CONTROLLER_H

ecl.h

@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl.h
+ * Adapter / shim layer for system calls needed by ECL
+ *
+ */
+
+#include <drivers/drv_hrt.h>
+#include <px4_log.h>
+
+#define ecl_absolute_time hrt_absolute_time
+#define ecl_elapsed_time hrt_elapsed_time
+#define ECL_WARN PX4_WARN
+#define ECL_INFO PX4_INFO

l1/ecl_l1_pos_controller.cpp

@@ -0,0 +1,372 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_controller.h
+ * Implementation of L1 position control.
+ * Authors and acknowledgements in header.
+ *
+ */
+
+#include <float.h>
+
+#include "ecl_l1_pos_controller.h"
+
+float ECL_L1_Pos_Controller::nav_roll()
+{
+	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
+	ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
+	return ret;
+}
+
+float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand()
+{
+	return _lateral_accel;
+}
+
+float ECL_L1_Pos_Controller::nav_bearing()
+{
+	return _wrap_pi(_nav_bearing);
+}
+
+float ECL_L1_Pos_Controller::bearing_error()
+{
+	return _bearing_error;
+}
+
+float ECL_L1_Pos_Controller::target_bearing()
+{
+	return _target_bearing;
+}
+
+float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
+{
+	/* following [2], switching on L1 distance */
+	return math::min(wp_radius, _L1_distance);
+}
+
+bool ECL_L1_Pos_Controller::reached_loiter_target(void)
+{
+	return _circle_mode;
+}
+
+float ECL_L1_Pos_Controller::crosstrack_error(void)
+{
+	return _crosstrack_error;
+}
+
+void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+				       const math::Vector<2> &ground_speed_vector)
+{
+
+	/* this follows the logic presented in [1] */
+
+	float eta;
+	float xtrack_vel;
+	float ltrack_vel;
+
+	/* get the direction between the last (visited) and next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
+
+	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate vector from A to B */
+	math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
+
+	/*
+	 * check if waypoints are on top of each other. If yes,
+	 * skip A and directly continue to B
+	 */
+	if (vector_AB.length() < 1.0e-6f) {
+		vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
+	}
+
+	vector_AB.normalize();
+
+	/* calculate the vector from waypoint A to the aircraft */
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	/* calculate crosstrack error (output only) */
+	_crosstrack_error = vector_AB % vector_A_to_airplane;
+
+	/*
+	 * If the current position is in a +-135 degree angle behind waypoint A
+	 * and further away from A than the L1 distance, then A becomes the L1 point.
+	 * If the aircraft is already between A and B normal L1 logic is applied.
+	 */
+	float distance_A_to_airplane = vector_A_to_airplane.length();
+	float alongTrackDist = vector_A_to_airplane * vector_AB;
+
+	/* estimate airplane position WRT to B */
+	math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	
+	/* calculate angle of airplane position vector relative to line) */
+
+	// XXX this could probably also be based solely on the dot product
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
+
+	/* extension from [2], fly directly to A */
+	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
+
+		/* calculate eta to fly to waypoint A */
+
+		/* unit vector from waypoint A to current position */
+		math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
+		eta = atan2f(xtrack_vel, ltrack_vel);
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
+
+	/*
+	 * If the AB vector and the vector from B to airplane point in the same
+	 * direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
+	 */
+	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
+		/*
+		 * Extension, fly back to waypoint.
+		 * 
+		 * This corner case is possible if the system was following
+		 * the AB line from waypoint A to waypoint B, then is
+		 * switched to manual mode (or otherwise misses the waypoint)
+		 * and behind the waypoint continues to follow the AB line.
+		 */
+
+		/* calculate eta to fly to waypoint B */
+		
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
+		eta = atan2f(xtrack_vel, ltrack_vel);
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
+
+	} else {
+
+		/* calculate eta to fly along the line between A and B */
+
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % vector_AB;
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * vector_AB;
+		/* calculate eta2 (angle of velocity vector relative to line) */
+		float eta2 = atan2f(xtrack_vel, ltrack_vel);
+		/* calculate eta1 (angle to L1 point) */
+		float xtrackErr = vector_A_to_airplane % vector_AB;
+		float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
+		/* limit output to 45 degrees */
+		sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
+		float eta1 = asinf(sine_eta1);
+		eta = eta1 + eta2;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
+
+	}
+
+	/* limit angle to +-90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* flying to waypoints, not circling them */
+	_circle_mode = false;
+
+	/* the bearing angle, in NED frame */
+	_bearing_error = eta;
+}
+
+void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+				       const math::Vector<2> &ground_speed_vector)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	/* calculate the gains for the PD loop (circle tracking) */
+	float omega = (2.0f * M_PI_F / _L1_period);
+	float K_crosstrack = omega * omega;
+	float K_velocity = 2.0f * _L1_damping * omega;
+
+	/* update bearing to next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
+
+	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate the vector from waypoint A to current position */
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	math::Vector<2> vector_A_to_airplane_unit;
+
+	/* prevent NaN when normalizing */
+	if (vector_A_to_airplane.length() > FLT_EPSILON) {
+		/* store the normalized vector from waypoint A to current position */
+		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+	} else {
+		vector_A_to_airplane_unit = vector_A_to_airplane;
+	}
+
+	/* calculate eta angle towards the loiter center */
+
+	/* velocity across / orthogonal to line from waypoint to current position */
+	float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
+	/* velocity along line from waypoint to current position */
+	float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
+	float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
+
+	/* calculate the lateral acceleration to capture the center point */
+	float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* for PD control: Calculate radial position and velocity errors */
+
+	/* radial velocity error */
+	float xtrack_vel_circle = -ltrack_vel_center;
+	/* radial distance from the loiter circle (not center) */
+	float xtrack_err_circle = vector_A_to_airplane.length() - radius;
+
+	/* cross track error for feedback */
+	_crosstrack_error = xtrack_err_circle;
+
+	/* calculate PD update to circle waypoint */
+	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
+
+	/* calculate velocity on circle / along tangent */
+	float tangent_vel = xtrack_vel_center * loiter_direction;
+
+	/* prevent PD output from turning the wrong way */
+	if (tangent_vel < 0.0f) {
+		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f);
+	}
+
+	/* calculate centripetal acceleration setpoint */
+	float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle));
+
+	/* add PD control on circle and centripetal acceleration for total circle command */
+	float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
+
+	/*
+	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
+	 * the commands switch over. Only fly towards waypoint if outside the circle.
+	 */
+
+	// XXX check switch over
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
+		(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
+		_lateral_accel = lateral_accel_sp_center;
+		_circle_mode = false;
+		/* angle between requested and current velocity vector */
+		_bearing_error = eta;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
+
+	} else {
+		_lateral_accel = lateral_accel_sp_circle;
+		_circle_mode = true;
+		_bearing_error = 0.0f;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
+	}
+}
+
+
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	float eta;
+
+	/* 
+	 * As the commanded heading is the only reference
+	 * (and no crosstrack correction occurs),
+	 * target and navigation bearing become the same
+	 */
+	_target_bearing = _nav_bearing = _wrap_pi(navigation_heading);
+	eta = _target_bearing - _wrap_pi(current_heading);
+	eta = _wrap_pi(eta);
+
+	/* consequently the bearing error is exactly eta: */
+	_bearing_error = eta; 
+
+	/* ground speed is the length of the ground speed vector */
+	float ground_speed = ground_speed_vector.length();
+
+	/* adjust L1 distance to keep constant frequency */
+	_L1_distance = ground_speed / _heading_omega;
+	float omega_vel = ground_speed * _heading_omega;
+
+	/* not circling a waypoint */
+	_circle_mode = false;
+
+	/* navigating heading means by definition no crosstrack error */
+	_crosstrack_error = 0;
+
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
+}
+
+void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
+{
+	/* the logic in this section is trivial, but originally proposed by [2] */
+
+	/* reset all heading / error measures resulting in zero roll */
+	_target_bearing = current_heading;
+	_nav_bearing = current_heading;
+	_bearing_error = 0;
+	_crosstrack_error = 0;
+	_lateral_accel = 0;
+
+	/* not circling a waypoint when flying level */
+	_circle_mode = false;
+
+}
+
+
+math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
+{
+	/* this is an approximation for small angles, proposed by [2] */
+
+	math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
+
+	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
+}
+

l1/ecl_l1_pos_controller.h

@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_control.h
+ * Implementation of L1 position control.
+ *
+ *
+ * Acknowledgements and References:
+ *
+ *    This implementation has been built for PX4 based on the original
+ *    publication from [1] and does include a lot of the ideas (not code)
+ *    from [2].
+ *
+ *
+ *    [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
+ *    Proceedings of the AIAA Guidance, Navigation and Control
+ *    Conference, Aug 2004. AIAA-2004-4900.
+ *
+ *    [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013.
+ *     - Explicit control over frequency and damping
+ *     - Explicit control over track capture angle
+ *     - Ability to use loiter radius smaller than L1 length
+ *     - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
+ *     - Modified to enable period and damping of guidance loop to be set explicitly
+ *     - Modified to provide explicit control over capture angle
+ *
+ */
+
+#ifndef ECL_L1_POS_CONTROLLER_H
+#define ECL_L1_POS_CONTROLLER_H
+
+#include <mathlib/mathlib.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+
+/**
+ * L1 Nonlinear Guidance Logic
+ */
+class __EXPORT ECL_L1_Pos_Controller
+{
+public:
+	ECL_L1_Pos_Controller() {
+		_L1_period = 25;
+		_L1_damping = 0.75f;
+	}
+
+	/**
+	 * The current target bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float nav_bearing();
+
+
+	/**
+	 * Get lateral acceleration demand.
+	 *
+	 * @return Lateral acceleration in m/s^2
+	 */
+	float nav_lateral_acceleration_demand();
+
+
+	/**
+	 * Heading error.
+	 *
+	 * The heading error is either compared to the current track
+	 * or to the tangent of the current loiter radius.
+	 */
+	float bearing_error();
+
+
+	/**
+	 * Bearing from aircraft to current target.
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float target_bearing();
+
+
+	/**
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float nav_roll();
+
+
+	/**
+	 * Get the current crosstrack error.
+	 *
+	 * @return Crosstrack error in meters.
+	 */
+	float crosstrack_error();
+
+
+	/**
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reached_loiter_target();
+
+
+	/**
+	 * Returns true if following a circle (loiter)
+	 */
+	bool circle_mode() {
+		return _circle_mode;
+	}
+
+
+	/**
+	 * Get the switch distance
+	 * 
+	 * This is the distance at which the system will
+	 * switch to the next waypoint. This depends on the
+	 * period and damping
+	 *
+	 * @param waypoint_switch_radius The switching radius the waypoint has set.
+	 */
+	float switch_distance(float waypoint_switch_radius);
+
+
+	/**
+	 * Navigate between two waypoints
+	 *
+	 * Calling this function with two waypoints results in the
+	 * control outputs to fly to the line segment defined by
+	 * the points and once captured following the line segment.
+	 * This follows the logic in [1].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+			   const math::Vector<2> &ground_speed);
+
+
+	/**
+	 * Navigate on an orbit around a loiter waypoint.
+	 *
+	 * This allow orbits smaller than the L1 length,
+	 * this modification was introduced in [2].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+			   const math::Vector<2> &ground_speed_vector);
+
+
+	/**
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain direction and does not perform cross
+	 * track correction. Helpful for semi-autonomous modes. Introduced
+	 * by [2].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
+
+
+	/**
+	 * Keep the wings level.
+	 *
+	 * This is typically needed for maximum-lift-demand situations,
+	 * such as takeoff or near stall. Introduced in [2].
+	 */
+	void navigate_level_flight(float current_heading);
+
+
+	/**
+	 * Set the L1 period.
+	 */
+	void set_l1_period(float period) {
+		_L1_period = period;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate normalized frequency for heading tracking */
+		_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
+	}
+
+
+	/**
+	 * Set the L1 damping factor.
+	 *
+	 * The original publication recommends a default of sqrt(2) / 2 = 0.707
+	 */
+	void set_l1_damping(float damping) {
+		_L1_damping = damping;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate the L1 gain (following [2]) */
+		_K_L1 = 4.0f * _L1_damping * _L1_damping;
+	}
+
+
+	/**
+	 * Set the maximum roll angle output in radians
+	 *
+	 */
+	void set_l1_roll_limit(float roll_lim_rad) {
+		_roll_lim_rad = roll_lim_rad;
+	}
+
+private:
+
+	float _lateral_accel;		///< Lateral acceleration setpoint in m/s^2
+	float _L1_distance;		///< L1 lead distance, defined by period and damping
+	bool _circle_mode;		///< flag for loiter mode
+	float _nav_bearing;		///< bearing to L1 reference point
+	float _bearing_error;		///< bearing error
+	float _crosstrack_error;	///< crosstrack error in meters
+	float _target_bearing;		///< the heading setpoint
+
+	float _L1_period;		///< L1 tracking period in seconds
+	float _L1_damping;		///< L1 damping ratio
+	float _L1_ratio;		///< L1 ratio for navigation
+	float _K_L1;			///< L1 control gain for _L1_damping
+	float _heading_omega;		///< Normalized frequency
+
+	float _roll_lim_rad;  ///<maximum roll angle
+
+	/**
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
+	 * @return The vector in meters pointing from the reference position to the coordinates
+	 */
+	math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
+
+};
+
+
+#endif /* ECL_L1_POS_CONTROLLER_H */

validation/data_validator.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   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 data_validator.c
+ *
+ * A data validation class to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include "data_validator.h"
+#include <ecl/ecl.h>
+
+DataValidator::DataValidator(DataValidator *prev_sibling) :
+	_time_last(0),
+	_timeout_interval(20000),
+	_event_count(0),
+	_error_count(0),
+	_error_density(0),
+	_priority(0),
+	_mean{0.0f},
+	_lp{0.0f},
+	_M2{0.0f},
+	_rms{0.0f},
+	_value{0.0f},
+	_value_equal_count(0),
+	_sibling(prev_sibling)
+{
+
+}
+
+DataValidator::~DataValidator()
+{
+
+}
+
+void
+DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
+{
+	_event_count++;
+
+	if (error_count_in > _error_count) {
+		_error_density += (error_count_in - _error_count);
+	} else if (_error_density > 0) {
+		_error_density--;
+	}
+
+	_error_count = error_count_in;
+	_priority = priority_in;
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		if (_time_last == 0) {
+			_mean[i] = 0;
+			_lp[i] = val[i];
+			_M2[i] = 0;
+		} else {
+			float lp_val = val[i] - _lp[i];
+
+			float delta_val = lp_val - _mean[i];
+			_mean[i] += delta_val / _event_count;
+			_M2[i] += delta_val * (lp_val - _mean[i]);
+			_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
+
+			if (fabsf(_value[i] - val[i]) < 0.000001f) {
+				_value_equal_count++;
+			} else {
+				_value_equal_count = 0;
+			}
+		}
+
+		// XXX replace with better filter, make it auto-tune to update rate
+		_lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
+
+		_value[i] = val[i];
+	}
+
+	_time_last = timestamp;
+}
+
+float
+DataValidator::confidence(uint64_t timestamp)
+{
+	/* check if we have any data */
+	if (_time_last == 0) {
+		return 0.0f;
+	}
+
+	/* check error count limit */
+	if (_error_count > NORETURN_ERRCOUNT) {
+		return 0.0f;
+	}
+
+	/* we got the exact same sensor value N times in a row */
+	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
+		return 0.0f;
+	}
+
+	/* timed out - that's it */
+	if (timestamp - _time_last > _timeout_interval) {
+		return 0.0f;
+	}
+
+	/* cap error density counter at window size */
+	if (_error_density > ERROR_DENSITY_WINDOW) {
+		_error_density = ERROR_DENSITY_WINDOW;
+	}
+
+	/* return local error density for last N measurements */
+	return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
+}
+
+int
+DataValidator::priority()
+{
+	return _priority;
+}
+
+void
+DataValidator::print()
+{
+	if (_time_last == 0) {
+		ECL_INFO("\tno data");
+		return;
+	}
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f",
+			(double) _value[i], (double)_lp[i], (double)_mean[i],
+			(double)_rms[i], (double)confidence(hrt_absolute_time()));
+	}
+}

validation/data_validator.h

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   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 data_validator.h
+ *
+ * A data validation class to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include <cmath>
+#include <stdint.h>
+
+class __EXPORT DataValidator {
+public:
+	DataValidator(DataValidator *prev_sibling = nullptr);
+	virtual ~DataValidator();
+
+	/**
+	 * Put an item into the validator.
+	 *
+	 * @param val		Item to put
+	 */
+	void			put(uint64_t timestamp, float val[3], uint64_t error_count, int priority);
+
+	/**
+	 * Get the next sibling in the group
+	 *
+	 * @return		the next sibling
+	 */
+	DataValidator*		sibling() { return _sibling; }
+
+	/**
+	 * Get the confidence of this validator
+	 * @return		the confidence between 0 and 1
+	 */
+	float			confidence(uint64_t timestamp);
+
+	/**
+	 * Get the error count of this validator
+	 * @return		the error count
+	 */
+	uint64_t		error_count() { return _error_count; }
+
+	/**
+	 * Get the values of this validator
+	 * @return		the stored value
+	 */
+	float*			value() { return _value; }
+
+	/**
+	 * Get the used status of this validator
+	 * @return		true if this validator ever saw data
+	 */
+	bool			used() { return (_time_last > 0); }
+
+	/**
+	 * Get the priority of this validator
+	 * @return		the stored priority
+	 */
+	int			priority();
+
+	/**
+	 * Get the RMS values of this validator
+	 * @return		the stored RMS
+	 */
+	float*			rms() { return _rms; }
+
+	/**
+	 * Print the validator value
+	 *
+	 */
+	void			print();
+
+	/**
+	 * Set the timeout value
+	 *
+	 * @param timeout_interval_us The timeout interval in microseconds
+	 */
+	void			set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
+
+private:
+	static const unsigned _dimensions = 3;
+	uint64_t _time_last;			/**< last timestamp */
+	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
+	uint64_t _event_count;			/**< total data counter */
+	uint64_t _error_count;			/**< error count */
+	int _error_density;			/**< ratio between successful reads and errors */
+	int _priority;				/**< sensor nominal priority */
+	float _mean[_dimensions];		/**< mean of value */
+	float _lp[3];				/**< low pass value */
+	float _M2[3];				/**< RMS component value */
+	float _rms[3];				/**< root mean square error */
+	float _value[3];			/**< last value */
+	float _value_equal_count;		/**< equal values in a row */
+	DataValidator *_sibling;		/**< sibling in the group */
+	const unsigned NORETURN_ERRCOUNT = 10000;	/**< if the error count reaches this value, return sensor as invalid */
+	const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
+	const unsigned VALUE_EQUAL_COUNT_MAX = 100;	/**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
+
+	/* we don't want this class to be copied */
+	DataValidator(const DataValidator&);
+	DataValidator operator=(const DataValidator&);
+};

validation/data_validator_group.cpp

@@ -0,0 +1,224 @@
+/****************************************************************************
+ *
+ *   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 data_validator_group.cpp
+ *
+ * A data validation group to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include "data_validator_group.h"
+#include <ecl/ecl.h>
+
+DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
+	_first(nullptr),
+	_curr_best(-1),
+	_prev_best(-1),
+	_first_failover_time(0),
+	_toggle_count(0)
+{
+	DataValidator *next = _first;
+
+	for (unsigned i = 0; i < siblings; i++) {
+		next = new DataValidator(next);
+	}
+
+	_first = next;
+}
+
+DataValidatorGroup::~DataValidatorGroup()
+{
+
+}
+
+void
+DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
+{
+	DataValidator *next = _first;
+
+	while (next != nullptr) {
+		next->set_timeout(timeout_interval_us);
+		next = next->sibling();
+	}
+}
+
+void
+DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (i == index) {
+			next->put(timestamp, val, error_count, priority);
+			break;
+		}
+		next = next->sibling();
+		i++;
+	}
+}
+
+float*
+DataValidatorGroup::get_best(uint64_t timestamp, int *index)
+{
+	DataValidator *next = _first;
+
+	// XXX This should eventually also include voting
+	int pre_check_best = _curr_best;
+	float pre_check_confidence = 1.0f;
+	int pre_check_prio = -1;
+	float max_confidence = -1.0f;
+	int max_priority = -1000;
+	int max_index = -1;
+	DataValidator *best = nullptr;
+
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		float confidence = next->confidence(timestamp);
+
+		if (static_cast<int>(i) == pre_check_best) {
+			pre_check_prio = next->priority();
+			pre_check_confidence = confidence;
+		}
+
+		/*
+		 * Switch if:
+		 * 1) the confidence is higher and priority is equal or higher
+		 * 2) the confidence is no less than 1% different and the priority is higher
+		 */
+		if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
+			(confidence > max_confidence && (next->priority() >= max_priority)) ||
+			(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
+			) {
+			max_index = i;
+			max_confidence = confidence;
+			max_priority = next->priority();
+			best = next;
+		}
+
+		next = next->sibling();
+		i++;
+	}
+
+	/* the current best sensor is not matching the previous best sensor */
+	if (max_index != _curr_best) {
+
+		bool true_failsafe = true;
+
+		/* check wether the switch was a failsafe or preferring a higher priority sensor */
+		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
+			fabsf(pre_check_confidence - max_confidence) < 0.1f) {
+			/* this is not a failover */
+			true_failsafe = false;
+		}
+
+		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
+		if (_curr_best < 0) {
+			_prev_best = max_index;
+		} else {
+			/* we were initialized before, this is a real failsafe */
+			_prev_best = pre_check_best;
+
+			if (true_failsafe) {
+				_toggle_count++;
+
+				/* if this is the first time, log when we failed */
+				if (_first_failover_time == 0) {
+					_first_failover_time = timestamp;
+				}
+			}
+		}
+
+		/* for all cases we want to keep a record of the best index */
+		_curr_best = max_index;
+	}
+	*index = max_index;
+	return (best) ? best->value() : nullptr;
+}
+
+float
+DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
+{
+	DataValidator *next = _first;
+
+	float vibe = 0.0f;
+
+	/* find the best RMS value of a non-timed out sensor */
+	while (next != nullptr) {
+
+		if (next->confidence(timestamp) > 0.5f) {
+			float* rms = next->rms();
+
+			for (unsigned j = 0; j < 3; j++) {
+				if (rms[j] > vibe) {
+					vibe = rms[j];
+				}
+			}
+		}
+
+		next = next->sibling();
+	}
+
+	return vibe;
+}
+
+void
+DataValidatorGroup::print()
+{
+	/* print the group's state */
+	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
+		_toggle_count);
+
+
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (next->used()) {
+			ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+			next->print();
+		}
+		next = next->sibling();
+		i++;
+	}
+}
+
+unsigned
+DataValidatorGroup::failover_count()
+{
+	return _toggle_count;
+}

validation/data_validator_group.h

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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 data_validator_group.h
+ *
+ * A data validation group to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include "data_validator.h"
+
+class __EXPORT DataValidatorGroup {
+public:
+	DataValidatorGroup(unsigned siblings);
+	virtual ~DataValidatorGroup();
+
+	/**
+	 * Put an item into the validator group.
+	 *
+	 * @param index		Sensor index
+	 * @param timestamp	The timestamp of the measurement
+	 * @param val		The 3D vector
+	 * @param error_count	The current error count of the sensor
+	 * @param priority	The priority of the sensor
+	 */
+	void			put(unsigned index, uint64_t timestamp,
+					float val[3], uint64_t error_count, int priority);
+
+	/**
+	 * Get the best data triplet of the group
+	 *
+	 * @return		pointer to the array of best values
+	 */
+	float*			get_best(uint64_t timestamp, int *index);
+
+	/**
+	 * Get the RMS / vibration factor
+	 *
+	 * @return		float value representing the RMS, which a valid indicator for vibration
+	 */
+	float			get_vibration_factor(uint64_t timestamp);
+
+	/**
+	 * Get the number of failover events
+	 *
+	 * @return		the number of failovers
+	 */
+	unsigned		failover_count();
+
+	/**
+	 * Print the validator value
+	 *
+	 */
+	void			print();
+
+	/**
+	 * Set the timeout value
+	 *
+	 * @param timeout_interval_us The timeout interval in microseconds
+	 */
+	void			set_timeout(uint64_t timeout_interval_us);
+
+private:
+	DataValidator *_first;		/**< sibling in the group */
+	int _curr_best;		/**< currently best index */
+	int _prev_best;		/**< the previous best index */
+	uint64_t _first_failover_time;	/**< timestamp where the first failover occured or zero if none occured */
+	unsigned _toggle_count;		/**< number of back and forth switches between two sensors */
+	static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;
+
+	/* we don't want this class to be copied */
+	DataValidatorGroup(const DataValidatorGroup&);
+	DataValidatorGroup operator=(const DataValidatorGroup&);
+};