From 4e5fdebb13ee7b2b1f4639183bfb296ed2e8379b Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 11 Dec 2015 10:11:39 +0100 Subject: [PATCH] added tailsitter recovery library --- src/lib/tailsitter_recovery/CMakeLists.txt | 42 ++++ .../tailsitter_recovery.cpp | 182 ++++++++++++++++++ .../tailsitter_recovery/tailsitter_recovery.h | 74 +++++++ 3 files changed, 298 insertions(+) create mode 100644 src/lib/tailsitter_recovery/CMakeLists.txt create mode 100644 src/lib/tailsitter_recovery/tailsitter_recovery.cpp create mode 100644 src/lib/tailsitter_recovery/tailsitter_recovery.h diff --git a/src/lib/tailsitter_recovery/CMakeLists.txt b/src/lib/tailsitter_recovery/CMakeLists.txt new file mode 100644 index 0000000000..c7bd713592 --- /dev/null +++ b/src/lib/tailsitter_recovery/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__tailsitter_recovery + COMPILE_FLAGS + -Os + SRCS + tailsitter_recovery.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/tailsitter_recovery/tailsitter_recovery.cpp b/src/lib/tailsitter_recovery/tailsitter_recovery.cpp new file mode 100644 index 0000000000..f18081b6f5 --- /dev/null +++ b/src/lib/tailsitter_recovery/tailsitter_recovery.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * 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 tailsitter_recovery.cpp +*/ + +#include "tailsitter_recovery.h" +#include + +TailsitterRecovery::TailsitterRecovery(): + _in_recovery_mode(false) +{ + _att_p(0) = _att_p(1) = _att_p(2) = 4.0f; +} + +TailsitterRecovery::~TailsitterRecovery() +{ + +} + +void TailsitterRecovery::setAttGains(math::Vector<3> &att_p, float yaw_ff) +{ + _att_p = att_p; + _yaw_ff = yaw_ff; +} + +void TailsitterRecovery::calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, + math::Vector<3> &rates_opt) +{ + math::Matrix<3, 3> R = q.to_dcm(); + + // compute error quaternion + math::Quaternion q_sp_inv = {q_sp(0), -q_sp(1), -q_sp(2), -q_sp(3)}; + math::Quaternion q_error = q * q_sp_inv; + + // compute tilt angle and corresponding tilt axis + math::Vector<3> zB = {0, 0, -1.0f}; + + math::Vector<3> zI = q_error.conjugate(zB); + + float tilt_angle; + float inner_prod = zI * zB; + + if (inner_prod >= 1.0f) { + tilt_angle = 0.0f; + + } else if (inner_prod <= -1.0f) { + tilt_angle = M_PI_F; + + } else { + tilt_angle = acosf(inner_prod); + } + + math::Vector<3> tilt_axis = {0, 0, -1}; + + if (math::min(fabsf(tilt_angle), fabsf(tilt_angle - M_PI_F)) > 0.00001f) { + tilt_axis = zI % zB; + } + + tilt_axis = R.transposed() * tilt_axis; + + // compute desired rates based on tilt angle and tilt axis + float tilt_dir = atan2f(tilt_axis(0), tilt_axis(1)); + float sign_x; + float sign_y; + float sign_z; + + if (tilt_dir < -M_PI_2_F) { + tilt_dir += M_PI_F; + sign_x = -1; + sign_y = -1; + sign_z = 1; + + } else if (tilt_dir < 0.0f) { + tilt_dir = 0.0f - tilt_dir; + sign_x = -1; + sign_y = 1; + sign_z = -1; + + } else if (tilt_dir < M_PI_2_F) { + tilt_dir += 0; + sign_x = 1; + sign_y = 1; + sign_z = 1; + + } else { + tilt_dir = M_PI_F - tilt_dir; + sign_x = 1; + sign_y = -1; + sign_z = -1; + } + + // optimal coefficients + float pwx_kx1 = -1.676f; + float pwx_ky1 = 1.38f; + float pwx_ky2 = 0.8725f; + float pwx_sx0 = 0.3586f; + float pwx_sx1 = 2.642f; + + float pwy_kx1 = -3.997f; + float pwy_sx0 = 2.133f; + float pwy_sx1 = 4.013f; + + float pwz_kx1 = 2.726f; + float pwz_ky1 = 3.168f; + float pwz_ky2 = -0.3913f; + float pwz_sx0 = 1.75f; + float pwz_sx1 = 2.298f; + + float x = tilt_angle; + float y = tilt_dir; + + rates_opt(1) = (pwx_ky1 * (M_PI_2_F - y) + pwx_ky2 * powf(M_PI_2_F - y, 2)) * + (pwx_kx1 * x * SigmoidFunction(pwx_sx1 * (pwx_sx0 - x)) - pwx_kx1 * x * expf(pwx_sx1 * + (pwx_sx0 - M_PI_F)) * SigmoidFunction(pwx_sx1 * (M_PI_F - pwx_sx0))); + rates_opt(0) = pwy_kx1 * x + powf((M_PI_2_F - y) / (M_PI_2_F), + 2) * (-pwy_kx1 * x * SigmoidFunction(pwy_sx1 * (pwy_sx0 - x))); + rates_opt(2) = (pwz_ky1 * (M_PI_2_F - y) + pwz_ky2 * powf(M_PI_2_F - y, 2)) * + (pwz_kx1 * (x - M_PI_F) * SigmoidFunction(pwz_sx1 * (x - pwz_sx0)) - pwz_kx1 * (x - M_PI_F) * SigmoidFunction( + -pwz_sx0 * pwz_sx1)); + rates_opt(0) *= -sign_x; + rates_opt(1) *= -sign_y * 1.5f; + rates_opt(2) *= -sign_z; + + float yaw_w = R(2, 2) > 0.0f ? R(2, 2) : 0.0f; + yaw_w = math::constrain(yaw_w, 0.0f, 1.0f); + + // we activate recovery mode if our tilt error is more that 30 degrees or + if (!_in_recovery_mode) { + if (fabsf(tilt_angle) > math::radians(30.0f)) { + _in_recovery_mode = true; + } + + } else { + if (fabsf(tilt_angle) < math::radians(5.0f)) { + _in_recovery_mode = false; + } + } + + // do normal attitude control, the other case has already been handled + if (!_in_recovery_mode) { + q_error = q_sp_inv * q; + rates_opt = q_error(0) > 0.0f ? _att_p.emult(q_error.imag()) * (-2.0f) : _att_p.emult(q_error.imag()) * (2.0f); + // don't want too strong yaw control. after recovery vehicle might have + // very large yaw error and shouldn't turn too fast + rates_opt(2) = math::constrain(rates_opt(2), -1.0f, 1.0f); + } + + rates_opt(2) += yaw_move_rate * yaw_w * _yaw_ff; + +} diff --git a/src/lib/tailsitter_recovery/tailsitter_recovery.h b/src/lib/tailsitter_recovery/tailsitter_recovery.h new file mode 100644 index 0000000000..5de16d25e8 --- /dev/null +++ b/src/lib/tailsitter_recovery/tailsitter_recovery.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 tailsitter_recovery.h + * + * Tailsitter optimal rate controller (underactuated pitch axis) + * + * This function can be used to compute desired body rates of a tailsitter + * suffering from an underactuated pitch axis. Tailsitters which produce a pitching moment + * from airflow over control surfaces mostly suffer from an underactuated pitch axis. + * This functions captures the solution of an optimal control problem which minimises + * the vehicle's tilt error and which penalises the desired rates of the underactuated + * pitch axis. + * Publication: + * Robin Ritz and Raffaello D'Andrea. A Global Strategy for Tailsitter Hover Control. + * + * @author Roman Bapst +*/ +#include + +#define SigmoidFunction(val) 1/(1 + expf(-val)) + +class TailsitterRecovery +{ +public: + TailsitterRecovery(); + ~TailsitterRecovery(); + + // Calculate the optimal rates: + // If the vehicle is not in need of a recovery, this function will do normal + // attitude control based on attitude error. If a recovery situation is detected + // then the rates are computed in an optimal way as described above. + void calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, math::Vector<3> &rates_opt); + + // Set the gains of the controller attitude loop. + void setAttGains(math::Vector<3> &att_p, float yaw_ff); + +private: + bool _in_recovery_mode; // indicates that the tailsitter is performing a recovery to hover + + math::Vector<3> _att_p; // gains for attitude loop + float _yaw_ff; // yaw feed forward gain +};