Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_line_border.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Tobias Muench
3  * modified nav_line by Anton Kochevar, ENAC
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
33 #include "generated/airframe.h"
35 
36 
39 
41 {
43  return false;
44 }
45 
47 {
48  radius = fabs(radius);
49  float alt = waypoints[l1].a;
50  waypoints[l2].a = alt;
51 
52  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
53  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
54  float d = sqrt(l2_l1_x * l2_l1_x + l2_l1_y * l2_l1_y);
55 
56  float u_x = l2_l1_x / d;
57  float u_y = l2_l1_y / d;
58 
59  float angle = atan2((WaypointY(l1) - WaypointY(l2)), (WaypointX(l2) - WaypointX(l1)));
60 
61  struct point l2_c1 = { WaypointX(l1) - sin(angle) *radius,
62  WaypointY(l1) - cos(angle) *radius,
63  alt
64  };
65  struct point l2_c2 = { l2_c1.x + 2 * radius * cos(angle) ,
66  l2_c1.y - 2 * radius * sin(angle),
67  alt
68  };
69 
70 
71 
72  struct point l1_c2 = { WaypointX(l2) - sin(angle) *radius,
73  WaypointY(l2) - cos(angle) *radius,
74  alt
75  };
76  struct point l1_c3 = { l1_c2.x - 2 * radius * cos(angle) ,
77  l1_c2.y + 2 * radius * sin(angle),
78  alt
79  };
80 
81  float qdr_out_2_1 = M_PI / 3. - atan2(u_y, u_x);
82  float qdr_out_2_2 = -M_PI / 3. - atan2(u_y, u_x);
83  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
84 
85 
88 
89  switch (line_border_status) {
90  case LR12:
91  NavSegment(l2, l1);
92  if (NavApproachingFrom(l1, l2, CARROT)) {
95  }
96  break;
97  case LQC21:
98  nav_circle_XY(l2_c1.x, l2_c1.y, -radius);
99  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2) - 10)) {
101  nav_init_stage();
102  }
103  break;
104  case LTC2:
105  nav_circle_XY(l2_c2.x, l2_c2.y, radius);
106  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3) - 10)) {
108  nav_init_stage();
109  }
110  break;
111 
112  case LR21:
113  NavSegment(l1, l2);
114  if (NavApproachingFrom(l2, l1, CARROT)) {
116  nav_init_stage();
117  }
118  break;
119 
120  case LTC1:
121  nav_circle_XY(l1_c2.x, l1_c2.y, radius);
122  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) + 10)) {
124  nav_init_stage();
125  }
126  break;
127  case LQC11:
128  nav_circle_XY(l1_c3.x, l1_c3.y, -radius);
129  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI + 10))) {
131  nav_init_stage();
132  }
133  break;
134 
135  default: /* Should not occur !!! End the pattern */
136  return false;
137  }
138  return true; /* This pattern never ends */
139 }
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float x
Definition: common_nav.h:40
static float radius
Definition: chemotaxis.c:15
float y
Definition: common_nav.h:41
#define WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointY(_wp)
Definition: common_nav.h:46
unsigned char uint8_t
Definition: types.h:14
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float a
Definition: common_nav.h:42