diff --git a/CMakeLists.txt b/CMakeLists.txt index d8fc6104f4..f7345c74c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,7 +166,6 @@ if(ECL_ASAN) endif() add_subdirectory(airdata) -add_subdirectory(attitude_fw) add_subdirectory(EKF) add_subdirectory(geo) add_subdirectory(geo_lookup) diff --git a/attitude_fw/CMakeLists.txt b/attitude_fw/CMakeLists.txt deleted file mode 100644 index 7053ac69ec..0000000000 --- a/attitude_fw/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 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. -# -############################################################################ - -add_library(ecl_attitude_fw - ecl_controller.cpp - ecl_pitch_controller.cpp - ecl_roll_controller.cpp - ecl_wheel_controller.cpp - ecl_yaw_controller.cpp - ) -add_dependencies(ecl_attitude_fw prebuild_targets) -target_compile_definitions(ecl_attitude_fw PRIVATE -DMODULE_NAME="ecl/attitude_fw") -target_include_directories(ecl_attitude_fw PUBLIC ${ECL_SOURCE_DIR}) diff --git a/attitude_fw/ecl_controller.cpp b/attitude_fw/ecl_controller.cpp deleted file mode 100644 index c44af60215..0000000000 --- a/attitude_fw/ecl_controller.cpp +++ /dev/null @@ -1,145 +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() : - _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) -{ -} - -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; -} - -void ECL_Controller::set_bodyrate_setpoint(float rate) -{ - _bodyrate_setpoint = math::constrain(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::get_integrator() -{ - return _integrator; -} - -float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) -{ - float airspeed_result = airspeed; - - if (!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 deleted file mode 100644 index 6028825503..0000000000 --- a/attitude_fw/ecl_controller.h +++ /dev/null @@ -1,116 +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 - -struct ECL_ControlData { - float roll; - float pitch; - float yaw; - float body_x_rate; - float body_y_rate; - float body_z_rate; - 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 ECL_Controller -{ -public: - ECL_Controller(); - virtual ~ECL_Controller() = default; - - virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0; - virtual float control_euler_rate(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); - void set_bodyrate_setpoint(float rate); - - /* Getters */ - float get_rate_error(); - float get_desired_rate(); - float get_desired_bodyrate(); - float get_integrator(); - - 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; - 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 deleted file mode 100644 index e1b5e08b93..0000000000 --- a/attitude_fw/ecl_pitch_controller.cpp +++ /dev/null @@ -1,133 +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 - -float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - - /* Do not calculate control signal with bad inputs */ - if (!(ISFINITE(ctl_data.pitch_setpoint) && - ISFINITE(ctl_data.roll) && - ISFINITE(ctl_data.pitch) && - ISFINITE(ctl_data.airspeed))) { - ECL_WARN("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; - - return _rate_setpoint; -} - -float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(ISFINITE(ctl_data.roll) && - ISFINITE(ctl_data.pitch) && - ISFINITE(ctl_data.body_y_rate) && - ISFINITE(ctl_data.body_z_rate) && - ISFINITE(ctl_data.yaw_rate_setpoint) && - ISFINITE(ctl_data.airspeed_min) && - ISFINITE(ctl_data.airspeed_max) && - ISFINITE(ctl_data.scaler))) { - 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; - } - - _rate_error = _bodyrate_setpoint - ctl_data.body_y_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); - } - - /* add and constrain */ - _integrator = math::constrain(_integrator + id * _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; //scaler is proportional to 1/airspeed - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data) -{ - /* 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; - - set_bodyrate_setpoint(_bodyrate_setpoint); - - return control_bodyrate(ctl_data); -} diff --git a/attitude_fw/ecl_pitch_controller.h b/attitude_fw/ecl_pitch_controller.h deleted file mode 100644 index 459ef1e1e6..0000000000 --- a/attitude_fw/ecl_pitch_controller.h +++ /dev/null @@ -1,93 +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 "ecl_controller.h" - -class ECL_PitchController : - public ECL_Controller -{ -public: - ECL_PitchController() = default; - ~ECL_PitchController() = default; - - float control_attitude(const struct ECL_ControlData &ctl_data) override; - float control_euler_rate(const struct ECL_ControlData &ctl_data) override; - float control_bodyrate(const struct ECL_ControlData &ctl_data) override; - - /* 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_bodyrate_setpoint(float rate) - { - _bodyrate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate); - } - - void set_roll_ff(float roll_ff) - { - _roll_ff = roll_ff; - } - -protected: - float _max_rate_neg{0.0f}; - float _roll_ff{0.0f}; -}; - -#endif // ECL_PITCH_CONTROLLER_H diff --git a/attitude_fw/ecl_roll_controller.cpp b/attitude_fw/ecl_roll_controller.cpp deleted file mode 100644 index f1bf17eaf6..0000000000 --- a/attitude_fw/ecl_roll_controller.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_roll_controller.cpp - * Implementation of a simple orthogonal roll PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_roll_controller.h" -#include -#include -#include - -float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(ISFINITE(ctl_data.roll_setpoint) && ISFINITE(ctl_data.roll))) { - return _rate_setpoint; - } - - /* Calculate error */ - float roll_error = ctl_data.roll_setpoint - ctl_data.roll; - - /* Apply P controller */ - _rate_setpoint = roll_error / _tc; - - return _rate_setpoint; -} - -float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(ISFINITE(ctl_data.pitch) && - ISFINITE(ctl_data.body_x_rate) && - ISFINITE(ctl_data.body_z_rate) && - ISFINITE(ctl_data.yaw_rate_setpoint) && - ISFINITE(ctl_data.airspeed_min) && - ISFINITE(ctl_data.airspeed_max) && - ISFINITE(ctl_data.scaler))) { - 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; - } - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.body_x_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); - } - - /* add and constrain */ - _integrator = math::constrain(_integrator + id * _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; //scaler is proportional to 1/airspeed - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data) -{ - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - - set_bodyrate_setpoint(_bodyrate_setpoint); - - return control_bodyrate(ctl_data); - -} diff --git a/attitude_fw/ecl_roll_controller.h b/attitude_fw/ecl_roll_controller.h deleted file mode 100644 index e0fe925325..0000000000 --- a/attitude_fw/ecl_roll_controller.h +++ /dev/null @@ -1,66 +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 "ecl_controller.h" - -class ECL_RollController : - public ECL_Controller -{ -public: - ECL_RollController() = default; - ~ECL_RollController() = default; - - float control_attitude(const struct ECL_ControlData &ctl_data) override; - float control_euler_rate(const struct ECL_ControlData &ctl_data) override; - float control_bodyrate(const struct ECL_ControlData &ctl_data) override; -}; - -#endif // ECL_ROLL_CONTROLLER_H diff --git a/attitude_fw/ecl_wheel_controller.cpp b/attitude_fw/ecl_wheel_controller.cpp deleted file mode 100644 index 3067436228..0000000000 --- a/attitude_fw/ecl_wheel_controller.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2016 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 - -using matrix::wrap_pi; - -float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(ISFINITE(ctl_data.body_z_rate) && - ISFINITE(ctl_data.groundspeed) && - ISFINITE(ctl_data.groundspeed_scaler))) { - 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.body_z_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); - } - - /* add and constrain */ - _integrator = math::constrain(_integrator + id * _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 + - ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + _integrator); - - 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 (!(ISFINITE(ctl_data.yaw_setpoint) && - ISFINITE(ctl_data.yaw))) { - return _rate_setpoint; - } - - /* Calculate the error */ - float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw); - - /* 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/attitude_fw/ecl_wheel_controller.h b/attitude_fw/ecl_wheel_controller.h deleted file mode 100644 index 861943b147..0000000000 --- a/attitude_fw/ecl_wheel_controller.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2016 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 "ecl_controller.h" - -class ECL_WheelController : - public ECL_Controller -{ -public: - ECL_WheelController() = default; - ~ECL_WheelController() = default; - - float control_attitude(const struct ECL_ControlData &ctl_data) override; - - float control_bodyrate(const struct ECL_ControlData &ctl_data) override; - - float control_euler_rate(const struct ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; } -}; - -#endif // ECL_HEADING_CONTROLLER_H diff --git a/attitude_fw/ecl_yaw_controller.cpp b/attitude_fw/ecl_yaw_controller.cpp deleted file mode 100644 index 4ff7559a3b..0000000000 --- a/attitude_fw/ecl_yaw_controller.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2016 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 - -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 ecl_abstime last_print = 0; - - if (ecl_elapsed_time(&last_print) > 5e6) { - ECL_WARN("invalid param setting FW_YCO_METHOD"); - last_print = ecl_absolute_time(); - } - } - - return _rate_setpoint; -} - -float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(ISFINITE(ctl_data.roll) && - ISFINITE(ctl_data.pitch) && - ISFINITE(ctl_data.roll_rate_setpoint) && - ISFINITE(ctl_data.pitch_rate_setpoint))) { - return _rate_setpoint; - } - - float constrained_roll; - bool inverted = false; - - /* 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 = true; - - // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity - //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(-180.0f), math::radians(-100.0f)); - } - } - - constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint)); - - - if (!inverted) { - /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < - ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); - } - - if (!ISFINITE(_rate_setpoint)) { - ECL_WARN("yaw rate sepoint not finite"); - _rate_setpoint = 0.0f; - } - - return _rate_setpoint; -} - -float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(ISFINITE(ctl_data.roll) && ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.body_y_rate) && - ISFINITE(ctl_data.body_z_rate) && ISFINITE(ctl_data.pitch_rate_setpoint) && - ISFINITE(ctl_data.airspeed_min) && ISFINITE(ctl_data.airspeed_max) && - ISFINITE(ctl_data.scaler))) { - 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 (!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; - } - - /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */ - if (_coordinated_method == COORD_METHOD_CLOSEACC) { - // XXX lateral acceleration needs to go into integrator with a gain - //_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch))); - } - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.body_z_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); - } - - /* add and constrain */ - _integrator = math::constrain(_integrator + id * _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) * ctl_data.scaler * - ctl_data.scaler; //scaler is proportional to 1/airspeed - - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data) -{ - (void)ctl_data; // unused - - /* dont set a rate setpoint */ - return 0.0f; -} - -float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data) -{ - /* 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; - - set_bodyrate_setpoint(_bodyrate_setpoint); - - return control_bodyrate(ctl_data); - -} diff --git a/attitude_fw/ecl_yaw_controller.h b/attitude_fw/ecl_yaw_controller.h deleted file mode 100644 index 5ec6cd6f26..0000000000 --- a/attitude_fw/ecl_yaw_controller.h +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2016 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 "ecl_controller.h" - -class ECL_YawController : - public ECL_Controller -{ -public: - ECL_YawController() = default; - ~ECL_YawController() = default; - - float control_attitude(const struct ECL_ControlData &ctl_data) override; - float control_euler_rate(const struct ECL_ControlData &ctl_data) override; - float control_bodyrate(const struct ECL_ControlData &ctl_data) override; - - /* 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{1.0f}; - float _max_rate{0.0f}; - - int32_t _coordinated_method{COORD_METHOD_OPEN}; - - 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