From 1a9452e411210a2f8ae583e95491749ff5940500 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 15 Mar 2020 14:14:59 -0400 Subject: [PATCH] fw_att_control: move ecl/attitude_fw into fw_att_control module - ecl/attitude_fw was never maintained as a standalone library - moving ecl/attitude_fw library into the fw_att_control module to ease further development --- src/lib/ecl | 2 +- src/modules/fw_att_control/CMakeLists.txt | 10 +- .../FixedwingAttitudeControl.hpp | 8 +- src/modules/fw_att_control/ecl_controller.cpp | 145 ++++++++++++ src/modules/fw_att_control/ecl_controller.h | 117 ++++++++++ .../fw_att_control/ecl_pitch_controller.cpp | 135 ++++++++++++ .../fw_att_control/ecl_pitch_controller.h | 93 ++++++++ .../fw_att_control/ecl_roll_controller.cpp | 128 +++++++++++ .../fw_att_control/ecl_roll_controller.h | 66 ++++++ .../fw_att_control/ecl_wheel_controller.cpp | 131 +++++++++++ .../fw_att_control/ecl_wheel_controller.h | 68 ++++++ .../fw_att_control/ecl_yaw_controller.cpp | 207 ++++++++++++++++++ .../fw_att_control/ecl_yaw_controller.h | 92 ++++++++ 13 files changed, 1195 insertions(+), 7 deletions(-) create mode 100644 src/modules/fw_att_control/ecl_controller.cpp create mode 100644 src/modules/fw_att_control/ecl_controller.h create mode 100644 src/modules/fw_att_control/ecl_pitch_controller.cpp create mode 100644 src/modules/fw_att_control/ecl_pitch_controller.h create mode 100644 src/modules/fw_att_control/ecl_roll_controller.cpp create mode 100644 src/modules/fw_att_control/ecl_roll_controller.h create mode 100644 src/modules/fw_att_control/ecl_wheel_controller.cpp create mode 100644 src/modules/fw_att_control/ecl_wheel_controller.h create mode 100644 src/modules/fw_att_control/ecl_yaw_controller.cpp create mode 100644 src/modules/fw_att_control/ecl_yaw_controller.h diff --git a/src/lib/ecl b/src/lib/ecl index 975060d108..f1aa53db8a 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 975060d108e901f3ea70a9b88d1e5fa2112e49ff +Subproject commit f1aa53db8a8053e5f27a698084a7362f727ce375 diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index 53eed2886a..a8c79cf31c 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -35,7 +35,13 @@ px4_add_module( MAIN fw_att_control SRCS FixedwingAttitudeControl.cpp + FixedwingAttitudeControl.hpp + + ecl_controller.cpp + ecl_pitch_controller.cpp + ecl_roll_controller.cpp + ecl_wheel_controller.cpp + ecl_yaw_controller.cpp DEPENDS - git_ecl - ecl_attitude_fw + px4_work_queue ) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 6d9d693077..d150abffe8 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -32,10 +32,10 @@ ****************************************************************************/ #include -#include -#include -#include -#include +#include "ecl_pitch_controller.h" +#include "ecl_roll_controller.h" +#include "ecl_wheel_controller.h" +#include "ecl_yaw_controller.h" #include #include #include diff --git a/src/modules/fw_att_control/ecl_controller.cpp b/src/modules/fw_att_control/ecl_controller.cpp new file mode 100644 index 0000000000..f785319c68 --- /dev/null +++ b/src/modules/fw_att_control/ecl_controller.cpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * 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 (!PX4_ISFINITE(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed_result = 0.5f * (minspeed + maxspeed); + + } else if (airspeed < minspeed) { + airspeed_result = minspeed; + } + + return airspeed_result; +} diff --git a/src/modules/fw_att_control/ecl_controller.h b/src/modules/fw_att_control/ecl_controller.h new file mode 100644 index 0000000000..56c091b724 --- /dev/null +++ b/src/modules/fw_att_control/ecl_controller.h @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 + +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/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp new file mode 100644 index 0000000000..015cf8bbb5 --- /dev/null +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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 (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && + PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.airspeed))) { + + PX4_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 (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.body_y_rate) && + PX4_ISFINITE(ctl_data.body_z_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))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = hrt_elapsed_time(&_last_run); + _last_run = hrt_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/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/ecl_pitch_controller.h new file mode 100644 index 0000000000..459ef1e1e6 --- /dev/null +++ b/src/modules/fw_att_control/ecl_pitch_controller.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp new file mode 100644 index 0000000000..e6924ea350 --- /dev/null +++ b/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_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 (!(PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.body_x_rate) && + PX4_ISFINITE(ctl_data.body_z_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))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = hrt_elapsed_time(&_last_run); + _last_run = hrt_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/src/modules/fw_att_control/ecl_roll_controller.h b/src/modules/fw_att_control/ecl_roll_controller.h new file mode 100644 index 0000000000..e0fe925325 --- /dev/null +++ b/src/modules/fw_att_control/ecl_roll_controller.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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/src/modules/fw_att_control/ecl_wheel_controller.cpp b/src/modules/fw_att_control/ecl_wheel_controller.cpp new file mode 100644 index 0000000000..7a8393acfb --- /dev/null +++ b/src/modules/fw_att_control/ecl_wheel_controller.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 (!(PX4_ISFINITE(ctl_data.body_z_rate) && + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = hrt_elapsed_time(&_last_run); + _last_run = hrt_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 (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_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/src/modules/fw_att_control/ecl_wheel_controller.h b/src/modules/fw_att_control/ecl_wheel_controller.h new file mode 100644 index 0000000000..861943b147 --- /dev/null +++ b/src/modules/fw_att_control/ecl_wheel_controller.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp new file mode 100644 index 0000000000..33f54fa6b2 --- /dev/null +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * 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 hrt_abstime last_print = 0; + + if (hrt_elapsed_time(&last_print) > 5e6) { + PX4_WARN("invalid param setting FW_YCO_METHOD"); + last_print = hrt_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 (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.roll_rate_setpoint) && + PX4_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 (!PX4_ISFINITE(_rate_setpoint)) { + PX4_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 (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.body_y_rate) && + PX4_ISFINITE(ctl_data.body_z_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))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = hrt_elapsed_time(&_last_run); + _last_run = hrt_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; + } + + /* 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/src/modules/fw_att_control/ecl_yaw_controller.h b/src/modules/fw_att_control/ecl_yaw_controller.h new file mode 100644 index 0000000000..5ec6cd6f26 --- /dev/null +++ b/src/modules/fw_att_control/ecl_yaw_controller.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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