Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gvf_traj.c
Go to the documentation of this file.
1/*
2 * This file is part of paparazzi.
3 *
4 * paparazzi is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2, or (at your option)
7 * any later version.
8 *
9 * paparazzi is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with paparazzi; see the file COPYING. If not, write to
16 * the Free Software Foundation, 59 Temple Place - Suite 330,
17 * Boston, MA 02111-1307, USA.
18 *
19 */
20
21#include "gvf_traj.h"
22
23// Trajectory
26
29void gvf_line_info(float *phi, struct gvf_grad *grad,
30 struct gvf_Hess *hess)
31{
33 float px = p->x;
34 float py = p->y;
35 float a = gvf_trajectory.p[0];
36 float b = gvf_trajectory.p[1];
37 float alpha = gvf_trajectory.p[2];
38
39 // Phi(x,y)
40 *phi = -(px - a) * cosf(alpha) + (py - b) * sinf(alpha);
41
42 // grad Phi
43 grad->nx = -cosf(alpha);
44 grad->ny = sinf(alpha);
45
46 // Hessian Phi
47 hess->H11 = 0;
48 hess->H12 = 0;
49 hess->H21 = 0;
50 hess->H22 = 0;
51}
52
53void gvf_ellipse_info(float *phi, struct gvf_grad *grad,
54 struct gvf_Hess *hess)
55{
57 float px = p->x;
58 float py = p->y;
59 float wx = gvf_trajectory.p[0];
60 float wy = gvf_trajectory.p[1];
61 float a = gvf_trajectory.p[2];
62 float b = gvf_trajectory.p[3];
63 float alpha = gvf_trajectory.p[4];
64
65 float cosa = cosf(alpha);
66 float sina = sinf(alpha);
67
68 // Phi(x,y)
69 float xel = (px - wx) * cosa - (py - wy) * sina;
70 float yel = (px - wx) * sina + (py - wy) * cosa;
71 *phi = (xel / a) * (xel / a) + (yel / b) * (yel / b) - 1;
72
73 // grad Phi
74 grad->nx = (2 * xel / (a * a)) * cosa + (2 * yel / (b * b)) * sina;
75 grad->ny = (2 * yel / (b * b)) * cosa - (2 * xel / (a * a)) * sina;
76
77 // Hessian Phi
78 hess->H11 = 2 * (cosa * cosa / (a * a)
79 + sina * sina / (b * b));
80 hess->H12 = 2 * sina * cosa * (1 / (b * b) - 1 / (a * a));
81 hess->H21 = hess->H12;
82 hess->H22 = 2 * (sina * sina / (a * a)
83 + cosa * cosa / (b * b));
84}
85
86void gvf_sin_info(float *phi, struct gvf_grad *grad,
87 struct gvf_Hess *hess)
88{
90 float px = p->x;
91 float py = p->y;
92 float a = gvf_trajectory.p[0];
93 float b = gvf_trajectory.p[1];
94 float alpha = gvf_trajectory.p[2];
95 float w = gvf_trajectory.p[3];
96 float off = gvf_trajectory.p[4];
97 float A = gvf_trajectory.p[5];
98
99 float cosa = cosf(alpha);
100 float sina = sinf(alpha);
101
102 // Phi(x,y)
103 float xs = (px - a) * sina - (py - b) * cosa;
104 float ys = -(px - a) * cosa - (py - b) * sina;
105
106 // TODO Make it always in (-pi, pi] in an efficient way
107 float ang = (w * xs + off);
108 float cosang = cosf(ang);
109 float sinang = sinf(ang);
110
111 *phi = ys - A * sinang;
112
113 // grad Phi
114 grad->nx = -cosa - A * w * sina * cosang;
115 grad->ny = -sina + A * w * cosa * cosang;
116
117 // Hessian Phi
118 hess->H11 = A * w * w * sina * sina * sinang;
119 hess->H12 = -A * w * w * sina * cosa * sinang;
120 hess->H21 = -A * w * w * cosa * sina * sinang;
121 hess->H22 = A * w * w * cosa * cosa * sinang;
122}
#define A
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
void gvf_ellipse_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition gvf_traj.c:53
void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition gvf_traj.c:29
gvf_seg gvf_segment
Definition gvf_traj.c:25
void gvf_sin_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition gvf_traj.c:86
gvf_tra gvf_trajectory
Definition gvf_traj.c:24
float p[16]
Definition gvf_traj.h:35
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
vector in East North Up coordinates Units: meters
float alpha
Definition textons.c:133
float b
Definition wedgebug.c:202