Paparazzi UAS  v5.17_devel-14-g4575375
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 
54 #ifndef RECTANGLE_SURVEY_HEADING_NS
55 #define RECTANGLE_SURVEY_HEADING_NS 0.f
56 #endif
57 
58 #ifndef RECTANGLE_SURVEY_HEADING_WE
59 #define RECTANGLE_SURVEY_HEADING_WE 90.f
60 #endif
61 
63 static bool nav_survey_rectangle_active = false;
65 bool nav_in_segment = false;
66 bool nav_in_circle = false;
68 
70 static struct EnuCoor_i survey_from_i, survey_to_i;
71 
72 static bool survey_uturn __attribute__((unused)) = false;
74 
77 
78 #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y))
79 #define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y))
80 #define SurveyGoingEast() ((survey_orientation == WE) && (survey_to.x > survey_from.x))
81 #define SurveyGoingWest() ((survey_orientation == WE) && (survey_to.x < survey_from.x))
82 
83 #include "generated/flight_plan.h"
84 
85 #ifndef LINE_START_FUNCTION
86 #define LINE_START_FUNCTION {}
87 #endif
88 #ifndef LINE_STOP_FUNCTION
89 #define LINE_STOP_FUNCTION {}
90 #endif
91 
92 static void send_survey(struct transport_tx *trans, struct link_device *dev)
93 {
94  if (nav_survey_active) {
95  pprz_msg_send_SURVEY(trans, dev, AC_ID,
96  &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
97  }
98 }
99 
101 {
102 #if PERIODIC_TELEMETRY
104 #endif
105 }
106 
108 {
110  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
111  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
112  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
113  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
114  survey_orientation = so;
115 
116  if (survey_orientation == NS) {
117  if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
118  survey_from.x = survey_to.x = nav_survey_west + grid / 4.;
119  } else {
120  survey_from.x = survey_to.x = nav_survey_east - grid / 4.;
121  grid = -grid;
122  }
123 
124  if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
127  } else {
130  }
131  } else { /* survey_orientation == WE */
132  if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
133  survey_from.y = survey_to.y = nav_survey_south + grid / 4.;
134  } else {
135  survey_from.y = survey_to.y = nav_survey_north - grid / 4.;
136  grid = -grid;
137  }
138 
139  if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
142  } else {
145  }
146  }
147  nav_survey_shift = grid;
148  sweep = grid;
149  survey_uturn = false;
151 
152  //go to start position
153  ENU_BFP_OF_REAL(survey_from_i, survey_from);
155  VECT3_COPY(navigation_target, survey_from_i);
157  NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
158  if (survey_orientation == NS) {
160  } else {
162  }
163 }
164 
166 {
167  #ifdef NAV_SURVEY_RECTANGLE_DYNAMIC
168  nav_survey_shift = (nav_survey_shift > 0 ? sweep : -sweep);
169  #endif
170 
171  static bool is_last_half = false;
172  static float survey_radius;
173  nav_survey_active = true;
174 
175  /* entry scan */ // wait for start position and altitude be reached
176  if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0))
177  || (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
178  } else {
182  }
183 
184  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
185  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
186  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
187  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
188 
189  /* Update the current segment from corners' coordinates*/
190  if (SurveyGoingNorth()) {
193  } else if (SurveyGoingSouth()) {
196  } else if (SurveyGoingEast()) {
199  } else if (SurveyGoingWest()) {
202  }
203 
204  if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
205  /* if you like to use position croos instead of approaching uncoment this line
206  if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
207  (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
208  (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
209  (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
210  */
211  /* Continue ... */
213 
214  if (!nav_approaching_from(&survey_to_i, NULL, 0)) {
215  ENU_BFP_OF_REAL(survey_from_i, survey_from);
216 
218  nav_route(&survey_from_i, &survey_to_i);
219 
220  } else {
221  if (survey_orientation == NS) {
222  /* North or South limit reached, prepare turn and next leg */
223  float x0 = survey_from.x; /* Current longitude */
224  if ((x0 + nav_survey_shift < nav_survey_west)
225  || (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep
226  if (((x0 + (nav_survey_shift / 2)) < nav_survey_west)
227  || ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep
228  if (is_last_half) {// was last sweep half?
229  nav_survey_shift = -nav_survey_shift;
230  if (interleave) {
231  survey_radius = nav_survey_shift;
232  }else {
233  survey_radius = nav_survey_shift /2.;
234  }
235  is_last_half = false;
236  } else { // last sweep not half
237  nav_survey_shift = -nav_survey_shift;
238  if (interleave) {
239  survey_radius = nav_survey_shift /2.;
240  }else{
241  survey_radius = nav_survey_shift ;
242  }
243  }
245  } else { //room for half sweep after
246  survey_radius = nav_survey_shift / 2.;
247  is_last_half = true;
248  }
249  } else {// room for full sweep after
250  survey_radius = nav_survey_shift;
251  }
252 
253  x0 = x0 + survey_radius; /* Longitude of next leg */
254  survey_from.x = survey_to.x = x0;
255 
256  /* Swap South and North extremities */
257  float tmp = survey_from.y;
259  survey_to.y = tmp;
260 
262  waypoints[0].enu_f.x = x0;
263  waypoints[0].enu_f.y = survey_from.y;
264 
265  /* Computes the right direction */
266  if (SurveyGoingEast()) {
267  survey_radius = -survey_radius;
268  }
269  } else { /* (survey_orientation == WE) */
270  /* East or West limit reached, prepare turn and next leg */
271  /* There is a y0 declared in math.h (for ARM) !!! */
272  float my_y0 = survey_from.y; /* Current latitude */
273  if (my_y0 + nav_survey_shift < nav_survey_south
274  || my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep
275  if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south)
276  || ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep
277  if (is_last_half) {// was last sweep half?
278  nav_survey_shift = -nav_survey_shift;
279  if (interleave) {
280  survey_radius = nav_survey_shift;
281  }else {
282  survey_radius = nav_survey_shift /2.;
283  }
284  is_last_half = false;
285  } else { // last sweep not half
286  nav_survey_shift = -nav_survey_shift;
287  if (interleave) {
288  survey_radius = nav_survey_shift /2.;
289  }else{
290  survey_radius = nav_survey_shift ;
291  }
292  }
294  } else { //room for half sweep after
295  survey_radius = nav_survey_shift / 2.;
296  is_last_half = true;
297  }
298  } else {// room for full sweep after
299  survey_radius = nav_survey_shift;
300  }
301 
302  my_y0 = my_y0 + survey_radius; /* latitude of next leg */
303  survey_from.y = survey_to.y = my_y0;
304 
305  /* Swap West and East extremities */
306  float tmp = survey_from.x;
308  survey_to.x = tmp;
309 
311  waypoints[0].enu_f.x = survey_from.x;
312  waypoints[0].enu_f.y = my_y0;
313 
314  /* Computes the right direction */
315  if (SurveyGoingNorth()) {
316  survey_radius = -survey_radius;
317  }
318  }
319 
320  nav_in_segment = false;
321  survey_uturn = true;
323 #ifdef DIGITAL_CAM
324  float temp;
325  if (survey_orientation == NS) {
326  temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval;
327  } else{
328  temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval;
329  }
330  double inteiro;
331  double fract = modf (temp , &inteiro);
332  if (fract > .5) {
333  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
334  }
335 #endif
336  }
337  } else { /* START turn */
338 
339  static struct EnuCoor_f temp_f;
340  if (survey_orientation == WE) {
341  temp_f.x = waypoints[0].enu_f.x;
342  temp_f.y = waypoints[0].enu_f.y - survey_radius;
343  } else {
344  temp_f.y = waypoints[0].enu_f.y;
345  temp_f.x = waypoints[0].enu_f.x - survey_radius;
346  }
347 
348  //detect when segment has done
349  /* if you like to use position croos instead of approaching uncoment this line
350  if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )||
351  (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )||
352  (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )||
353  (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) {
354  */
355 
356  if (survey_orientation == WE) {
357  ENU_BFP_OF_REAL(survey_from_i, temp_f);
359  } else {
360  ENU_BFP_OF_REAL(survey_from_i, temp_f);
362  }
363  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
364  survey_uturn = false;
365  nav_in_circle = false;
367  } else {
368 
369  if (survey_orientation == WE) {
370  ENU_BFP_OF_REAL(survey_from_i, temp_f);
372  } else {
373  ENU_BFP_OF_REAL(survey_from_i, temp_f);
375  }
376 
378  nav_route(&survey_from_i, &survey_to_i);
379  }
380  } /* END turn */
381 
382  } /* END entry scan */
383  return true;
384 
385 }// /* END survey_retangle */
struct EnuCoor_i navigation_target
Definition: navigation.c:87
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define Min(x, y)
Definition: esc_dshot.c:85
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:140
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
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 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:74
Rotorcraft navigation functions.
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
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:491
#define ENU_BFP_OF_REAL(_o, _i)
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:584
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:83
float y
in meters
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
Definition: navigation.c:257
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46