Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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"
31 #include "subsystems/nav.h"
32 #include "generated/flight_plan.h"
33 
41 
42 bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
43 
44  int32_t j, start_bx, start_by, start_bz, start_ex, start_ey, start_ez;
45  int32_t bx, by, ex, ey;
46  float alpha, cos_alpha, sin_alpha;
47  int32_t cube_nline_x_t, cube_nline_z_t;
48  int32_t cube_pos_x, cube_pos_z;
49  int32_t cube_line_x_start, cube_line_x_end;
50  int32_t cube_line_z_start, cube_line_z_end;
51 
52  /* sanity checks */
53  if (cube_nsect_x <= 0) cube_nsect_x = 1;
54  if (cube_nsect_z <= 0) cube_nsect_z = 1;
55  if ((cube_sect <= 0) ||
57 
58  /* total number of lines/layers to fly */
59  if (cube_grid_x == 0) cube_nline_x_t = 1;
60  else cube_nline_x_t = cube_size_x / cube_grid_x + 1;
61  if (cube_grid_z == 0) cube_nline_z_t = 1;
62  else cube_nline_z_t = cube_size_z / cube_grid_z + 1;
63 
64  /* position and number of lines in this sector */
65  cube_pos_x = (cube_sect-1) % cube_nsect_x;
66  cube_line_x_start = (cube_pos_x * cube_nline_x_t)/cube_nsect_x;
67  cube_line_x_end = ((cube_pos_x+1) * cube_nline_x_t)/cube_nsect_x;
68  if (cube_line_x_end > cube_nline_x_t) cube_line_x_end = cube_nline_x_t;
69  cube_nline_x = cube_line_x_end - cube_line_x_start;
70 
71  /* do not do more than pre-set number of lines */
73 
74  /* position and number of layers in this sector */
75  cube_pos_z = (cube_sect-1) / cube_nsect_x;
76  cube_line_z_start = (cube_pos_z * cube_nline_z_t)/cube_nsect_z;
77  cube_line_z_end = ((cube_pos_z+1) * cube_nline_z_t)/cube_nsect_z;
78  cube_nline_z = cube_line_z_end - cube_line_z_start;
79 
80  /* do the costly stuff only once */
81  alpha = ((360. - cube_alpha) / 360.) * 2 * M_PI;
82  cos_alpha = cos(alpha);
83  sin_alpha = sin(alpha);
84 
85  /* calculate lower left start begin/end x coord */
86  start_bx = WaypointX(center) - (((cube_nline_x_t-1) * cube_grid_x)/2)
87  + cube_offs_x;
88  start_ex = start_bx;
89 
90  /* calculate lower left start end point y coord */
91  start_ey = WaypointY(center) - cube_offs_y;
92 
93  /* calculate lower left start begin point y coord */
94  start_by = start_ey - cube_size_y;
95 
96  /* calculate lower left start begin/end z coord */
97  start_bz = waypoints[center].a - (((cube_nline_z_t-1) * cube_grid_z)/2)
98  + (cube_line_z_start*cube_grid_z) + cube_offs_z;
99  start_ez = start_bz;
100 
101  /* reset all waypoints to the standby position */
102  for (j=0; j < MAX_LINES_X; j++) {
103  waypoints[tb+j].x = WaypointX(center) + STBY_OFFSET;
104  waypoints[tb+j].y = WaypointY(center);
105  waypoints[te+j].x = WaypointX(center) + STBY_OFFSET;
106  waypoints[te+j].y = WaypointY(center);
107  }
108 
109  /* set used waypoints */
110  for (j=0; j < cube_nline_x; j++) {
111  int i = cube_line_x_start+j;
112  /* set waypoints and vectorize in regard to center */
113  bx = (start_bx + i*cube_grid_x) - WaypointX(center);
114  by = start_by - WaypointY(center);
115  ex = (start_ex + i*cube_grid_x) - WaypointX(center);
116  ey = start_ey - WaypointY(center);
117  /* rotate clockwise with alpha and un-vectorize*/
118  waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center);
119  waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center);
120  waypoints[tb+j].a = start_bz;
121  waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center);
122  waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center);
123  waypoints[te+j].a = start_ez;
124  }
125 
126  /* bug in <for from="" to=""> ? */
127  cube_nline_x--;
128  cube_nline_z--;
129 
130  return FALSE;
131 }
132 
133 bool_t nav_cube(int8_t j, int8_t i,
134  uint8_t dest_b, uint8_t dest_e,
135  uint8_t src_b, uint8_t src_e) {
136 
137  if (i > cube_nline_x) return FALSE;
138  if (j > cube_nline_z) return FALSE;
139 
140  waypoints[dest_b].x = waypoints[src_b+i].x;
141  waypoints[dest_b].y = waypoints[src_b+i].y;
142  waypoints[dest_b].a = waypoints[src_b+i].a + j*cube_grid_z;
143  /* always keep at least security altitude */
144  if (waypoints[dest_b].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_b].a = ground_alt+SECURITY_HEIGHT;
145 
146  waypoints[dest_e].x = waypoints[src_e+i].x;
147  waypoints[dest_e].y = waypoints[src_e+i].y;
148  waypoints[dest_e].a = waypoints[src_e+i].a + j*cube_grid_z;
149  /* always keep at least security altitude */
150  if (waypoints[dest_e].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_e].a = ground_alt+SECURITY_HEIGHT;
151 
152  return FALSE;
153 }
#define FALSE
Definition: imu_chimu.h:141
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
int32_t y
North.
signed char int8_t
Definition: types.h:15
int32_t x
East.