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_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 bool_t nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te) {
40 
41  int32_t j, start_bx, start_by, start_bz, start_ex, start_ey, start_ez;
42  int32_t bx, by, ex, ey;
43  float alpha, cos_alpha, sin_alpha;
44  int32_t cube_nline_x_t, cube_nline_z_t;
45  int32_t cube_pos_x, cube_pos_z;
46  int32_t cube_line_x_start, cube_line_x_end;
47  int32_t cube_line_z_start, cube_line_z_end;
48 
49  /* sanity checks */
50  if (nav_cube.nsect_x <= 0)
51  nav_cube.nsect_x = 1;
52  if (nav_cube.nsect_z <= 0)
53  nav_cube.nsect_z = 1;
55  nav_cube.sect = 1;
56 
57  /* total number of lines/layers to fly */
58  if (nav_cube.grid_x == 0)
59  cube_nline_x_t = 1;
60  else
61  cube_nline_x_t = nav_cube.size.x / nav_cube.grid_x + 1;
62  if (nav_cube.grid_z == 0)
63  cube_nline_z_t = 1;
64  else
65  cube_nline_z_t = nav_cube.size.z / nav_cube.grid_z + 1;
66 
67  /* position and number of lines in this sector */
68  cube_pos_x = (nav_cube.sect-1) % nav_cube.nsect_x;
69  cube_line_x_start = (cube_pos_x * cube_nline_x_t)/nav_cube.nsect_x;
70  cube_line_x_end = ((cube_pos_x+1) * cube_nline_x_t)/nav_cube.nsect_x;
71  if (cube_line_x_end > cube_nline_x_t)
72  cube_line_x_end = cube_nline_x_t;
73  nav_cube.nline_x = cube_line_x_end - cube_line_x_start;
74 
75  /* do not do more than pre-set number of lines */
77 
78  /* position and number of layers in this sector */
79  cube_pos_z = (nav_cube.sect-1) / nav_cube.nsect_x;
80  cube_line_z_start = (cube_pos_z * cube_nline_z_t)/nav_cube.nsect_z;
81  cube_line_z_end = ((cube_pos_z+1) * cube_nline_z_t)/nav_cube.nsect_z;
82  nav_cube.nline_z = cube_line_z_end - cube_line_z_start;
83 
84  /* do the costly stuff only once */
85  alpha = ((360. - nav_cube.alpha) / 360.) * 2 * M_PI;
86  cos_alpha = cos(alpha);
87  sin_alpha = sin(alpha);
88 
89  /* calculate lower left start begin/end x coord */
90  start_bx = WaypointX(center) - (((cube_nline_x_t-1) * nav_cube.grid_x)/2)
91  + nav_cube.offset.x;
92  start_ex = start_bx;
93 
94  /* calculate lower left start end point y coord */
95  start_ey = WaypointY(center) - nav_cube.offset.y;
96 
97  /* calculate lower left start begin point y coord */
98  start_by = start_ey - nav_cube.size.y;
99 
100  /* calculate lower left start begin/end z coord */
101  start_bz = waypoints[center].a - (((cube_nline_z_t-1) * nav_cube.grid_z)/2)
102  + (cube_line_z_start*nav_cube.grid_z) + nav_cube.offset.z;
103  start_ez = start_bz;
104 
105  /* reset all waypoints to the standby position */
106  for (j=0; j < MAX_LINES_X; j++) {
107  waypoints[tb+j].x = WaypointX(center) + STBY_OFFSET;
108  waypoints[tb+j].y = WaypointY(center);
109  waypoints[te+j].x = WaypointX(center) + STBY_OFFSET;
110  waypoints[te+j].y = WaypointY(center);
111  }
112 
113  /* set used waypoints */
114  for (j=0; j < nav_cube.nline_x; j++) {
115  int i = cube_line_x_start+j;
116  /* set waypoints and vectorize in regard to center */
117  bx = (start_bx + i*nav_cube.grid_x) - WaypointX(center);
118  by = start_by - WaypointY(center);
119  ex = (start_ex + i*nav_cube.grid_x) - WaypointX(center);
120  ey = start_ey - WaypointY(center);
121  /* rotate clockwise with alpha and un-vectorize*/
122  waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center);
123  waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center);
124  waypoints[tb+j].a = start_bz;
125  waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center);
126  waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center);
127  waypoints[te+j].a = start_ez;
128  }
129 
130  /* bug in <for from="" to=""> ? */
131  nav_cube.nline_x--;
132  nav_cube.nline_z--;
133 
134  return FALSE;
135 }
136 
138  uint8_t dest_b, uint8_t dest_e,
139  uint8_t src_b, uint8_t src_e) {
140 
141  if (i > nav_cube.nline_x)
142  return FALSE;
143  if (j > nav_cube.nline_z)
144  return FALSE;
145 
146  waypoints[dest_b].x = waypoints[src_b+i].x;
147  waypoints[dest_b].y = waypoints[src_b+i].y;
148  waypoints[dest_b].a = waypoints[src_b+i].a + j*nav_cube.grid_z;
149  /* always keep at least security altitude */
150  if (waypoints[dest_b].a < (ground_alt+SECURITY_HEIGHT))
151  waypoints[dest_b].a = ground_alt+SECURITY_HEIGHT;
152 
153  waypoints[dest_e].x = waypoints[src_e+i].x;
154  waypoints[dest_e].y = waypoints[src_e+i].y;
155  waypoints[dest_e].a = waypoints[src_e+i].a + j*nav_cube.grid_z;
156  /* always keep at least security altitude */
157  if (waypoints[dest_e].a < (ground_alt+SECURITY_HEIGHT))
158  waypoints[dest_e].a = ground_alt+SECURITY_HEIGHT;
159 
160  return FALSE;
161 }
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
int32_t sect
sector to fly in (1..[nsect_x*nsect_z])
Definition: nav_cube.h:116
int32_t nline_x
number of lines x (horizontal)
Definition: nav_cube.h:119
int32_t nsect_z
number of sectors vertical
Definition: nav_cube.h:118
#define FALSE
Definition: imu_chimu.h:141
struct Int32Vect3 offset
offset to center.
Definition: nav_cube.h:112
int32_t nsect_x
number of sectors horizontal
Definition: nav_cube.h:117
struct Int32Vect3 size
size of the cube.
Definition: nav_cube.h:105
int32_t alpha
angle in degrees of flight direction to north, clockwise
Definition: nav_cube.h:113
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
int32_t y
North.
int32_t grid_x
grid distance x (horizontal)
Definition: nav_cube.h:114
signed char int8_t
Definition: types.h:15
int32_t grid_z
grid distance z (vertical)
Definition: nav_cube.h:115
int32_t nline_z
number of lines z (vertical)
Definition: nav_cube.h:120
int32_t x
East.