mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
added tailsitter recovery library
This commit is contained in:
Roman
Lorenz Meier
parent
f36669723d
commit
4e5fdebb13
42
src/lib/tailsitter_recovery/CMakeLists.txt
Normal file
42
src/lib/tailsitter_recovery/CMakeLists.txt
Normal file
@ -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 :
|
||||
182
src/lib/tailsitter_recovery/tailsitter_recovery.cpp
Normal file
182
src/lib/tailsitter_recovery/tailsitter_recovery.cpp
Normal file
@ -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 <math.h>
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
74
src/lib/tailsitter_recovery/tailsitter_recovery.h
Normal file
74
src/lib/tailsitter_recovery/tailsitter_recovery.h
Normal file
@ -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 <bapstroman@gmail.com>
|
||||
*/
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
#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
|
||||
};
|
||||
Loading…
x
Reference in New Issue
Block a user