diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000000..fb17243f14 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 ECL Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name ECL nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__ecl + COMPILE_FLAGS + -Os + SRCS + attitude_fw/ecl_controller.cpp + attitude_fw/ecl_pitch_controller.cpp + attitude_fw/ecl_roll_controller.cpp + attitude_fw/ecl_yaw_controller.cpp + l1/ecl_l1_pos_controller.cpp + validation/data_validator.cpp + validation/data_validator_group.cpp + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/attitude_fw/ecl_controller.cpp b/attitude_fw/ecl_controller.cpp new file mode 100644 index 0000000000..7bf4129e48 --- /dev/null +++ b/attitude_fw/ecl_controller.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_controller.cpp + * Definition of base class for other controllers + * + * @author Lorenz Meier + * @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/attitude_fw/ecl_controller.h b/attitude_fw/ecl_controller.h new file mode 100644 index 0000000000..ac1ac88d04 --- /dev/null +++ b/attitude_fw/ecl_controller.h @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_controller.h + * Definition of base class for other controllers + * + * @author Lorenz Meier + * @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; + 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/attitude_fw/ecl_pitch_controller.cpp b/attitude_fw/ecl_pitch_controller.cpp new file mode 100644 index 0000000000..d04fe38ac3 --- /dev/null +++ b/attitude_fw/ecl_pitch_controller.cpp @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_pitch_controller.cpp + * Implementation of a simple orthogonal pitch PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_pitch_controller.h" +#include +#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/attitude_fw/ecl_pitch_controller.h b/attitude_fw/ecl_pitch_controller.h new file mode 100644 index 0000000000..fa7612cb99 --- /dev/null +++ b/attitude_fw/ecl_pitch_controller.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_pitch_controller.h + * Definition of a simple orthogonal pitch PID controller. + * + * @author Lorenz Meier + * @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/attitude_fw/ecl_roll_controller.cpp b/attitude_fw/ecl_roll_controller.cpp new file mode 100644 index 0000000000..9d40c73f3b --- /dev/null +++ b/attitude_fw/ecl_roll_controller.cpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_roll_controller.cpp + * Implementation of a simple orthogonal roll PID controller. + * + * Authors and acknowledgements in header. + */ + +#include +#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/attitude_fw/ecl_roll_controller.h b/attitude_fw/ecl_roll_controller.h new file mode 100644 index 0000000000..6d07bab8ab --- /dev/null +++ b/attitude_fw/ecl_roll_controller.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_roll_controller.h + * Definition of a simple orthogonal roll PID controller. + * + * @author Lorenz Meier + * @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/attitude_fw/ecl_yaw_controller.cpp b/attitude_fw/ecl_yaw_controller.cpp new file mode 100644 index 0000000000..4849bdad79 --- /dev/null +++ b/attitude_fw/ecl_yaw_controller.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_yaw_controller.h" +#include +#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/attitude_fw/ecl_yaw_controller.h b/attitude_fw/ecl_yaw_controller.h new file mode 100644 index 0000000000..81d8210549 --- /dev/null +++ b/attitude_fw/ecl_yaw_controller.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @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; + } + +protected: + float _coordinated_min_speed; + + enum { + COORD_METHOD_OPEN = 0, + COORD_METHOD_CLOSEACC = 1, + }; + + int32_t _coordinated_method; + + float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); + + float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data); + + float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); + +}; + +#endif // ECL_YAW_CONTROLLER_H diff --git a/ecl.h b/ecl.h new file mode 100644 index 0000000000..39e1f50809 --- /dev/null +++ b/ecl.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name APL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl.h + * Adapter / shim layer for system calls needed by ECL + * + */ + +#include +#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/l1/ecl_l1_pos_controller.cpp b/l1/ecl_l1_pos_controller.cpp new file mode 100644 index 0000000000..d1c864d782 --- /dev/null +++ b/l1/ecl_l1_pos_controller.cpp @@ -0,0 +1,372 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_l1_pos_controller.h + * Implementation of L1 position control. + * Authors and acknowledgements in header. + * + */ + +#include + +#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/l1/ecl_l1_pos_controller.h b/l1/ecl_l1_pos_controller.h new file mode 100644 index 0000000000..5c0804a392 --- /dev/null +++ b/l1/ecl_l1_pos_controller.h @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_l1_pos_control.h + * Implementation of L1 position control. + * + * + * Acknowledgements and References: + * + * This implementation has been built for PX4 based on the original + * publication from [1] and does include a lot of the ideas (not code) + * from [2]. + * + * + * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length + * - Modified to enable period and damping of guidance loop to be set explicitly + * - Modified to provide explicit control over capture angle + * + */ + +#ifndef ECL_L1_POS_CONTROLLER_H +#define ECL_L1_POS_CONTROLLER_H + +#include +#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/validation/data_validator.cpp b/validation/data_validator.cpp new file mode 100644 index 0000000000..737d9ea1a3 --- /dev/null +++ b/validation/data_validator.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator.c + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "data_validator.h" +#include + +DataValidator::DataValidator(DataValidator *prev_sibling) : + _time_last(0), + _timeout_interval(20000), + _event_count(0), + _error_count(0), + _error_density(0), + _priority(0), + _mean{0.0f}, + _lp{0.0f}, + _M2{0.0f}, + _rms{0.0f}, + _value{0.0f}, + _value_equal_count(0), + _sibling(prev_sibling) +{ + +} + +DataValidator::~DataValidator() +{ + +} + +void +DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) +{ + _event_count++; + + if (error_count_in > _error_count) { + _error_density += (error_count_in - _error_count); + } else if (_error_density > 0) { + _error_density--; + } + + _error_count = error_count_in; + _priority = priority_in; + + for (unsigned i = 0; i < _dimensions; i++) { + if (_time_last == 0) { + _mean[i] = 0; + _lp[i] = val[i]; + _M2[i] = 0; + } else { + float lp_val = val[i] - _lp[i]; + + float delta_val = lp_val - _mean[i]; + _mean[i] += delta_val / _event_count; + _M2[i] += delta_val * (lp_val - _mean[i]); + _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); + + if (fabsf(_value[i] - val[i]) < 0.000001f) { + _value_equal_count++; + } else { + _value_equal_count = 0; + } + } + + // XXX replace with better filter, make it auto-tune to update rate + _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; + + _value[i] = val[i]; + } + + _time_last = timestamp; +} + +float +DataValidator::confidence(uint64_t timestamp) +{ + /* check if we have any data */ + if (_time_last == 0) { + return 0.0f; + } + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + return 0.0f; + } + + /* we got the exact same sensor value N times in a row */ + if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { + return 0.0f; + } + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + return 0.0f; + } + + /* cap error density counter at window size */ + if (_error_density > ERROR_DENSITY_WINDOW) { + _error_density = ERROR_DENSITY_WINDOW; + } + + /* return local error density for last N measurements */ + return 1.0f - (_error_density / ERROR_DENSITY_WINDOW); +} + +int +DataValidator::priority() +{ + return _priority; +} + +void +DataValidator::print() +{ + if (_time_last == 0) { + ECL_INFO("\tno data"); + return; + } + + for (unsigned i = 0; i < _dimensions; i++) { + ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", + (double) _value[i], (double)_lp[i], (double)_mean[i], + (double)_rms[i], (double)confidence(hrt_absolute_time())); + } +} diff --git a/validation/data_validator.h b/validation/data_validator.h new file mode 100644 index 0000000000..dde9cb51aa --- /dev/null +++ b/validation/data_validator.h @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator.h + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#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(); + + /** + * Get the RMS values of this validator + * @return the stored RMS + */ + float* rms() { return _rms; } + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + +private: + static const unsigned _dimensions = 3; + uint64_t _time_last; /**< last timestamp */ + uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ + uint64_t _event_count; /**< total data counter */ + uint64_t _error_count; /**< error count */ + int _error_density; /**< ratio between successful reads and errors */ + int _priority; /**< sensor nominal priority */ + float _mean[_dimensions]; /**< mean of value */ + float _lp[3]; /**< low pass value */ + float _M2[3]; /**< RMS component value */ + float _rms[3]; /**< root mean square error */ + float _value[3]; /**< last value */ + float _value_equal_count; /**< equal values in a row */ + DataValidator *_sibling; /**< sibling in the group */ + const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */ + const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ + const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ + + /* we don't want this class to be copied */ + DataValidator(const DataValidator&); + DataValidator operator=(const DataValidator&); +}; diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp new file mode 100644 index 0000000000..ac4bcc8b9b --- /dev/null +++ b/validation/data_validator_group.cpp @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator_group.cpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#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 wether the switch was a failsafe or preferring a higher priority sensor */ + if (pre_check_prio != -1 && pre_check_prio < max_priority && + fabsf(pre_check_confidence - max_confidence) < 0.1f) { + /* this is not a failover */ + true_failsafe = false; + } + + /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ + if (_curr_best < 0) { + _prev_best = max_index; + } else { + /* we were initialized before, this is a real failsafe */ + _prev_best = pre_check_best; + + if (true_failsafe) { + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } + } + } + + /* for all cases we want to keep a record of the best index */ + _curr_best = max_index; + } + *index = max_index; + return (best) ? best->value() : nullptr; +} + +float +DataValidatorGroup::get_vibration_factor(uint64_t timestamp) +{ + DataValidator *next = _first; + + float vibe = 0.0f; + + /* find the best RMS value of a non-timed out sensor */ + while (next != nullptr) { + + if (next->confidence(timestamp) > 0.5f) { + float* rms = next->rms(); + + for (unsigned j = 0; j < 3; j++) { + if (rms[j] > vibe) { + vibe = rms[j]; + } + } + } + + next = next->sibling(); + } + + return vibe; +} + +void +DataValidatorGroup::print() +{ + /* print the group's state */ + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", + _toggle_count); + + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used()) { + ECL_INFO("sensor #%u, prio: %d", i, next->priority()); + next->print(); + } + next = next->sibling(); + i++; + } +} + +unsigned +DataValidatorGroup::failover_count() +{ + return _toggle_count; +} diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h new file mode 100644 index 0000000000..3756be2638 --- /dev/null +++ b/validation/data_validator_group.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator_group.h + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include "data_validator.h" + +class __EXPORT DataValidatorGroup { +public: + DataValidatorGroup(unsigned siblings); + virtual ~DataValidatorGroup(); + + /** + * Put an item into the validator group. + * + * @param index Sensor index + * @param timestamp The timestamp of the measurement + * @param val The 3D vector + * @param error_count The current error count of the sensor + * @param priority The priority of the sensor + */ + void put(unsigned index, uint64_t timestamp, + float val[3], uint64_t error_count, int priority); + + /** + * Get the best data triplet of the group + * + * @return pointer to the array of best values + */ + float* get_best(uint64_t timestamp, int *index); + + /** + * Get the RMS / vibration factor + * + * @return float value representing the RMS, which a valid indicator for vibration + */ + float get_vibration_factor(uint64_t timestamp); + + /** + * Get the number of failover events + * + * @return the number of failovers + */ + unsigned failover_count(); + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us); + +private: + DataValidator *_first; /**< sibling in the group */ + int _curr_best; /**< currently best index */ + int _prev_best; /**< the previous best index */ + uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */ + unsigned _toggle_count; /**< number of back and forth switches between two sensors */ + static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; + + /* we don't want this class to be copied */ + DataValidatorGroup(const DataValidatorGroup&); + DataValidatorGroup operator=(const DataValidatorGroup&); +};