Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 {
30  struct FloatEulers rpy_f;
31  EULERS_FLOAT_OF_BFP(rpy_f, *rpy);
32  struct FloatQuat quat_f;
33  quat_from_rpy_cmd_f(&quat_f, &rpy_f);
34  QUAT_BFP_OF_REAL(*quat, quat_f);
35 }
36 
37 void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy)
38 {
39  // only a plug for now... doesn't apply roll/pitch wrt. current yaw angle
40 
41  /* orientation vector describing simultaneous rotation of roll/pitch/yaw */
42  const struct FloatVect3 ov = {rpy->phi, rpy->theta, rpy->psi};
43  /* quaternion from that orientation vector */
45 
46 }
47 
48 void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
49 {
50  // use float conversion for now...
51  struct FloatVect2 cmd_f;
52  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
53  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
54  float heading_f = ANGLE_FLOAT_OF_BFP(heading);
55 
56  struct FloatQuat quat_f;
57  quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f);
58 
59  // convert back to fixed point
60  QUAT_BFP_OF_REAL(*quat, quat_f);
61 }
62 
63 void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading)
64 {
65 
66  /* cmd_x is positive to north = negative pitch
67  * cmd_y is positive to east = positive roll
68  *
69  * orientation vector describing simultaneous rotation of roll/pitch
70  */
71  const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0};
72  /* quaternion from that orientation vector */
73  struct FloatQuat q_rp;
75 
76  /* as rotation matrix */
77  struct FloatRMat R_rp;
78  float_rmat_of_quat(&R_rp, &q_rp);
79  /* body x-axis (before heading command) is first column */
80  struct FloatVect3 b_x;
81  VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]);
82  /* body z-axis (thrust vect) is last column */
83  struct FloatVect3 thrust_vect;
84  VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]);
85 
87 
88  /*
89  * Instead of using the psi setpoint angle to rotate around the body z-axis,
90  * calculate the real angle needed to align the projection of the body x-axis
91  * onto the horizontal plane with the psi setpoint.
92  *
93  * angle between two vectors a and b:
94  * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
95  * where the normal n is the thrust vector (i.e. both a and b lie in that plane)
96  */
97 
98  // desired heading vect in earth x-y plane
99  const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0};
100 
101  /* projection of desired heading onto body x-y plane
102  * b = v - dot(v,n)*n
103  */
104  float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
105  struct FloatVect3 dotn;
106  VECT3_SMUL(dotn, thrust_vect, dot);
107 
108  // b = v - dot(v,n)*n
109  struct FloatVect3 b;
110  VECT3_DIFF(b, psi_vect, dotn);
111  dot = VECT3_DOT_PRODUCT(b_x, b);
112  struct FloatVect3 cross;
113  VECT3_CROSS_PRODUCT(cross, b_x, b);
114  // norm of the cross product
115  float nc = FLOAT_VECT3_NORM(cross);
116  // angle = atan2(norm(cross(a,b)), dot(a,b))
117  float yaw2 = atan2(nc, dot) / 2.0;
118 
119  // negative angle if needed
120  // sign(dot(cross(a,b), n)
121  float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect);
122  if (dot_cross_ab < 0) {
123  yaw2 = -yaw2;
124  }
125 
126  /* quaternion with yaw command */
127  struct FloatQuat q_yaw;
128  QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
129 
130  /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
131  float_quat_comp(quat, &q_rp, &q_yaw);
132  float_quat_normalize(quat);
134 
135 }
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:243
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:249
float phi
in radians
Quaternion transformation functions.
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:181
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
float psi
in radians
#define FLOAT_VECT3_NORM(_v)
#define ANGLE_FLOAT_OF_BFP(_ai)
euler angles
Roation quaternion.
float theta
in radians
static float heading
Definition: ahrs_infrared.c:45
void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy)
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
euler angles
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:516
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:685
static void float_quat_normalize(struct FloatQuat *q)
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
signed long int32_t
Definition: types.h:19
float m[3 *3]
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:188
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading)
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
static void float_quat_wrap_shortest(struct FloatQuat *q)
void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy)
rotation matrix
Rotation quaternion.
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:642