Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
gvf_line.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_line.h"
32 #include "generated/airframe.h"
33 
35 #ifndef GVF_LINE_KE
36 #define GVF_LINE_KE 1
37 #endif
38 
40 #ifndef GVF_LINE_KN
41 #define GVF_LINE_KN 1
42 #endif
43 
45 #ifndef GVF_LINE_HEADING
46 #define GVF_LINE_HEADING 0
47 #endif
48 
50 #ifndef GVF_SEGMENT_D1
51 #define GVF_SEGMENT_D1 0
52 #endif
53 
55 #ifndef GVF_SEGMENT_D2
56 #define GVF_SEGMENT_D2 0
57 #endif
58 
61 
62 void gvf_line_info(float *phi, struct gvf_grad *grad,
63  struct gvf_Hess *hess)
64 {
65 
66  struct EnuCoor_f *p = stateGetPositionEnu_f();
67  float px = p->x;
68  float py = p->y;
69  float a = gvf_trajectory.p[0];
70  float b = gvf_trajectory.p[1];
71  float alpha = gvf_trajectory.p[2];
72 
73  // Phi(x,y)
74  *phi = -(px - a) * cosf(alpha) + (py - b) * sinf(alpha);
75 
76  // grad Phi
77  grad->nx = -cosf(alpha);
78  grad->ny = sinf(alpha);
79 
80  // Hessian Phi
81  hess->H11 = 0;
82  hess->H12 = 0;
83  hess->H21 = 0;
84  hess->H22 = 0;
85 }
gvf_Hess::H22
float H22
Definition: gvf.h:92
GVF_SEGMENT_D1
#define GVF_SEGMENT_D1
Definition: gvf_line.c:51
b
float b
Definition: wedgebug.c:202
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
common_nav.h
GVF_LINE_KE
#define GVF_LINE_KE
Definition: gvf_line.c:36
alpha
float alpha
Definition: textons.c:107
gvf_grad::ny
float ny
Definition: gvf.h:83
gvf_segment_par
gvf_seg_par gvf_segment_par
Definition: gvf_line.c:60
gvf_li_par
Definition: gvf_line.h:40
gvf_line_par
gvf_li_par gvf_line_par
Definition: gvf_line.c:59
gvf_grad::nx
float nx
Definition: gvf.h:82
gvf_tra::p
float p[16]
Definition: gvf.h:60
gvf_trajectory
gvf_tra gvf_trajectory
Definition: gvf.c:40
gvf_seg_par
Definition: gvf_line.h:51
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
GVF_LINE_HEADING
#define GVF_LINE_HEADING
Definition: gvf_line.c:46
GVF_SEGMENT_D2
#define GVF_SEGMENT_D2
Definition: gvf_line.c:56
gvf_line.h
gvf_Hess::H12
float H12
Definition: gvf.h:89
gvf_Hess
Definition: gvf.h:87
gvf_line_info
void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_line.c:62
gvf_Hess::H11
float H11
Definition: gvf.h:88
gvf_grad
Definition: gvf.h:81
GVF_LINE_KN
#define GVF_LINE_KN
Definition: gvf_line.c:41
gvf_Hess::H21
float H21
Definition: gvf.h:91
p
static float p[2][2]
Definition: ins_alt_float.c:268