Paparazzi UAS  v5.15_devel-109-gee85905
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  survey_uturn = false;
150 
151  //go to start position
152  ENU_BFP_OF_REAL(survey_from_i, survey_from);
154  VECT3_COPY(navigation_target, survey_from_i);
156  NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
157  if (survey_orientation == NS) {
159  } else {
161  }
162 }
163 
164 
166 {
167  static bool is_last_half = false;
168  static float survey_radius;
169  nav_survey_active = true;
170 
171  /* entry scan */ // wait for start position and altitude be reached
172  if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0))
173  || (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
174  } else {
178  }
179 
180  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
181  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
182  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
183  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
184 
185  /* Update the current segment from corners' coordinates*/
186  if (SurveyGoingNorth()) {
189  } else if (SurveyGoingSouth()) {
192  } else if (SurveyGoingEast()) {
195  } else if (SurveyGoingWest()) {
198  }
199 
200  if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
201  /* if you like to use position croos instead of approaching uncoment this line
202  if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
203  (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
204  (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
205  (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
206  */
207  /* Continue ... */
209 
210  if (!nav_approaching_from(&survey_to_i, NULL, 0)) {
211  ENU_BFP_OF_REAL(survey_from_i, survey_from);
212 
214  nav_route(&survey_from_i, &survey_to_i);
215 
216  } else {
217  if (survey_orientation == NS) {
218  /* North or South limit reached, prepare turn and next leg */
219  float x0 = survey_from.x; /* Current longitude */
220  if ((x0 + nav_survey_shift < nav_survey_west)
221  || (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep
222  if (((x0 + (nav_survey_shift / 2)) < nav_survey_west)
223  || ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep
224  if (is_last_half) {// was last sweep half?
225  nav_survey_shift = -nav_survey_shift;
226  if (interleave) {
227  survey_radius = nav_survey_shift;
228  }else {
229  survey_radius = nav_survey_shift /2.;
230  }
231  is_last_half = false;
232  } else { // last sweep not half
233  nav_survey_shift = -nav_survey_shift;
234  if (interleave) {
235  survey_radius = nav_survey_shift /2.;
236  }else{
237  survey_radius = nav_survey_shift ;
238  }
239  }
241  } else { //room for half sweep after
242  survey_radius = nav_survey_shift / 2.;
243  is_last_half = true;
244  }
245  } else {// room for full sweep after
246  survey_radius = nav_survey_shift;
247  }
248 
249  x0 = x0 + survey_radius; /* Longitude of next leg */
250  survey_from.x = survey_to.x = x0;
251 
252  /* Swap South and North extremities */
253  float tmp = survey_from.y;
255  survey_to.y = tmp;
256 
258  waypoints[0].enu_f.x = x0;
259  waypoints[0].enu_f.y = survey_from.y;
260 
261  /* Computes the right direction */
262  if (SurveyGoingEast()) {
263  survey_radius = -survey_radius;
264  }
265  } else { /* (survey_orientation == WE) */
266  /* East or West limit reached, prepare turn and next leg */
267  /* There is a y0 declared in math.h (for ARM) !!! */
268  float my_y0 = survey_from.y; /* Current latitude */
269  if (my_y0 + nav_survey_shift < nav_survey_south
270  || my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep
271  if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south)
272  || ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep
273  if (is_last_half) {// was last sweep half?
274  nav_survey_shift = -nav_survey_shift;
275  if (interleave) {
276  survey_radius = nav_survey_shift;
277  }else {
278  survey_radius = nav_survey_shift /2.;
279  }
280  is_last_half = false;
281  } else { // last sweep not half
282  nav_survey_shift = -nav_survey_shift;
283  if (interleave) {
284  survey_radius = nav_survey_shift /2.;
285  }else{
286  survey_radius = nav_survey_shift ;
287  }
288  }
290  } else { //room for half sweep after
291  survey_radius = nav_survey_shift / 2.;
292  is_last_half = true;
293  }
294  } else {// room for full sweep after
295  survey_radius = nav_survey_shift;
296  }
297 
298  my_y0 = my_y0 + survey_radius; /* latitude of next leg */
299  survey_from.y = survey_to.y = my_y0;
300 
301  /* Swap West and East extremities */
302  float tmp = survey_from.x;
304  survey_to.x = tmp;
305 
307  waypoints[0].enu_f.x = survey_from.x;
308  waypoints[0].enu_f.y = my_y0;
309 
310  /* Computes the right direction */
311  if (SurveyGoingNorth()) {
312  survey_radius = -survey_radius;
313  }
314  }
315 
316  nav_in_segment = false;
317  survey_uturn = true;
319 #ifdef DIGITAL_CAM
320  float temp;
321  if (survey_orientation == NS) {
322  temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval;
323  } else{
324  temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval;
325  }
326  double inteiro;
327  double fract = modf (temp , &inteiro);
328  if (fract > .5) {
329  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
330  }
331 #endif
332  }
333  } else { /* START turn */
334 
335  static struct EnuCoor_f temp_f;
336  if (survey_orientation == WE) {
337  temp_f.x = waypoints[0].enu_f.x;
338  temp_f.y = waypoints[0].enu_f.y - survey_radius;
339  } else {
340  temp_f.y = waypoints[0].enu_f.y;
341  temp_f.x = waypoints[0].enu_f.x - survey_radius;
342  }
343 
344  //detect when segment has done
345  /* if you like to use position croos instead of approaching uncoment this line
346  if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )||
347  (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )||
348  (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )||
349  (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) {
350  */
351 
352  if (survey_orientation == WE) {
353  ENU_BFP_OF_REAL(survey_from_i, temp_f);
355  } else {
356  ENU_BFP_OF_REAL(survey_from_i, temp_f);
358  }
359  if (nav_approaching_from(&survey_to_i, NULL, 0)) {
360  survey_uturn = false;
361  nav_in_circle = false;
363  } else {
364 
365  if (survey_orientation == WE) {
366  ENU_BFP_OF_REAL(survey_from_i, temp_f);
368  } else {
369  ENU_BFP_OF_REAL(survey_from_i, temp_f);
371  }
372 
374  nav_route(&survey_from_i, &survey_to_i);
375  }
376  } /* END turn */
377 
378  } /* END entry scan */
379  return true;
380 
381 }// /* END survey_retangle */
382 
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:73
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