Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
gvf_ellipse.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Hector Garcia de Marina
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  */
22 
30 #include "modules/nav/common_nav.h"
31 #include "gvf_ellipse.h"
32 #include "generated/airframe.h"
33 
35 #ifndef GVF_ELLIPSE_KE
36 #define GVF_ELLIPSE_KE 1
37 #endif
38 
40 #ifndef GVF_ELLIPSE_KN
41 #define GVF_ELLIPSE_KN 1
42 #endif
43 
45 #ifndef GVF_ELLIPSE_A
46 #define GVF_ELLIPSE_A 80
47 #endif
48 
50 #ifndef GVF_ELLIPSE_B
51 #define GVF_ELLIPSE_B 80
52 #endif
53 
55 #ifndef GVF_ELLIPSE_ALPHA
56 #define GVF_ELLIPSE_ALPHA 0
57 #endif
58 
61  };
62 
63 void gvf_ellipse_info(float *phi, struct gvf_grad *grad,
64  struct gvf_Hess *hess)
65 {
66 
67  struct EnuCoor_f *p = stateGetPositionEnu_f();
68  float px = p->x;
69  float py = p->y;
70  float wx = gvf_trajectory.p[0];
71  float wy = gvf_trajectory.p[1];
72  float a = gvf_trajectory.p[2];
73  float b = gvf_trajectory.p[3];
74  float alpha = gvf_trajectory.p[4];
75 
76  float cosa = cosf(alpha);
77  float sina = sinf(alpha);
78 
79  // Phi(x,y)
80  float xel = (px - wx) * cosa - (py - wy) * sina;
81  float yel = (px - wx) * sina + (py - wy) * cosa;
82  *phi = (xel / a) * (xel / a) + (yel / b) * (yel / b) - 1;
83 
84  // grad Phi
85  grad->nx = (2 * xel / (a * a)) * cosa + (2 * yel / (b * b)) * sina;
86  grad->ny = (2 * yel / (b * b)) * cosa - (2 * xel / (a * a)) * sina;
87 
88  // Hessian Phi
89  hess->H11 = 2 * (cosa * cosa / (a * a)
90  + sina * sina / (b * b));
91  hess->H12 = 2 * sina * cosa * (1 / (b * b) - 1 / (a * a));
92  hess->H21 = hess->H12;
93  hess->H22 = 2 * (sina * sina / (a * a)
94  + cosa * cosa / (b * b));
95 }
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
gvf_tra gvf_trajectory
Definition: gvf.c:42
float H12
Definition: gvf.h:108
float p[16]
Definition: gvf.h:79
float H21
Definition: gvf.h:110
float H11
Definition: gvf.h:107
float ny
Definition: gvf.h:102
float nx
Definition: gvf.h:101
float H22
Definition: gvf.h:111
Definition: gvf.h:106
Definition: gvf.h:100
#define GVF_ELLIPSE_A
Definition: gvf_ellipse.c:46
#define GVF_ELLIPSE_KE
Definition: gvf_ellipse.c:36
void gvf_ellipse_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_ellipse.c:63
gvf_ell_par gvf_ellipse_par
Definition: gvf_ellipse.c:59
#define GVF_ELLIPSE_KN
Definition: gvf_ellipse.c:41
#define GVF_ELLIPSE_B
Definition: gvf_ellipse.c:51
#define GVF_ELLIPSE_ALPHA
Definition: gvf_ellipse.c:56
Guidance algorithm based on vector fields 2D Ellipse trajectory.
static float p[2][2]
vector in East North Up coordinates Units: meters
float alpha
Definition: textons.c:133
float b
Definition: wedgebug.c:202