Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
border_line.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2012 Tobias Muench
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
25 
26 //modified nav_linie by Anton Kochevar, ENAC
27 
38 #include "generated/airframe.h"
39 #include "subsystems/nav.h"
40 
41 
44 
45 bool_t border_line_init( void ) {
47  return FALSE;
48 }
49 
50 bool_t border_line(uint8_t l1, uint8_t l2, float radius) {
51  radius = fabs(radius);
52  float alt = waypoints[l1].a;
53  waypoints[l2].a = alt;
54 
55  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
56  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
57  float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
58 
59  float u_x = l2_l1_x / d;
60  float u_y = l2_l1_y / d;
61 
62  float angle = atan2((WaypointY(l1)-WaypointY(l2)),(WaypointX(l2)-WaypointX(l1)));
63 
64  struct point l2_c1 = { WaypointX(l1) - sin(angle)*radius,
65  WaypointY(l1) - cos(angle)*radius,
66  alt };
67  struct point l2_c2 = { l2_c1.x + 2*radius*cos(angle) ,
68  l2_c1.y - 2*radius*sin(angle),
69  alt };
70 
71 
72 
73  struct point l1_c2 = { WaypointX(l2) - sin(angle)*radius,
74  WaypointY(l2) - cos(angle)*radius,
75  alt };
76  struct point l1_c3 = { l1_c2.x - 2*radius*cos(angle) ,
77  l1_c2.y + 2*radius*sin(angle),
78  alt };
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 (border_line_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 }
static float radius
Definition: chemotaxis.c:15
bool_t border_line(uint8_t l1, uint8_t l2, float radius)
Definition: border_line.c:50
#define FALSE
Definition: imu_chimu.h:141
navigate along a border line (line 1-2) with turns in the same direction
bool_t border_line_init(void)
Definition: border_line.c:45
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
float y
Definition: common_nav.h:38
border_line_status
Definition: border_line.c:42
float x
Definition: common_nav.h:37