Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
44 {
45  line_status = LR12;
46 }
47 
48 bool nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSweep)
49 {
50  radius = fabs(radius);
51  float alt = waypoints[l1].a;
52  waypoints[l2].a = alt;
53 
54  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
55  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
56  float d = sqrt(l2_l1_x * l2_l1_x + l2_l1_y * l2_l1_y);
57 
58  /* Unit vector from l1 to l2 */
59  float u_x = l2_l1_x / d;
60  float u_y = l2_l1_y / d;
61 
62  /* The half circle centers and the other leg */
63  struct point l2_c1 = { WaypointX(l1) + radius * u_y,
64  WaypointY(l1) + radius * -u_x,
65  alt
66  };
67  struct point l2_c2 = { WaypointX(l1) + 1.732 * radius * u_x,
68  WaypointY(l1) + 1.732 * radius * u_y,
69  alt
70  };
71  struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
72  WaypointY(l1) + radius * u_x,
73  alt
74  };
75 
76  struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
77  WaypointY(l2) + radius * u_x,
78  alt
79  };
80  struct point l1_c2 = { WaypointX(l2) + 1.732 * radius * -u_x,
81  WaypointY(l2) + 1.732 * radius * -u_y,
82  alt
83  };
84  struct point l1_c3 = { WaypointX(l2) + radius * u_y,
85  WaypointY(l2) + radius * -u_x,
86  alt
87  };
88  float qdr_out_2_1 = M_PI / 3. - atan2(u_y, u_x);
89 
90  float qdr_out_2_2 = -M_PI / 3. - atan2(u_y, u_x);
91  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
92 
93  /* Vertical target */
94  NavVerticalAutoThrottleMode(0); /* No pitch */
96 
97  switch (line_status) {
98  case LR12: /* From wp l2 to wp l1 */
99  NavSegment(l2, l1);
100  if (NavApproachingFrom(l1, l2, CARROT)) {
101  line_status = LQC21;
102  waypoints[l1].a = waypoints[l1].a + AltSweep;
103  nav_init_stage();
104  }
105  break;
106  case LQC21:
107  nav_circle_XY(l2_c1.x, l2_c1.y, radius);
108  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) - 10)) {
109  line_status = LTC2;
110  nav_init_stage();
111  }
112  break;
113  case LTC2:
114  nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
115  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2) + 10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a - 10)) {
116  line_status = LQC22;
117  nav_init_stage();
118  }
119  break;
120  case LQC22:
121  nav_circle_XY(l2_c3.x, l2_c3.y, radius);
122  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3) - 10)) {
123  line_status = LR21;
124  nav_init_stage();
125  }
126  break;
127  case LR21: /* From wp l1 to wp l2 */
128  NavSegment(l1, l2);
129  if (NavApproachingFrom(l2, l1, CARROT)) {
130  line_status = LQC12;
131  waypoints[l1].a = waypoints[l1].a + AltSweep;
132  nav_init_stage();
133  }
134  break;
135  case LQC12:
136  nav_circle_XY(l1_c1.x, l1_c1.y, radius);
137  if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI) - 10)) {
138  line_status = LTC1;
139  nav_init_stage();
140  }
141  break;
142  case LTC1:
143  nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
144  if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI) + 10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a - 5)) {
145  line_status = LQC11;
146  nav_init_stage();
147  }
148  break;
149  case LQC11:
150  nav_circle_XY(l1_c3.x, l1_c3.y, radius);
151  if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI) - 10)) {
152  line_status = LR12;
153  nav_init_stage();
154  }
155  default:
156  break;
157  }
158  return true; /* This pattern never ends */
159 }
point::a
float a
Definition: common_nav.h:42
LQC21
@ LQC21
Definition: nav_vertical_raster.c:40
line_status
line_status
Copy of nav line.
Definition: nav_vertical_raster.c:40
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:109
LQC12
@ LQC12
Definition: nav_vertical_raster.c:40
nav_vertical_raster.h
LR21
@ LR21
Definition: nav_vertical_raster.c:40
NavSegment
#define NavSegment(_start, _end)
Definition: nav.h:161
NavQdrCloseTo
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:155
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
point
Definition: common_nav.h:39
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
LR12
@ LR12
Definition: nav_vertical_raster.c:40
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:171
NavApproachingFrom
#define NavApproachingFrom(wp, from, time)
Definition: nav.h:166
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
nav_vertical_raster_run
bool nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSweep)
Definition: nav_vertical_raster.c:48
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:93
uint8_t
unsigned char uint8_t
Definition: types.h:14
point::x
float x
Definition: common_nav.h:40
autopilot.h
nav_vertical_raster_setup
void nav_vertical_raster_setup(void)
Definition: nav_vertical_raster.c:43
nav.h
WaypointAlt
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
line_status
line_status
Status along the pattern.
Definition: nav_line.c:33
LTC1
@ LTC1
Definition: nav_vertical_raster.c:40
point::y
float y
Definition: common_nav.h:41
LQC11
@ LQC11
Definition: nav_vertical_raster.c:40
LTC2
@ LTC2
Definition: nav_vertical_raster.c:40
state.h
LQC22
@ LQC22
Definition: nav_vertical_raster.c:40
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:185