Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
nav_cube.c
Go to the documentation of this file.
1 /*
2  * $Id: nav_cube.c 3600 2009-07-01 20:05:12Z hecto $
3  *
4  * Copyright (C) 2010 Martin Mueller
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
30 #include "generated/airframe.h"
32 #include "subsystems/nav.h"
33 #include "generated/flight_plan.h"
34 
42 
43 bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
44 
45  int32_t j, start_bx, start_by, start_bz, start_ex, start_ey, start_ez;
46  int32_t bx, by, ex, ey;
47  float alpha, cos_alpha, sin_alpha;
48  int32_t cube_nline_x_t, cube_nline_z_t;
49  int32_t cube_pos_x, cube_pos_z;
50  int32_t cube_line_x_start, cube_line_x_end;
51  int32_t cube_line_z_start, cube_line_z_end;
52 
53  /* sanity checks */
54  if (cube_nsect_x <= 0) cube_nsect_x = 1;
55  if (cube_nsect_z <= 0) cube_nsect_z = 1;
56  if ((cube_sect <= 0) ||
58 
59  /* total number of lines/layers to fly */
60  if (cube_grid_x == 0) cube_nline_x_t = 1;
61  else cube_nline_x_t = cube_size_x / cube_grid_x + 1;
62  if (cube_grid_z == 0) cube_nline_z_t = 1;
63  else cube_nline_z_t = cube_size_z / cube_grid_z + 1;
64 
65  /* position and number of lines in this sector */
66  cube_pos_x = (cube_sect-1) % cube_nsect_x;
67  cube_line_x_start = (cube_pos_x * cube_nline_x_t)/cube_nsect_x;
68  cube_line_x_end = ((cube_pos_x+1) * cube_nline_x_t)/cube_nsect_x;
69  if (cube_line_x_end > cube_nline_x_t) cube_line_x_end = cube_nline_x_t;
70  cube_nline_x = cube_line_x_end - cube_line_x_start;
71 
72  /* do not do more than pre-set number of lines */
74 
75  /* position and number of layers in this sector */
76  cube_pos_z = (cube_sect-1) / cube_nsect_x;
77  cube_line_z_start = (cube_pos_z * cube_nline_z_t)/cube_nsect_z;
78  cube_line_z_end = ((cube_pos_z+1) * cube_nline_z_t)/cube_nsect_z;
79  cube_nline_z = cube_line_z_end - cube_line_z_start;
80 
81  /* do the costly stuff only once */
82  alpha = ((360. - cube_alpha) / 360.) * 2 * M_PI;
83  cos_alpha = cos(alpha);
84  sin_alpha = sin(alpha);
85 
86  /* calculate lower left start begin/end x coord */
87  start_bx = WaypointX(center) - (((cube_nline_x_t-1) * cube_grid_x)/2)
88  + cube_offs_x;
89  start_ex = start_bx;
90 
91  /* calculate lower left start end point y coord */
92  start_ey = WaypointY(center) - cube_offs_y;
93 
94  /* calculate lower left start begin point y coord */
95  start_by = start_ey - cube_size_y;
96 
97  /* calculate lower left start begin/end z coord */
98  start_bz = waypoints[center].a - (((cube_nline_z_t-1) * cube_grid_z)/2)
99  + (cube_line_z_start*cube_grid_z) + cube_offs_z;
100  start_ez = start_bz;
101 
102  /* reset all waypoints to the standby position */
103  for (j=0; j < MAX_LINES_X; j++) {
104  waypoints[tb+j].x = WaypointX(center) + STBY_OFFSET;
105  waypoints[tb+j].y = WaypointY(center);
106  waypoints[te+j].x = WaypointX(center) + STBY_OFFSET;
107  waypoints[te+j].y = WaypointY(center);
108  }
109 
110  /* set used waypoints */
111  for (j=0; j < cube_nline_x; j++) {
112  int i = cube_line_x_start+j;
113  /* set waypoints and vectorize in regard to center */
114  bx = (start_bx + i*cube_grid_x) - WaypointX(center);
115  by = start_by - WaypointY(center);
116  ex = (start_ex + i*cube_grid_x) - WaypointX(center);
117  ey = start_ey - WaypointY(center);
118  /* rotate clockwise with alpha and un-vectorize*/
119  waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center);
120  waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center);
121  waypoints[tb+j].a = start_bz;
122  waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center);
123  waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center);
124  waypoints[te+j].a = start_ez;
125  }
126 
127  /* bug in <for from="" to=""> ? */
128  cube_nline_x--;
129  cube_nline_z--;
130 
131  return FALSE;
132 }
133 
134 bool_t nav_cube(int8_t j, int8_t i,
135  uint8_t dest_b, uint8_t dest_e,
136  uint8_t src_b, uint8_t src_e) {
137 
138  if (i > cube_nline_x) return FALSE;
139  if (j > cube_nline_z) return FALSE;
140 
141  waypoints[dest_b].x = waypoints[src_b+i].x;
142  waypoints[dest_b].y = waypoints[src_b+i].y;
143  waypoints[dest_b].a = waypoints[src_b+i].a + j*cube_grid_z;
144  /* always keep at least security altitude */
145  if (waypoints[dest_b].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_b].a = ground_alt+SECURITY_HEIGHT;
146 
147  waypoints[dest_e].x = waypoints[src_e+i].x;
148  waypoints[dest_e].y = waypoints[src_e+i].y;
149  waypoints[dest_e].a = waypoints[src_e+i].a + j*cube_grid_z;
150  /* always keep at least security altitude */
151  if (waypoints[dest_e].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_e].a = ground_alt+SECURITY_HEIGHT;
152 
153  return FALSE;
154 }
#define FALSE
Definition: imu_chimu.h:141
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
signed char int8_t
Definition: types.h:15