PX4 Firmware
PX4 Autopilot Software http://px4.io
ControlMath.cpp
Go to the documentation of this file.
1 
2 /****************************************************************************
3  *
4  * Copyright (C) 2018 PX4 Development Team. All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file ControlMath.cpp
37  */
38 
39 #include "ControlMath.hpp"
40 #include <px4_platform_common/defines.h>
41 #include <float.h>
42 #include <mathlib/mathlib.h>
43 
44 using namespace matrix;
45 namespace ControlMath
46 {
47 void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
48 {
49  att_sp.yaw_body = yaw_sp;
50 
51  // desired body_z axis = -normalize(thrust_vector)
52  Vector3f body_x, body_y, body_z;
53 
54  if (thr_sp.length() > 0.00001f) {
55  body_z = -thr_sp.normalized();
56 
57  } else {
58  // no thrust, set Z axis to safe value
59  body_z = Vector3f(0.f, 0.f, 1.f);
60  }
61 
62  // vector of desired yaw direction in XY plane, rotated by PI/2
63  Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);
64 
65  if (fabsf(body_z(2)) > 0.000001f) {
66  // desired body_x axis, orthogonal to body_z
67  body_x = y_C % body_z;
68 
69  // keep nose to front while inverted upside down
70  if (body_z(2) < 0.0f) {
71  body_x = -body_x;
72  }
73 
74  body_x.normalize();
75 
76  } else {
77  // desired thrust is in XY plane, set X downside to construct correct matrix,
78  // but yaw component will not be used actually
79  body_x.zero();
80  body_x(2) = 1.0f;
81  }
82 
83  // desired body_y axis
84  body_y = body_z % body_x;
85 
86  Dcmf R_sp;
87 
88  // fill rotation matrix
89  for (int i = 0; i < 3; i++) {
90  R_sp(i, 0) = body_x(i);
91  R_sp(i, 1) = body_y(i);
92  R_sp(i, 2) = body_z(i);
93  }
94 
95  //copy quaternion setpoint to attitude setpoint topic
96  Quatf q_sp = R_sp;
97  q_sp.copyTo(att_sp.q_d);
98  att_sp.q_d_valid = true;
99 
100  // calculate euler angles, for logging only, must not be used for control
101  Eulerf euler = R_sp;
102  att_sp.roll_body = euler(0);
103  att_sp.pitch_body = euler(1);
104  att_sp.thrust_body[2] = -thr_sp.length();
105 }
106 
107 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
108 {
109  if (Vector2f(v0 + v1).norm() <= max) {
110  // vector does not exceed maximum magnitude
111  return v0 + v1;
112 
113  } else if (v0.length() >= max) {
114  // the magnitude along v0, which has priority, already exceeds maximum.
115  return v0.normalized() * max;
116 
117  } else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) {
118  // the two vectors are equal
119  return v0.normalized() * max;
120 
121  } else if (v0.length() < 0.001f) {
122  // the first vector is 0.
123  return v1.normalized() * max;
124 
125  } else {
126  // vf = final vector with ||vf|| <= max
127  // s = scaling factor
128  // u1 = unit of v1
129  // vf = v0 + v1 = v0 + s * u1
130  // constraint: ||vf|| <= max
131  //
132  // solve for s: ||vf|| = ||v0 + s * u1|| <= max
133  //
134  // Derivation:
135  // For simplicity, replace v0 -> v, u1 -> u
136  // v0(0/1/2) -> v0/1/2
137  // u1(0/1/2) -> u0/1/2
138  //
139  // ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2
140  // v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2
141  // s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0
142  //
143  // quadratic equation:
144  // -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a)
145  //
146  // b = 2 * u.dot(v)
147  // a = 1 (because u is normalized)
148  // c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2
149  //
150  // sqrt(b^2 - 4*a*c) =
151  // sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2))
152  //
153  // s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2
154  // = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2))
155  // m = u.dot(v)
156  // s = -m + sqrt(m^2 - c)
157  //
158  //
159  //
160  // notes:
161  // - s (=scaling factor) needs to be positive
162  // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement
163 
164  Vector2f u1 = v1.normalized();
165  float m = u1.dot(v0);
166  float c = v0.dot(v0) - max * max;
167  float s = -m + sqrtf(m * m - c);
168  return v0 + u1 * s;
169  }
170 }
171 
172 bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
173  const Vector3f &line_a, const Vector3f &line_b, Vector3f &res)
174 {
175  // project center of sphere on line normalized AB
176  Vector3f ab_norm = line_b - line_a;
177 
178  if (ab_norm.length() < 0.01f) {
179  return true;
180  }
181 
182  ab_norm.normalize();
183  Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
184  float cd_len = (sphere_c - d).length();
185 
186  if (sphere_r > cd_len) {
187  // we have triangle CDX with known CD and CX = R, find DX
188  float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
189 
190  if ((sphere_c - line_b) * ab_norm > 0.0f) {
191  // target waypoint is already behind us
192  res = line_b;
193 
194  } else {
195  // target is in front of us
196  res = d + ab_norm * dx_len; // vector A->B on line
197  }
198 
199  return true;
200 
201  } else {
202 
203  // have no roots, return D
204  res = d; // go directly to line
205 
206  // previous waypoint is still in front of us
207  if ((sphere_c - line_a) * ab_norm < 0.0f) {
208  res = line_a;
209  }
210 
211  // target waypoint is already behind us
212  if ((sphere_c - line_b) * ab_norm > 0.0f) {
213  res = line_b;
214  }
215 
216  return false;
217  }
218 }
219 }
void normalize()
Definition: Vector.hpp:87
bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, const Vector3f &line_a, const Vector3f &line_b, Vector3f &res)
This method was used for smoothing the corners along two lines.
void copyTo(Type dst[M *N]) const
Definition: Matrix.hpp:115
Quaternion class.
Definition: Dcm.hpp:24
Vector normalized() const
Definition: Vector.hpp:103
Type dot(const MatrixM1 &b) const
Definition: Vector.hpp:55
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector2< float > Vector2f
Definition: Vector2.hpp:69
Type length() const
Definition: Vector.hpp:83
Euler angles class.
Definition: AxisAngle.hpp:18
void zero()
Definition: Matrix.hpp:421
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
Converts thrust vector and yaw set-point to a desired attitude.
Definition: ControlMath.cpp:47
Vector3< float > Vector3f
Definition: Vector3.hpp:136
Vector3 normalized() const
Definition: Vector3.hpp:104
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
Outputs the sum of two vectors but respecting the limits and priority.
Dual< Scalar, N > max(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:224
Simple functions for vector manipulation that do not fit into matrix lib.