Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
gvf_parametric_3d_lissajous.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
32 
34 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KX
35 #define GVF_PARAMETRIC_3D_LISSAJOUS_KX 0.001
36 #endif
37 
39 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KY
40 #define GVF_PARAMETRIC_3D_LISSAJOUS_KY 0.001
41 #endif
42 
44 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KZ
45 #define GVF_PARAMETRIC_3D_LISSAJOUS_KZ 0.001
46 #endif
47 
49 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CX
50 #define GVF_PARAMETRIC_3D_LISSAJOUS_CX 80
51 #endif
52 
54 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CY
55 #define GVF_PARAMETRIC_3D_LISSAJOUS_CY 80
56 #endif
57 
59 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CZ
60 #define GVF_PARAMETRIC_3D_LISSAJOUS_CZ 10
61 #endif
62 
64 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WX
65 #define GVF_PARAMETRIC_3D_LISSAJOUS_WX 1
66 #endif
67 
69 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WY
70 #define GVF_PARAMETRIC_3D_LISSAJOUS_WY 1
71 #endif
72 
74 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WZ
75 #define GVF_PARAMETRIC_3D_LISSAJOUS_WZ 1
76 #endif
77 
79 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DX
80 #define GVF_PARAMETRIC_3D_LISSAJOUS_DX 0
81 #endif
82 
84 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DY
85 #define GVF_PARAMETRIC_3D_LISSAJOUS_DY 0
86 #endif
87 
89 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DZ
90 #define GVF_PARAMETRIC_3D_LISSAJOUS_DZ 0
91 #endif
92 
94 #ifndef GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA
95 #define GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA 0
96 #endif
97 
99 
100 void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
101  float *f1dd, float *f2dd, float *f3dd)
102 {
112  float deltax_rad = gvf_parametric_trajectory.p_parametric[9]*M_PI/180;
113  float deltay_rad = gvf_parametric_trajectory.p_parametric[10]*M_PI/180;
114  float deltaz_rad = gvf_parametric_trajectory.p_parametric[11]*M_PI/180;
115  float alpha_rad = gvf_parametric_trajectory.p_parametric[12]*M_PI/180;
116 
117  float w = gvf_parametric_control.w;
119 
120  // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
121 
122  float nrf1 = cx*cosf(wx*wb + deltax_rad);
123  float nrf2 = cy*cosf(wy*wb + deltay_rad);
124 
125  *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
126  *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
127  *f3 = cz*cosf(wz*wb + deltaz_rad) + zo;
128 
129  float nrf1d = -wx*cx*sinf(wx*wb + deltax_rad);
130  float nrf2d = -wy*cy*sinf(wy*wb + deltay_rad);
131 
132  *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
133  *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
134  *f3d = -wz*cz*sinf(wz*wb + deltaz_rad);
135 
136  float nrf1dd = -wx*wx*cx*cosf(wx*wb + deltax_rad);
137  float nrf2dd = -wy*wy*cy*cosf(wy*wb + deltay_rad);
138 
139  *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
140  *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
141  *f3dd = -wz*wz*cz*cosf(wz*wb + deltaz_rad);
142 }
143 
GVF_PARAMETRIC_3D_LISSAJOUS_DY
#define GVF_PARAMETRIC_3D_LISSAJOUS_DY
Definition: gvf_parametric_3d_lissajous.c:85
GVF_PARAMETRIC_3D_LISSAJOUS_WY
#define GVF_PARAMETRIC_3D_LISSAJOUS_WY
Definition: gvf_parametric_3d_lissajous.c:70
GVF_PARAMETRIC_3D_LISSAJOUS_DX
#define GVF_PARAMETRIC_3D_LISSAJOUS_DX
Definition: gvf_parametric_3d_lissajous.c:80
common_nav.h
GVF_PARAMETRIC_3D_LISSAJOUS_KX
#define GVF_PARAMETRIC_3D_LISSAJOUS_KX
Definition: gvf_parametric_3d_lissajous.c:35
gvf_parametric_3d_lissajous_info
void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd)
Definition: gvf_parametric_3d_lissajous.c:100
gvf_parametric_tra::p_parametric
float p_parametric[16]
Definition: gvf_parametric.h:97
GVF_PARAMETRIC_3D_LISSAJOUS_KZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_KZ
Definition: gvf_parametric_3d_lissajous.c:45
gvf_parametric_3d_lissajous_par
gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par
Definition: gvf_parametric_3d_lissajous.c:98
gvf_parametric_control
gvf_parametric_con gvf_parametric_control
Definition: gvf_parametric.cpp:44
gvf_parametric.h
GVF_PARAMETRIC_3D_LISSAJOUS_CZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_CZ
Definition: gvf_parametric_3d_lissajous.c:60
GVF_PARAMETRIC_3D_LISSAJOUS_DZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_DZ
Definition: gvf_parametric_3d_lissajous.c:90
gvf_parametric_con::w
float w
Definition: gvf_parametric.h:75
GVF_PARAMETRIC_3D_LISSAJOUS_KY
#define GVF_PARAMETRIC_3D_LISSAJOUS_KY
Definition: gvf_parametric_3d_lissajous.c:40
gvf_parametric_trajectory
gvf_parametric_tra gvf_parametric_trajectory
Definition: gvf_parametric.cpp:47
gvf_parametric_con::s
int8_t s
Definition: gvf_parametric.h:77
GVF_PARAMETRIC_3D_LISSAJOUS_CX
#define GVF_PARAMETRIC_3D_LISSAJOUS_CX
Definition: gvf_parametric_3d_lissajous.c:50
GVF_PARAMETRIC_3D_LISSAJOUS_CY
#define GVF_PARAMETRIC_3D_LISSAJOUS_CY
Definition: gvf_parametric_3d_lissajous.c:55
GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA
#define GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA
Definition: gvf_parametric_3d_lissajous.c:95
gvf_parametric_con::beta
float beta
Definition: gvf_parametric.h:82
GVF_PARAMETRIC_3D_LISSAJOUS_WX
#define GVF_PARAMETRIC_3D_LISSAJOUS_WX
Definition: gvf_parametric_3d_lissajous.c:65
GVF_PARAMETRIC_3D_LISSAJOUS_WZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_WZ
Definition: gvf_parametric_3d_lissajous.c:75
gvf_par_3d_lis_par
Definition: gvf_parametric_3d_lissajous.h:52
gvf_parametric_3d_lissajous.h