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