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_vertical_raster.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2013 The Paparazzi Team
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 
29 
31 #include "state.h"
32 #include "autopilot.h"
33 #include "generated/flight_plan.h"
34 
35 /************** Vertical Raster **********************************************/
36 
42 
43 bool_t nav_vertical_raster_setup( void ) {
44  line_status = LR12;
45  return FALSE;
46 }
47 
48 bool_t nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
49  radius = fabs(radius);
50  float alt = waypoints[l1].a;
51  waypoints[l2].a = alt;
52 
53  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
54  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
55  float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
56 
57  /* Unit vector from l1 to l2 */
58  float u_x = l2_l1_x / d;
59  float u_y = l2_l1_y / d;
60 
61  /* The half circle centers and the other leg */
62  struct point l2_c1 = { WaypointX(l1) + radius * u_y,
63  WaypointY(l1) + radius * -u_x,
64  alt };
65  struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
66  WaypointY(l1) + 1.732*radius * u_y,
67  alt };
68  struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
69  WaypointY(l1) + radius * u_x,
70  alt };
71 
72  struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
73  WaypointY(l2) + radius * u_x,
74  alt };
75  struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
76  WaypointY(l2) + 1.732*radius * -u_y,
77  alt };
78  struct point l1_c3 = { WaypointX(l2) + radius * u_y,
79  WaypointY(l2) + radius * -u_x,
80  alt };
81  float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
82 
83  float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
84  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
85 
86  /* Vertical target */
87  NavVerticalAutoThrottleMode(0); /* No pitch */
89 
90  switch (line_status) {
91  case LR12: /* From wp l2 to wp l1 */
92  NavSegment(l2, l1);
93  if (NavApproachingFrom(l1, l2, CARROT)) {
95  waypoints[l1].a = waypoints[l1].a+AltSweep;
97  }
98  break;
99  case LQC21:
100  nav_circle_XY(l2_c1.x, l2_c1.y, radius);
101  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
102  line_status = LTC2;
103  nav_init_stage();
104  }
105  break;
106  case LTC2:
107  nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
108  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a-10)) {
109  line_status = LQC22;
110  nav_init_stage();
111  }
112  break;
113  case LQC22:
114  nav_circle_XY(l2_c3.x, l2_c3.y, radius);
115  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
116  line_status = LR21;
117  nav_init_stage();
118  }
119  break;
120  case LR21: /* From wp l1 to wp l2 */
121  NavSegment(l1, l2);
122  if (NavApproachingFrom(l2, l1, CARROT)) {
123  line_status = LQC12;
124  waypoints[l1].a = waypoints[l1].a+AltSweep;
125  nav_init_stage();
126  }
127  break;
128  case LQC12:
129  nav_circle_XY(l1_c1.x, l1_c1.y, radius);
130  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
131  line_status = LTC1;
132  nav_init_stage();
133  }
134  break;
135  case LTC1:
136  nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
137  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a-5)) {
138  line_status = LQC11;
139  nav_init_stage();
140  }
141  break;
142  case LQC11:
143  nav_circle_XY(l1_c3.x, l1_c3.y, radius);
144  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
145  line_status = LR12;
146  nav_init_stage();
147  }
148  default:
149  break;
150  }
151  return TRUE; /* This pattern never ends */
152 }
float a
Definition: common_nav.h:42
static float radius
Definition: chemotaxis.c:15
#define FALSE
Definition: imu_chimu.h:141
#define TRUE
Definition: imu_chimu.h:144
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40