Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_survey_rectangle_rotorcraft.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
3  * 2015 NAC-VA, Eduardo Lavratti
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 
31 #ifndef RECTANGLE_SURVEY_DEFAULT_SWEEP
32 #define RECTANGLE_SURVEY_DEFAULT_SWEEP 25
33 #endif
34 
35 #ifdef RECTANGLE_SURVEY_USE_INTERLEAVE
36 #define USE_INTERLEAVE TRUE
37 #else
38 #define USE_INTERLEAVE FALSE
39 #endif
40 
41 #include "mcu_periph/uart.h"
42 #include "pprzlink/messages.h"
44 
45 #if PERIODIC_TELEMETRY
47 #endif
48 
50 
52 #include "state.h"
53 
55 static bool nav_survey_rectangle_active = false;
57 bool nav_in_segment = false;
58 bool nav_in_circle = false;
60 
62 static struct EnuCoor_i survey_from_i, survey_to_i;
63 
64 static bool survey_uturn __attribute__((unused)) = false;
66 
69 
70 #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y))
71 #define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y))
72 #define SurveyGoingEast() ((survey_orientation == WE) && (survey_to.x > survey_from.x))
73 #define SurveyGoingWest() ((survey_orientation == WE) && (survey_to.x < survey_from.x))
74 
75 #include "generated/flight_plan.h"
76 
77 #ifndef LINE_START_FUNCTION
78 #define LINE_START_FUNCTION {}
79 #endif
80 #ifndef LINE_STOP_FUNCTION
81 #define LINE_STOP_FUNCTION {}
82 #endif
83 
84 static void send_survey(struct transport_tx *trans, struct link_device *dev)
85 {
86  if (nav_survey_active) {
87  pprz_msg_send_SURVEY(trans, dev, AC_ID,
88  &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
89  }
90 }
91 
93 {
94 #if PERIODIC_TELEMETRY
96 #endif
97 }
98 
100 {
102  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
103  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
104  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
105  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
106  survey_orientation = so;
107 
108  if (survey_orientation == NS) {
109  if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
110  survey_from.x = survey_to.x = nav_survey_west + grid / 4.;
111  } else {
112  survey_from.x = survey_to.x = nav_survey_east - grid / 4.;
113  grid = -grid;
114  }
115 
116  if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
119  } else {
122  }
123  } else { /* survey_orientation == WE */
124  if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
125  survey_from.y = survey_to.y = nav_survey_south + grid / 4.;
126  } else {
127  survey_from.y = survey_to.y = nav_survey_north - grid / 4.;
128  grid = -grid;
129  }
130 
131  if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
134  } else {
137  }
138  }
139  nav_survey_shift = grid;
140  survey_uturn = false;
142 
143  //go to start position
144  ENU_BFP_OF_REAL(survey_from_i, survey_from);
146  VECT3_COPY(navigation_target, survey_from_i);
148  NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
149  if (survey_orientation == NS) {
151  } else {
153  }
154  return false;
155 }
156 
157 
159 {
160  static bool is_last_half = false;
161  static float survey_radius;
162  nav_survey_active = true;
163 
164  /* entry scan */ // wait for start position and altitude be reached
165  if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0))
166  || (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
167  } else {
171  }
172 
173  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
174  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
175  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
176  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
177 
178  /* Update the current segment from corners' coordinates*/
179  if (SurveyGoingNorth()) {
182  } else if (SurveyGoingSouth()) {
185  } else if (SurveyGoingEast()) {
188  } else if (SurveyGoingWest()) {
191  }
192 
193  if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
194  /* if you like to use position croos instead of approaching uncoment this line
195  if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
196  (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
197  (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
198  (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
199  */
200  /* Continue ... */
202 
203  if (!nav_approaching_from(&survey_to_i, NULL, 0)) {
204  ENU_BFP_OF_REAL(survey_from_i, survey_from);
205 
207  nav_route(&survey_from_i, &survey_to_i);
208 
209  } else {
210  if (survey_orientation == NS) {
211  /* North or South limit reached, prepare turn and next leg */
212  float x0 = survey_from.x; /* Current longitude */
213  if ((x0 + nav_survey_shift < nav_survey_west)
214  || (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep
215  if (((x0 + (nav_survey_shift / 2)) < nav_survey_west)
216  || ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep
217  if (is_last_half) {// was last sweep half?
218  nav_survey_shift = -nav_survey_shift;
219  if (interleave) {
220  survey_radius = nav_survey_shift;
221  }else {
222  survey_radius = nav_survey_shift /2.;
223  }
224  is_last_half = false;
225  } else { // last sweep not half
226  nav_survey_shift = -nav_survey_shift;
227  if (interleave) {
228  survey_radius = nav_survey_shift /2.;
229  }else{
230  survey_radius = nav_survey_shift ;
231  }
232  }
234  } else { //room for half sweep after
235  survey_radius = nav_survey_shift / 2.;
236  is_last_half = true;
237  }
238  } else {// room for full sweep after
239  survey_radius = nav_survey_shift;
240  }
241 
242  x0 = x0 + survey_radius; /* Longitude of next leg */
243  survey_from.x = survey_to.x = x0;
244 
245  /* Swap South and North extremities */
246  float tmp = survey_from.y;
248  survey_to.y = tmp;
249 
251  waypoints[0].enu_f.x = x0;
252  waypoints[0].enu_f.y = survey_from.y;
253 
254  /* Computes the right direction */
255  if (SurveyGoingEast()) {
256  survey_radius = -survey_radius;
257  }
258  } else { /* (survey_orientation == WE) */
259  /* East or West limit reached, prepare turn and next leg */
260  /* There is a y0 declared in math.h (for ARM) !!! */
261  float my_y0 = survey_from.y; /* Current latitude */
262  if (my_y0 + nav_survey_shift < nav_survey_south
263  || my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep
264  if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south)
265  || ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep
266  if (is_last_half) {// was last sweep half?
267  nav_survey_shift = -nav_survey_shift;
268  if (interleave) {
269  survey_radius = nav_survey_shift;
270  }else {
271  survey_radius = nav_survey_shift /2.;
272  }
273  is_last_half = false;
274  } else { // last sweep not half
275  nav_survey_shift = -nav_survey_shift;
276  if (interleave) {
277  survey_radius = nav_survey_shift /2.;
278  }else{
279  survey_radius = nav_survey_shift ;
280  }
281  }
283  } else { //room for half sweep after
284  survey_radius = nav_survey_shift / 2.;
285  is_last_half = true;
286  }
287  } else {// room for full sweep after
288  survey_radius = nav_survey_shift;
289  }
290 
291  my_y0 = my_y0 + survey_radius; /* latitude of next leg */
292  survey_from.y = survey_to.y = my_y0;
293 
294  /* Swap West and East extremities */
295  float tmp = survey_from.x;
297  survey_to.x = tmp;
298 
300  waypoints[0].enu_f.x = survey_from.x;
301  waypoints[0].enu_f.y = my_y0;
302 
303  /* Computes the right direction */
304  if (SurveyGoingNorth()) {
305  survey_radius = -survey_radius;
306  }
307  }
308 
309  nav_in_segment = false;
310  survey_uturn = true;
312 #ifdef DIGITAL_CAM
313  float temp;
314  if (survey_orientation == NS) {
315  temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval;
316  } else{
317  temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval;
318  }
319  double inteiro;
320  double fract = modf (temp , &inteiro);
321  if (fract > .5) {
322  dc_send_command(DC_SHOOT); //if last shot is more than shot_distance/2 from the corner take a picture in the corner before go to the next sweep
323  }
324 #endif
325  }
326  } else { /* START turn */
327 
328  static struct EnuCoor_f temp_f;
329  if (survey_orientation == WE) {
330  temp_f.x = waypoints[0].enu_f.x;
331  temp_f.y = waypoints[0].enu_f.y - survey_radius;
332  } else {
333  temp_f.y = waypoints[0].enu_f.y;
334  temp_f.x = waypoints[0].enu_f.x - survey_radius;
335  }
336 
337  //detect when segment has done
338  /* if you like to use position croos instead of approaching uncoment this line
339  if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )||
340  (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )||
341  (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )||
342  (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) {
343  */
344 
345  if (survey_orientation == WE) {
346  ENU_BFP_OF_REAL(survey_from_i, temp_f);
348  } else {
349  ENU_BFP_OF_REAL(survey_from_i, temp_f);
351  }
352  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
353  survey_uturn = false;
354  nav_in_circle = false;
356  } else {
357 
358  if (survey_orientation == WE) {
359  ENU_BFP_OF_REAL(survey_from_i, temp_f);
361  } else {
362  ENU_BFP_OF_REAL(survey_from_i, temp_f);
364  }
365 
367  nav_route(&survey_from_i, &survey_to_i);
368  }
369  } /* END turn */
370 
371  } /* END entry scan */
372  return true;
373 
374 }// /* END survey_retangle */
375 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float x
Definition: common_nav.h:40
int32_t y
North.
int32_t x
East.
Periodic telemetry system header (includes downlink utility and generated code).
vector in East North Up coordinates Units: meters
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:713
Definition: dc.h:100
float y
Definition: common_nav.h:41
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t z
Up.
#define Min(x, y)
Definition: main_fbw.c:52
#define Max(x, y)
Definition: main_fbw.c:53
#define WaypointY(_wp)
Definition: common_nav.h:46
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
void dc_send_command(uint8_t cmd)
Send Command To Camera.
vector in East North Up coordinates
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
#define ENU_BFP_OF_REAL(_o, _i)
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:78
float y
in meters
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46