Paparazzi UAS  v7.0_unstable
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 
30 #include "modules/nav/common_nav.h"
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 }
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:103
float p[16]
Definition: gvf.h:74
float H21
Definition: gvf.h:105
float H11
Definition: gvf.h:102
float ny
Definition: gvf.h:97
float nx
Definition: gvf.h:96
float H22
Definition: gvf.h:106
Definition: gvf.h:101
Definition: gvf.h:95
#define GVF_LINE_HEADING
Definition: gvf_line.c:46
#define GVF_LINE_KE
Definition: gvf_line.c:36
#define GVF_SEGMENT_D2
Definition: gvf_line.c:56
void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition: gvf_line.c:62
#define GVF_SEGMENT_D1
Definition: gvf_line.c:51
#define GVF_LINE_KN
Definition: gvf_line.c:41
gvf_seg_par gvf_segment_par
Definition: gvf_line.c:60
gvf_li_par gvf_line_par
Definition: gvf_line.c:59
Guidance algorithm based on vector fields 2D straight line 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