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
nav_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
23#include "nav_ellipse.h"
24
25#include "generated/airframe.h"
27
29#ifndef GVF_ELLIPSE_KE
30#define GVF_ELLIPSE_KE 1
31#endif
32
34#ifndef GVF_ELLIPSE_KN
35#define GVF_ELLIPSE_KN 1
36#endif
37
39#ifndef GVF_ELLIPSE_A
40#define GVF_ELLIPSE_A 80
41#endif
42
44#ifndef GVF_ELLIPSE_B
45#define GVF_ELLIPSE_B 80
46#endif
47
49#ifndef GVF_ELLIPSE_ALPHA
50#define GVF_ELLIPSE_ALPHA 0
51#endif
52
56
57// Param array lenght
58static int gvf_p_len_wps = 0;
59
62// ELLIPSE
63
64bool nav_gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
65{
66 float e;
69
71 gvf_trajectory.p[0] = x;
72 gvf_trajectory.p[1] = y;
73 gvf_trajectory.p[2] = a;
74 gvf_trajectory.p[3] = b;
77 gvf_p_len_wps = 0;
78
79 // SAFE MODE
80 if (a < 1 || b < 1) {
81 gvf_trajectory.p[2] = 60;
82 gvf_trajectory.p[3] = 60;
83 }
84
85 if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
87
88 } else {
90 }
91
96
98
99 return true;
100}
101
102
103bool nav_gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
104{
105 gvf_trajectory.p[5] = wp;
106 gvf_p_len_wps = 1;
107
109 return true;
110}
#define WaypointX(_wp)
Definition common_nav.h:45
#define WaypointY(_wp)
Definition common_nav.h:46
gvf_con gvf_control
Definition gvf.c:30
void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition gvf.c:100
Guidance algorithm based on vector fields.
float ke
Definition gvf.h:54
float error
Definition gvf.h:56
void gvf_ellipse_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition gvf_traj.c:53
gvf_tra gvf_trajectory
Definition gvf_traj.c:24
enum trajectories type
Definition gvf_traj.h:34
float p[16]
Definition gvf_traj.h:35
int p_len
Definition gvf_traj.h:36
uint16_t foo
Definition main_demo5.c:58
bool nav_gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition nav_ellipse.c:64
#define GVF_ELLIPSE_A
Definition nav_ellipse.c:40
#define GVF_ELLIPSE_KE
Definition nav_ellipse.c:30
gvf_ell_par gvf_ellipse_par
Definition nav_ellipse.c:53
#define GVF_ELLIPSE_KN
Definition nav_ellipse.c:35
#define GVF_ELLIPSE_B
Definition nav_ellipse.c:45
bool nav_gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
#define GVF_ELLIPSE_ALPHA
Definition nav_ellipse.c:50
static int gvf_p_len_wps
Definition nav_ellipse.c:58
float alpha
Definition textons.c:133
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
float b
Definition wedgebug.c:202