diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt deleted file mode 100644 index 9f8e02ba2e7..00000000000 --- a/src/lib/ecl/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -############################################################################ -# -# 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_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 - attitude_fw/ecl_wheel_controller.cpp - l1/ecl_l1_pos_controller.cpp - validation/data_validator.cpp - validation/data_validator_group.cpp - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp deleted file mode 100644 index 7bf4129e480..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * 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 - * @author Thomas Gubler - * - * 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 -#include - -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; -} diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h deleted file mode 100644 index fed7d1d8fba..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ /dev/null @@ -1,125 +0,0 @@ -/**************************************************************************** - * - * 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 - * @author Thomas Gubler - * - * 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 -#include -#include - -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; - float groundspeed; - float groundspeed_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); -}; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp deleted file mode 100644 index d04fe38ac3f..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/**************************************************************************** - * - * 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 -#include -#include -#include -#include -#include -#include - -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); -} diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h deleted file mode 100644 index fa7612cb993..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ /dev/null @@ -1,89 +0,0 @@ -/**************************************************************************** - * - * 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 - * @author Thomas Gubler - * - * 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 -#include - -#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 diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp deleted file mode 100644 index 9d40c73f3b4..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/**************************************************************************** - * - * 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 -#include "ecl_roll_controller.h" -#include -#include -#include -#include -#include -#include - -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); -} - diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h deleted file mode 100644 index 6d07bab8abd..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * 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 - * @author Thomas Gubler - * - * 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 -#include - -#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 diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp deleted file mode 100644 index 7e34ff2a61f..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * 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_wheel_controller.cpp - * Implementation of a simple PID wheel controller for heading tracking. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_wheel_controller.h" -#include -#include -#include -#include -#include -#include -#include - -ECL_WheelController::ECL_WheelController() : - ECL_Controller("wheel") -{ -} - -ECL_WheelController::~ECL_WheelController() -{ -} - -float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.groundspeed) && - PX4_ISFINITE(ctl_data.groundspeed_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 min_speed = 1.0f; - - /* Calculate body angular rate error */ - _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - - if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { - - float id = _rate_error * dt * ctl_data.groundspeed_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 = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + - _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + - integrator_constrained; - /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", - (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ - - - return math::constrain(_last_output, -1.0f, 1.0f); -} - - -float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling wheel"); - return _rate_setpoint; - } - - /* Calculate the error */ - float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; - /* shortest angle (wrap around) */ - yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; - /*warnx("yaw_error: %.4f", (double)yaw_error);*/ - - /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = yaw_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - } - - return _rate_setpoint; -} diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h deleted file mode 100644 index f153a8f8f18..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * 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_wheel_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Andreas Antener - * - * 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_HEADING_CONTROLLER_H -#define ECL_HEADING_CONTROLLER_H - -#include -#include - -#include "ecl_controller.h" - -class __EXPORT ECL_WheelController : - public ECL_Controller -{ -public: - ECL_WheelController(); - - ~ECL_WheelController(); - - float control_attitude(const struct ECL_ControlData &ctl_data); - - float control_bodyrate(const struct ECL_ControlData &ctl_data); -}; - -#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp deleted file mode 100644 index 4849bdad79a..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/**************************************************************************** - * - * 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 -#include -#include -#include -#include -#include -#include - -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; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h deleted file mode 100644 index 511b5fd36f0..00000000000 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ /dev/null @@ -1,98 +0,0 @@ -/**************************************************************************** - * - * 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 - * @author Thomas Gubler - * - * 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 -#include - -#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; - } - - enum { - COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1 - }; - -protected: - float _coordinated_min_speed; - float _max_rate; - - 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 diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h deleted file mode 100644 index 39e1f508093..00000000000 --- a/src/lib/ecl/ecl.h +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * 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 -#include - -#define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time -#define ECL_WARN PX4_WARN -#define ECL_INFO PX4_INFO diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp deleted file mode 100644 index d1c864d7827..00000000000 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ /dev/null @@ -1,372 +0,0 @@ -/**************************************************************************** - * - * 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 - -#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(CONSTANTS_RADIUS_OF_EARTH); -} - diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h deleted file mode 100644 index 5c0804a3928..00000000000 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ /dev/null @@ -1,268 +0,0 @@ -/**************************************************************************** - * - * 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 -#include -#include - -/** - * 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; /// get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; - -}; - - -#endif /* ECL_L1_POS_CONTROLLER_H */ diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp deleted file mode 100644 index 4e55849df05..00000000000 --- a/src/lib/ecl/validation/data_validator.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#include "data_validator.h" -#include - -DataValidator::DataValidator(DataValidator *prev_sibling) : - _error_mask(ERROR_FLAG_NO_ERROR), - _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) -{ - float ret = 1.0f; - - /* check if we have any data */ - if (_time_last == 0) { - _error_mask |= ERROR_FLAG_NO_DATA; - ret = 0.0f; - - /* timed out - that's it */ - } else if (timestamp - _time_last > _timeout_interval) { - _error_mask |= ERROR_FLAG_TIMEOUT; - ret = 0.0f; - - /* we got the exact same sensor value N times in a row */ - } else if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { - _error_mask |= ERROR_FLAG_STALE_DATA; - ret = 0.0f; - - /* check error count limit */ - } else if (_error_count > NORETURN_ERRCOUNT) { - _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; - ret = 0.0f; - - /* cap error density counter at window size */ - } else if (_error_density > ERROR_DENSITY_WINDOW) { - _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; - _error_density = ERROR_DENSITY_WINDOW; - - /* no error */ - } else { - _error_mask = ERROR_FLAG_NO_ERROR; - } - - /* no critical errors */ - if (ret > 0.0f) { - /* return local error density for last N measurements */ - ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); - } - - return ret; -} - -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())); - } -} diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h deleted file mode 100644 index c313b1cb04b..00000000000 --- a/src/lib/ecl/validation/data_validator.h +++ /dev/null @@ -1,159 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#pragma once - -#include -#include - -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() { return (_priority); } - - /** - * Get the error state of this validator - * @return the bitmask with the error status - */ - uint32_t state() { return _error_mask; } - - /** - * Reset the error state of this validator - */ - void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } - - /** - * 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; } - - /** - * Data validator error states - */ - static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); - static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); - static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); - static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); - -private: - static const unsigned _dimensions = 3; - uint32_t _error_mask; /**< sensor error state */ - 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&); -}; diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp deleted file mode 100644 index 8bf060cf653..00000000000 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ /dev/null @@ -1,266 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#include "data_validator_group.h" -#include - -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(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 whether 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; - /* reset error flags, this is likely a hotplug sensor coming online late */ - best->reset_state(); - } - - /* 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 events)", - _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", - _toggle_count); - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used()) { - uint32_t flags = next->state(); - - ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), - ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); - - next->print(); - } - next = next->sibling(); - i++; - } -} - -unsigned -DataValidatorGroup::failover_count() -{ - return _toggle_count; -} - -int -DataValidatorGroup::failover_index() -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { - return i; - } - next = next->sibling(); - i++; - } - return -1; -} - -uint32_t -DataValidatorGroup::failover_state() -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { - return next->state(); - } - next = next->sibling(); - i++; - } - return DataValidator::ERROR_FLAG_NO_ERROR; -} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h deleted file mode 100644 index 855c3ed4aec..00000000000 --- a/src/lib/ecl/validation/data_validator_group.h +++ /dev/null @@ -1,122 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#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(); - - /** - * Get the index of the failed sensor in the group - * - * @return index of the failed sensor - */ - int failover_index(); - - /** - * Get the error state of the failed sensor in the group - * - * @return bitmask with erro states of the failed sensor - */ - uint32_t failover_state(); - - /** - * 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&); -};