Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
gvf_sin.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 
31 #include "gvf_sin.h"
32 #include "generated/airframe.h"
33 
35 #ifndef GVF_SIN_KE
36 #define GVF_SIN_KE 1
37 #endif
38 
40 #ifndef GVF_SIN_KN
41 #define GVF_SIN_KN 1
42 #endif
43 
45 #ifndef GVF_SIN_ALPHA
46 #define GVF_SIN_ALPHA 0
47 #endif
48 
50 #ifndef GVF_SIN_W
51 #define GVF_SIN_W 0
52 #endif
53 
55 #ifndef GVF_SIN_OFF
56 #define GVF_SIN_OFF 0
57 #endif
58 
60 #ifndef GVF_SIN_A
61 #define GVF_SIN_A 0
62 #endif
63 
66  };
67 
68 
69 void gvf_sin_info(float *phi, struct gvf_grad *grad,
70  struct gvf_Hess *hess)
71 {
72 
73  struct EnuCoor_f *p = stateGetPositionEnu_f();
74  float px = p->x;
75  float py = p->y;
76  float a = gvf_trajectory.p[0];
77  float b = gvf_trajectory.p[1];
78  float alpha = gvf_trajectory.p[2];
79  float w = gvf_trajectory.p[3];
80  float off = gvf_trajectory.p[4];
81  float A = gvf_trajectory.p[5];
82 
83  float cosa = cosf(alpha);
84  float sina = sinf(alpha);
85 
86  // Phi(x,y)
87  float xs = (px - a) * sina - (py - b) * cosa;
88  float ys = -(px - a) * cosa - (py - b) * sina;
89 
90  // TODO Make it always in (-pi, pi] in an efficient way
91  float ang = (w * xs + off);
92  float cosang = cosf(ang);
93  float sinang = sinf(ang);
94 
95  *phi = ys - A * sinang;
96 
97  // grad Phi
98  grad->nx = -cosa - A * w * sina * cosang;
99  grad->ny = -sina + A * w * cosa * cosang;
100 
101  // Hessian Phi
102  hess->H11 = A * w * w * sina * sina * sinang;
103  hess->H12 = -A * w * w * sina * cosa * sinang;
104  hess->H21 = -A * w * w * cosa * sina * sinang;
105  hess->H22 = A * w * w * cosa * cosa * sinang;
106 }
gvf_Hess::H22
float H22
Definition: gvf.h:92
b
float b
Definition: wedgebug.c:202
GVF_SIN_W
#define GVF_SIN_W
Definition: gvf_sin.c:51
gvf_sin_info
void gvf_sin_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_sin.c:69
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
common_nav.h
alpha
float alpha
Definition: textons.c:107
gvf_sin.h
GVF_SIN_KN
#define GVF_SIN_KN
Definition: gvf_sin.c:41
gvf_grad::ny
float ny
Definition: gvf.h:83
gvf_sin_par
gvf_s_par gvf_sin_par
Definition: gvf_sin.c:64
gvf_grad::nx
float nx
Definition: gvf.h:82
gvf_tra::p
float p[16]
Definition: gvf.h:60
A
#define A
Definition: pprz_geodetic_utm.h:44
gvf_trajectory
gvf_tra gvf_trajectory
Definition: gvf.c:40
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
gvf_Hess::H12
float H12
Definition: gvf.h:89
gvf_s_par
Definition: gvf_sin.h:43
gvf_Hess
Definition: gvf.h:87
GVF_SIN_ALPHA
#define GVF_SIN_ALPHA
Definition: gvf_sin.c:46
GVF_SIN_OFF
#define GVF_SIN_OFF
Definition: gvf_sin.c:56
GVF_SIN_A
#define GVF_SIN_A
Definition: gvf_sin.c:61
gvf_Hess::H11
float H11
Definition: gvf.h:88
gvf_grad
Definition: gvf.h:81
gvf_Hess::H21
float H21
Definition: gvf.h:91
p
static float p[2][2]
Definition: ins_alt_float.c:268
GVF_SIN_KE
#define GVF_SIN_KE
Definition: gvf_sin.c:36