PX4 Firmware
PX4 Autopilot Software http://px4.io
AttitudeControl.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file AttitudeControl.cpp
36  */
37 
38 #include <AttitudeControl.hpp>
39 
40 #include <mathlib/math/Limits.hpp>
42 
43 using namespace matrix;
44 
46 {
47  _proportional_gain = proportional_gain;
48 
49  // prepare yaw weight from the ratio between roll/pitch and yaw gains
50  const float roll_pitch_gain = (proportional_gain(0) + proportional_gain(1)) / 2.f;
51  _yaw_w = math::constrain(proportional_gain(2) / roll_pitch_gain, 0.f, 1.f);
52 
53  _proportional_gain(2) = roll_pitch_gain;
54 }
55 
56 matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, const float yawspeed_feedforward)
57 {
58  // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
59  q.normalize();
60  qd.normalize();
61 
62  // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
63  const Vector3f e_z = q.dcm_z();
64  const Vector3f e_z_d = qd.dcm_z();
65  Quatf qd_red(e_z, e_z_d);
66 
67  if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
68  // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
69  // full attitude control anyways generates no yaw input and directly takes the combination of
70  // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
71  qd_red = qd;
72 
73  } else {
74  // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
75  qd_red *= q;
76  }
77 
78  // mix full and reduced desired attitude
79  Quatf q_mix = qd_red.inversed() * qd;
80  q_mix *= math::signNoZero(q_mix(0));
81  // catch numerical problems with the domain of acosf and asinf
82  q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
83  q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
84  qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));
85 
86  // quaternion attitude control law, qe is rotation from q to qd
87  const Quatf qe = q.inversed() * qd;
88 
89  // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
90  // also taking care of the antipodal unit quaternion ambiguity
91  const Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();
92 
93  // calculate angular rates setpoint
94  matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);
95 
96  // Feed forward the yaw setpoint rate.
97  // yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,
98  // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
99  // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
100  // and multiply it by the yaw setpoint rate (yaw_sp_move_rate).
101  // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
102  // such that it can be added to the rates setpoint.
103  rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;
104 
105  // limit rates
106  for (int i = 0; i < 3; i++) {
107  rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
108  }
109 
110  return rate_setpoint;
111 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
Vector3< Type > dcm_z() const
Corresponding body z-axis to an attitude quaternion / last orthogonal unit basis vector.
Definition: Quaternion.hpp:514
int signNoZero(T val)
Definition: Functions.hpp:56
matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
Run one control loop cycle calculation.
void normalize()
Definition: Vector.hpp:87
Limiting / constrain helper functions.
Quaternion class.
Definition: Dcm.hpp:24
collection of rather simple mathematical functions that get used over and over again ...
A quaternion based attitude controller.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void setProportionalGain(const matrix::Vector3f &proportional_gain)
Set proportional attitude control gain.
Vector3< Type > imag() const
Imaginary components of quaternion.
Definition: Quaternion.hpp:501
Quaternion inversed() const
Invert quaternion.
Definition: Quaternion.hpp:343
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
Matrix< Type, M, N > emult(const Matrix< Type, M, N > &other) const
Definition: Matrix.hpp:157