Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
40 bool_t nav_line_border_setup( void ) {
42  return FALSE;
43 }
44 
45 bool_t nav_line_border_run(uint8_t l1, uint8_t l2, float radius) {
46  radius = fabs(radius);
47  float alt = waypoints[l1].a;
48  waypoints[l2].a = alt;
49 
50  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
51  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
52  float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
53 
54  float u_x = l2_l1_x / d;
55  float u_y = l2_l1_y / d;
56 
57  float angle = atan2((WaypointY(l1)-WaypointY(l2)),(WaypointX(l2)-WaypointX(l1)));
58 
59  struct point l2_c1 = { WaypointX(l1) - sin(angle)*radius,
60  WaypointY(l1) - cos(angle)*radius,
61  alt };
62  struct point l2_c2 = { l2_c1.x + 2*radius*cos(angle) ,
63  l2_c1.y - 2*radius*sin(angle),
64  alt };
65 
66 
67 
68  struct point l1_c2 = { WaypointX(l2) - sin(angle)*radius,
69  WaypointY(l2) - cos(angle)*radius,
70  alt };
71  struct point l1_c3 = { l1_c2.x - 2*radius*cos(angle) ,
72  l1_c2.y + 2*radius*sin(angle),
73  alt };
74 
75  float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
76  float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
77  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
78 
79 
82 
83  switch (line_border_status) {
84  case LR12:
85  NavSegment(l2, l1);
86  if (NavApproachingFrom(l1, l2, CARROT)) {
89  }
90  break;
91  case LQC21:
92  nav_circle_XY(l2_c1.x, l2_c1.y, -radius);
93  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)-10)) {
96  }
97  break;
98  case LTC2:
99  nav_circle_XY(l2_c2.x, l2_c2.y, radius);
100  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
102  nav_init_stage();
103  }
104  break;
105 
106  case LR21:
107  NavSegment(l1, l2);
108  if (NavApproachingFrom(l2, l1, CARROT)) {
110  nav_init_stage();
111  }
112  break;
113 
114  case LTC1:
115  nav_circle_XY(l1_c2.x, l1_c2.y, radius);
116  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) + 10)) {
118  nav_init_stage();
119  }
120  break;
121  case LQC11:
122  nav_circle_XY(l1_c3.x, l1_c3.y, -radius);
123  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI + 10))) {
125  nav_init_stage();
126  }
127  break;
128 
129  default: /* Should not occur !!! End the pattern */
130  return FALSE;
131  }
132  return TRUE; /* This pattern never ends */
133 }
static float radius
Definition: chemotaxis.c:15
#define FALSE
Definition: imu_chimu.h:141
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40