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