Paparazzi UAS  v5.15_devel-112-g521f3cf
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 }
44 
45 bool nav_line_border_run(uint8_t l1, uint8_t l2, float radius)
46 {
47  radius = fabs(radius);
48  float alt = waypoints[l1].a;
49  waypoints[l2].a = alt;
50 
51  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
52  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
53  float d = sqrt(l2_l1_x * l2_l1_x + l2_l1_y * l2_l1_y);
54 
55  float u_x = l2_l1_x / d;
56  float u_y = l2_l1_y / d;
57 
58  float angle = atan2((WaypointY(l1) - WaypointY(l2)), (WaypointX(l2) - WaypointX(l1)));
59 
60  struct point l2_c1 = { WaypointX(l1) - sin(angle) *radius,
61  WaypointY(l1) - cos(angle) *radius,
62  alt
63  };
64  struct point l2_c2 = { l2_c1.x + 2 * radius * cos(angle) ,
65  l2_c1.y - 2 * radius * sin(angle),
66  alt
67  };
68 
69 
70 
71  struct point l1_c2 = { WaypointX(l2) - sin(angle) *radius,
72  WaypointY(l2) - cos(angle) *radius,
73  alt
74  };
75  struct point l1_c3 = { l1_c2.x - 2 * radius * cos(angle) ,
76  l1_c2.y + 2 * radius * sin(angle),
77  alt
78  };
79 
80  float qdr_out_2_1 = M_PI / 3. - atan2(u_y, u_x);
81  float qdr_out_2_2 = -M_PI / 3. - atan2(u_y, u_x);
82  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
83 
84 
87 
88  switch (line_border_status) {
89  case LR12:
90  NavSegment(l2, l1);
91  if (NavApproachingFrom(l1, l2, CARROT)) {
94  }
95  break;
96  case LQC21:
97  nav_circle_XY(l2_c1.x, l2_c1.y, -radius);
98  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2) - 10)) {
100  nav_init_stage();
101  }
102  break;
103  case LTC2:
104  nav_circle_XY(l2_c2.x, l2_c2.y, radius);
105  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3) - 10)) {
107  nav_init_stage();
108  }
109  break;
110 
111  case LR21:
112  NavSegment(l1, l2);
113  if (NavApproachingFrom(l2, l1, CARROT)) {
115  nav_init_stage();
116  }
117  break;
118 
119  case LTC1:
120  nav_circle_XY(l1_c2.x, l1_c2.y, radius);
121  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) + 10)) {
123  nav_init_stage();
124  }
125  break;
126  case LQC11:
127  nav_circle_XY(l1_c3.x, l1_c3.y, -radius);
128  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI + 10))) {
130  nav_init_stage();
131  }
132  break;
133 
134  default: /* Should not occur !!! End the pattern */
135  return false;
136  }
137  return true; /* This pattern never ends */
138 }
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float x
Definition: common_nav.h:40
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
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40