Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 row */
80  struct FloatVect3 b_x;
81  VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[1], R_rp.m[2]);
82  /* body z-axis (thrust vect) is last row */
83  struct FloatVect3 thrust_vect;
84  VECT3_ASSIGN(thrust_vect, R_rp.m[6], R_rp.m[7], 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  * dot(b,n) = 0
103  */
104  float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
105 
106  struct FloatVect3 b;
107  VECT3_ASSIGN(b, psi_vect.x, psi_vect.y, -dot/thrust_vect.z);
108 
109  dot = VECT3_DOT_PRODUCT(b_x, b);
110  struct FloatVect3 cross;
111  VECT3_CROSS_PRODUCT(cross, b_x, b);
112  // norm of the cross product
113  float nc = FLOAT_VECT3_NORM(cross);
114  // angle = atan2(norm(cross(a,b)), dot(a,b))
115  float yaw2 = atan2(nc, dot) / 2.0;
116 
117  // negative angle if needed
118  // sign(dot(cross(a,b), n)
119  float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect);
120  if (dot_cross_ab < 0) {
121  yaw2 = -yaw2;
122  }
123 
124  /* quaternion with yaw command */
125  struct FloatQuat q_yaw;
126  QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
127 
128  /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
129  float_quat_comp(quat, &q_rp, &q_yaw);
130  float_quat_normalize(quat);
132 
133 }
ANGLE_FLOAT_OF_BFP
#define ANGLE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:211
AttRefQuatInt::quat
struct Int32Quat quat
Definition: stabilization_attitude_ref_quat_int.h:51
float_quat_comp
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:320
quat_from_earth_cmd_i
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_quat_transformations.c:48
b
float b
Definition: wedgebug.c:202
VECT3_CROSS_PRODUCT
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
VECT3_DOT_PRODUCT
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:250
quat_from_earth_cmd_f
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading)
Definition: stabilization_attitude_quat_transformations.c:63
FLOAT_VECT3_NORM
#define FLOAT_VECT3_NORM(_v)
Definition: pprz_algebra_float.h:164
quat_from_rpy_cmd_i
void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy)
Definition: stabilization_attitude_quat_transformations.c:28
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
float_quat_normalize
static void float_quat_normalize(struct FloatQuat *q)
Definition: pprz_algebra_float.h:380
float_rmat_of_quat
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
Definition: pprz_algebra_float.c:236
Int32Vect2
Definition: pprz_algebra_int.h:83
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
EULERS_FLOAT_OF_BFP
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:709
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
float_quat_wrap_shortest
static void float_quat_wrap_shortest(struct FloatQuat *q)
Definition: pprz_algebra_float.h:396
stabilization_attitude_quat_transformations.h
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
int32_t
signed long int32_t
Definition: types.h:19
QUAT_ASSIGN
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:580
float_quat_of_orientation_vect
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
Definition: pprz_algebra_float.c:560
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
FloatRMat::m
float m[3 *3]
Definition: pprz_algebra_float.h:78
quat_from_rpy_cmd_f
void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy)
Definition: stabilization_attitude_quat_transformations.c:37
heading
float heading
Definition: wedgebug.c:258