Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_quat_transformations.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 
28 void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) {
29  struct FloatEulers rpy_f;
30  EULERS_FLOAT_OF_BFP(rpy_f, *rpy);
31  struct FloatQuat quat_f;
32  quat_from_rpy_cmd_f(&quat_f, &rpy_f);
33  QUAT_BFP_OF_REAL(*quat, quat_f);
34 }
35 
36 void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) {
37  // only a plug for now... doesn't apply roll/pitch wrt. current yaw angle
38 
39  /* orientation vector describing simultaneous rotation of roll/pitch/yaw */
40  const struct FloatVect3 ov = {rpy->phi, rpy->theta, rpy->psi};
41  /* quaternion from that orientation vector */
43 
44 }
45 
46 void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) {
47  // use float conversion for now...
48  struct FloatVect2 cmd_f;
49  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
50  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
51  float heading_f = ANGLE_FLOAT_OF_BFP(heading);
52 
53  struct FloatQuat quat_f;
54  quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f);
55 
56  // convert back to fixed point
57  QUAT_BFP_OF_REAL(*quat, quat_f);
58 }
59 
60 void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) {
61 
62  /* cmd_x is positive to north = negative pitch
63  * cmd_y is positive to east = positive roll
64  *
65  * orientation vector describing simultaneous rotation of roll/pitch
66  */
67  const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0};
68  /* quaternion from that orientation vector */
69  struct FloatQuat q_rp;
71 
72  /* as rotation matrix */
73  struct FloatRMat R_rp;
74  FLOAT_RMAT_OF_QUAT(R_rp, q_rp);
75  /* body x-axis (before heading command) is first column */
76  struct FloatVect3 b_x;
77  VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]);
78  /* body z-axis (thrust vect) is last column */
79  struct FloatVect3 thrust_vect;
80  VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]);
81 
83 
84  /*
85  * Instead of using the psi setpoint angle to rotate around the body z-axis,
86  * calculate the real angle needed to align the projection of the body x-axis
87  * onto the horizontal plane with the psi setpoint.
88  *
89  * angle between two vectors a and b:
90  * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
91  * where the normal n is the thrust vector (i.e. both a and b lie in that plane)
92  */
93 
94  // desired heading vect in earth x-y plane
95  const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0};
96 
97  /* projection of desired heading onto body x-y plane
98  * b = v - dot(v,n)*n
99  */
100  float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
101  struct FloatVect3 dotn;
102  FLOAT_VECT3_SMUL(dotn, thrust_vect, dot);
103 
104  // b = v - dot(v,n)*n
105  struct FloatVect3 b;
106  FLOAT_VECT3_DIFF(b, psi_vect, dotn);
107  dot = FLOAT_VECT3_DOT_PRODUCT(b_x, b);
108  struct FloatVect3 cross;
109  VECT3_CROSS_PRODUCT(cross, b_x, b);
110  // norm of the cross product
111  float nc = FLOAT_VECT3_NORM(cross);
112  // angle = atan2(norm(cross(a,b)), dot(a,b))
113  float yaw2 = atan2(nc, dot) / 2.0;
114 
115  // negative angle if needed
116  // sign(dot(cross(a,b), n)
117  float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, thrust_vect);
118  if (dot_cross_ab < 0) {
119  yaw2 = -yaw2;
120  }
121 
122  /* quaternion with yaw command */
123  struct FloatQuat q_yaw;
124  QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
125 
126  /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
127  FLOAT_QUAT_COMP(*quat, q_rp, q_yaw);
128  FLOAT_QUAT_NORMALIZE(*quat);
130 
131 }
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:233
rotation matrix
#define ANGLE_FLOAT_OF_BFP(_ai)
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:665
Quaternion transformation functions.
float m[3 *3]
Rotation quaternion.
float psi
in radians
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)
float theta
in radians
euler angles
Roation quaternion.
#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov)
#define FLOAT_VECT3_NORM(_v)
float heading
Definition: ahrs_infrared.c:40
float phi
in radians
void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy)
#define FLOAT_VECT3_DIFF(_c, _a, _b)
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
signed long int32_t
Definition: types.h:19
#define FLOAT_QUAT_NORMALIZE(_q)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:496
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading)
#define FLOAT_VECT3_SMUL(_vo, _vi, _s)
void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy)
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:622
euler angles