Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_cube.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Martin Mueller
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 #include "generated/airframe.h"
30 #include "modules/nav/nav_cube.h"
32 #include "generated/flight_plan.h"
33 
34 #define MAX_LINES_X 8
35 #define STBY_OFFSET 500
36 
38 
39 void nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te)
40 {
41 
42  int32_t j, start_bx, start_by, start_bz, start_ex, start_ey, start_ez;
43  int32_t bx, by, ex, ey;
44  float alpha, cos_alpha, sin_alpha;
45  int32_t cube_nline_x_t, cube_nline_z_t;
46  int32_t cube_pos_x, cube_pos_z;
47  int32_t cube_line_x_start, cube_line_x_end;
48  int32_t cube_line_z_start, cube_line_z_end;
49 
50  /* sanity checks */
51  if (nav_cube.nsect_x <= 0) {
52  nav_cube.nsect_x = 1;
53  }
54  if (nav_cube.nsect_z <= 0) {
55  nav_cube.nsect_z = 1;
56  }
57  if ((nav_cube.sect <= 0) || (nav_cube.sect > (nav_cube.nsect_x * nav_cube.nsect_z))) {
58  nav_cube.sect = 1;
59  }
60 
61  /* total number of lines/layers to fly */
62  if (nav_cube.grid_x == 0) {
63  cube_nline_x_t = 1;
64  } else {
65  cube_nline_x_t = nav_cube.size.x / nav_cube.grid_x + 1;
66  }
67  if (nav_cube.grid_z == 0) {
68  cube_nline_z_t = 1;
69  } else {
70  cube_nline_z_t = nav_cube.size.z / nav_cube.grid_z + 1;
71  }
72 
73  /* position and number of lines in this sector */
74  cube_pos_x = (nav_cube.sect - 1) % nav_cube.nsect_x;
75  cube_line_x_start = (cube_pos_x * cube_nline_x_t) / nav_cube.nsect_x;
76  cube_line_x_end = ((cube_pos_x + 1) * cube_nline_x_t) / nav_cube.nsect_x;
77  if (cube_line_x_end > cube_nline_x_t) {
78  cube_line_x_end = cube_nline_x_t;
79  }
80  nav_cube.nline_x = cube_line_x_end - cube_line_x_start;
81 
82  /* do not do more than pre-set number of lines */
84 
85  /* position and number of layers in this sector */
86  cube_pos_z = (nav_cube.sect - 1) / nav_cube.nsect_x;
87  cube_line_z_start = (cube_pos_z * cube_nline_z_t) / nav_cube.nsect_z;
88  cube_line_z_end = ((cube_pos_z + 1) * cube_nline_z_t) / nav_cube.nsect_z;
89  nav_cube.nline_z = cube_line_z_end - cube_line_z_start;
90 
91  /* do the costly stuff only once */
92  alpha = ((360. - nav_cube.alpha) / 360.) * 2 * M_PI;
93  cos_alpha = cos(alpha);
94  sin_alpha = sin(alpha);
95 
96  /* calculate lower left start begin/end x coord */
97  start_bx = WaypointX(center) - (((cube_nline_x_t - 1) * nav_cube.grid_x) / 2)
98  + nav_cube.offset.x;
99  start_ex = start_bx;
100 
101  /* calculate lower left start end point y coord */
102  start_ey = WaypointY(center) - nav_cube.offset.y;
103 
104  /* calculate lower left start begin point y coord */
105  start_by = start_ey - nav_cube.size.y;
106 
107  /* calculate lower left start begin/end z coord */
108  start_bz = waypoints[center].a - (((cube_nline_z_t - 1) * nav_cube.grid_z) / 2)
109  + (cube_line_z_start * nav_cube.grid_z) + nav_cube.offset.z;
110  start_ez = start_bz;
111 
112  /* reset all waypoints to the standby position */
113  for (j = 0; j < MAX_LINES_X; j++) {
114  waypoints[tb + j].x = WaypointX(center) + STBY_OFFSET;
115  waypoints[tb + j].y = WaypointY(center);
116  waypoints[te + j].x = WaypointX(center) + STBY_OFFSET;
117  waypoints[te + j].y = WaypointY(center);
118  }
119 
120  /* set used waypoints */
121  for (j = 0; j < nav_cube.nline_x; j++) {
122  int i = cube_line_x_start + j;
123  /* set waypoints and vectorize in regard to center */
124  bx = (start_bx + i * nav_cube.grid_x) - WaypointX(center);
125  by = start_by - WaypointY(center);
126  ex = (start_ex + i * nav_cube.grid_x) - WaypointX(center);
127  ey = start_ey - WaypointY(center);
128  /* rotate clockwise with alpha and un-vectorize*/
129  waypoints[tb + j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center);
130  waypoints[tb + j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center);
131  waypoints[tb + j].a = start_bz;
132  waypoints[te + j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center);
133  waypoints[te + j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center);
134  waypoints[te + j].a = start_ez;
135  }
136 
137  /* bug in <for from="" to=""> ? */
138  nav_cube.nline_x--;
139  nav_cube.nline_z--;
140 }
141 
143  uint8_t dest_b, uint8_t dest_e,
144  uint8_t src_b, uint8_t src_e)
145 {
146 
147  if (i > nav_cube.nline_x) {
148  return false;
149  }
150  if (j > nav_cube.nline_z) {
151  return false;
152  }
153 
154  waypoints[dest_b].x = waypoints[src_b + i].x;
155  waypoints[dest_b].y = waypoints[src_b + i].y;
156  waypoints[dest_b].a = waypoints[src_b + i].a + j * nav_cube.grid_z;
157  /* always keep at least security altitude */
158  if (waypoints[dest_b].a < (ground_alt + SECURITY_HEIGHT)) {
159  waypoints[dest_b].a = ground_alt + SECURITY_HEIGHT;
160  }
161 
162  waypoints[dest_e].x = waypoints[src_e + i].x;
163  waypoints[dest_e].y = waypoints[src_e + i].y;
164  waypoints[dest_e].a = waypoints[src_e + i].a + j * nav_cube.grid_z;
165  /* always keep at least security altitude */
166  if (waypoints[dest_e].a < (ground_alt + SECURITY_HEIGHT)) {
167  waypoints[dest_e].a = ground_alt + SECURITY_HEIGHT;
168  }
169 
170  return false;
171 }
float x
Definition: common_nav.h:40
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
float alpha
Definition: textons.c:107
float y
Definition: common_nav.h:41
#define WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointY(_wp)
Definition: common_nav.h:46
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float a
Definition: common_nav.h:42
signed char int8_t
Definition: types.h:15