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.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2007 Anton Kochevar, ENAC
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 
28 #include "generated/airframe.h"
29 #include "modules/nav/nav_line.h"
31 
35 
36 bool_t nav_line_setup( void ) {
37  line_status = LR12;
38  return FALSE;
39 }
40 
41 bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) {
42  radius = fabs(radius);
43  float alt = waypoints[l1].a;
44  waypoints[l2].a = alt;
45 
46  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
47  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
48  float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
49 
50  /* Unit vector from l1 to l2 */
51  float u_x = l2_l1_x / d;
52  float u_y = l2_l1_y / d;
53 
54  /* The half circle centers and the other leg */
55  struct point l2_c1 = { WaypointX(l1) + radius * u_y,
56  WaypointY(l1) + radius * -u_x,
57  alt };
58  struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
59  WaypointY(l1) + 1.732*radius * u_y,
60  alt };
61  struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
62  WaypointY(l1) + radius * u_x,
63  alt };
64 
65  struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
66  WaypointY(l2) + radius * u_x,
67  alt };
68  struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
69  WaypointY(l2) + 1.732*radius * -u_y,
70  alt };
71  struct point l1_c3 = { WaypointX(l2) + radius * u_y,
72  WaypointY(l2) + radius * -u_x,
73  alt };
74  float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
75 
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  /* Vertical target */
80  NavVerticalAutoThrottleMode(0); /* No pitch */
82 
83  switch (line_status) {
84  case LR12: /* From wp l2 to wp l1 */
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_1)-10)) {
94  line_status = LTC2;
96  }
97  break;
98  case LTC2:
99  nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
100  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10)) {
101  line_status = LQC22;
102  nav_init_stage();
103  }
104  break;
105  case LQC22:
106  nav_circle_XY(l2_c3.x, l2_c3.y, radius);
107  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
108  line_status = LR21;
109  nav_init_stage();
110  }
111  break;
112  case LR21: /* From wp l1 to wp l2 */
113  NavSegment(l1, l2);
114  if (NavApproachingFrom(l2, l1, CARROT)) {
115  line_status = LQC12;
116  nav_init_stage();
117  }
118  break;
119  case LQC12:
120  nav_circle_XY(l1_c1.x, l1_c1.y, radius);
121  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
122  line_status = LTC1;
123  nav_init_stage();
124  }
125  break;
126  case LTC1:
127  nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
128  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10)) {
129  line_status = LQC11;
130  nav_init_stage();
131  }
132  break;
133  case LQC11:
134  nav_circle_XY(l1_c3.x, l1_c3.y, radius);
135  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
136  line_status = LR12;
137  nav_init_stage();
138  }
139  break;
140  default: /* Should not occur !!! End the pattern */
141  return FALSE;
142  }
143  return TRUE; /* This pattern never ends */
144 }
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