Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
nav_line_border.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Tobias Muench
3  * modified nav_line by Anton Kochevar, ENAC
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
33 #include "generated/airframe.h"
35 
36 
39 
41 {
43 }
44 
45 bool nav_line_border_run(uint8_t l1, uint8_t l2, float radius)
46 {
47  radius = fabs(radius);
48  float alt = waypoints[l1].a;
49  waypoints[l2].a = alt;
50 
51  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
52  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
53  float d = sqrt(l2_l1_x * l2_l1_x + l2_l1_y * l2_l1_y);
54 
55  float u_x = l2_l1_x / d;
56  float u_y = l2_l1_y / d;
57 
58  float angle = atan2((WaypointY(l1) - WaypointY(l2)), (WaypointX(l2) - WaypointX(l1)));
59 
60  struct point l2_c1 = { WaypointX(l1) - sin(angle) *radius,
61  WaypointY(l1) - cos(angle) *radius,
62  alt
63  };
64  struct point l2_c2 = { l2_c1.x + 2 * radius * cos(angle) ,
65  l2_c1.y - 2 * radius * sin(angle),
66  alt
67  };
68 
69 
70 
71  struct point l1_c2 = { WaypointX(l2) - sin(angle) *radius,
72  WaypointY(l2) - cos(angle) *radius,
73  alt
74  };
75  struct point l1_c3 = { l1_c2.x - 2 * radius * cos(angle) ,
76  l1_c2.y + 2 * radius * sin(angle),
77  alt
78  };
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 (line_border_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 }
point::a
float a
Definition: common_nav.h:42
nav_line_border_run
bool nav_line_border_run(uint8_t l1, uint8_t l2, float radius)
Definition: nav_line_border.c:45
nav_line_border_setup
void nav_line_border_setup(void)
Definition: nav_line_border.c:40
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
nav_line_border.h
navigate along a border line (line 1-2) with turns in the same direction
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
LR21
@ LR21
Definition: nav_line_border.c:37
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:109
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
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_line_border.c:37
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
LTC1
@ LTC1
Definition: nav_line_border.c:37
LQC22
@ LQC22
Definition: nav_line_border.c:37
LQC21
@ LQC21
Definition: nav_line_border.c:37
LTC2
@ LTC2
Definition: nav_line_border.c:37
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
LQC12
@ LQC12
Definition: nav_line_border.c:37
nav.h
WaypointAlt
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
line_border_status
line_border_status
Definition: nav_line_border.c:37
LQC11
@ LQC11
Definition: nav_line_border.c:37
point::y
float y
Definition: common_nav.h:41
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