Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
69 static struct EnuCoor_f survey_from, survey_to;
70 
71 static bool survey_uturn __attribute__((unused)) = false;
73 static bool nav_survey_send = 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_send) {
95  pprz_msg_send_SURVEY(trans, dev, AC_ID,
97  nav_survey_send= false; // stop sending when run function is not called anymore
98  }
99 }
100 
102 {
103 #if PERIODIC_TELEMETRY
105 #endif
106 }
107 
109 {
111  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
112  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
113  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
114  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
115  survey_orientation = so;
116 
117  if (survey_orientation == NS) {
119  survey_from.x = survey_to.x = nav_survey_west + grid / 4.;
120  } else {
121  survey_from.x = survey_to.x = nav_survey_east - grid / 4.;
122  grid = -grid;
123  }
124 
128  } else {
131  }
132  } else { /* survey_orientation == WE */
134  survey_from.y = survey_to.y = nav_survey_south + grid / 4.;
135  } else {
136  survey_from.y = survey_to.y = nav_survey_north - grid / 4.;
137  grid = -grid;
138  }
139 
143  } else {
146  }
147  }
148  nav_survey_shift = grid;
149  sweep = grid;
150  survey_uturn = false;
152 
153  //go to start position
157  NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
158  if (survey_orientation == NS) {
160  } else {
162  }
163 }
164 
166 {
167  // check if nav route is available
168  if (nav.nav_route == NULL || nav.nav_approaching == NULL) {
169  return false;
170  }
171 
172  #ifdef NAV_SURVEY_RECTANGLE_DYNAMIC
174  #endif
175 
176  static bool is_last_half = false;
177  static float survey_radius;
178  nav_survey_send = true;
179 
180  /* entry scan */ // wait for start position and altitude be reached
182  || (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
183  } else {
187  }
188 
189  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
190  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
191  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
192  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
193 
194  /* Update the current segment from corners' coordinates*/
195  if (SurveyGoingNorth()) {
198  } else if (SurveyGoingSouth()) {
201  } else if (SurveyGoingEast()) {
204  } else if (SurveyGoingWest()) {
207  }
208 
209  if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
210  /* if you like to use position cross instead of approaching uncoment this line
211  if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
212  (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
213  (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
214  (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
215  */
216  /* Continue ... */
217  if (!nav.nav_approaching(&survey_to, NULL, 0)) {
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?
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
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?
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
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) {
327  } else{
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 turn_from;
340  if (survey_orientation == WE) {
341  turn_from.x = waypoints[0].enu_f.x;
342  turn_from.y = waypoints[0].enu_f.y - survey_radius;
343  } else {
344  turn_from.y = waypoints[0].enu_f.y;
345  turn_from.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) && (turn_from.y < waypoints[0].enu_f.y)) )||
351  (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (turn_from.y > waypoints[0].enu_f.y)) )||
352  (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (turn_from.x > waypoints[0].enu_f.x)) )||
353  (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (turn_from.x < waypoints[0].enu_f.x)) ) ) {
354  */
355 
356  if (nav.nav_approaching(&waypoints[0].enu_f, NULL, 0)) {
357  survey_uturn = false;
358  nav_in_circle = false;
360  } else {
361  nav.nav_route(&turn_from, &waypoints[0].enu_f);
362  }
363  } /* END turn */
364 
365  } /* END entry scan */
366  return true;
367 
368 }// /* END survey_retangle */
void dc_send_command(uint8_t cmd)
Send Command To Camera.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
#define WaypointX(_wp)
Definition: common_nav.h:45
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
#define WaypointY(_wp)
Definition: common_nav.h:46
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:83
@ DC_SHOOT
Definition: dc.h:100
#define Min(x, y)
Definition: esc_dshot.c:101
#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
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:191
static struct point survey_from
survey_orientation_t
void nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
static bool survey_uturn
#define RECTANGLE_SURVEY_DEFAULT_SWEEP
uint16_t rectangle_survey_sweep_num
#define LINE_STOP_FUNCTION
static void send_survey(struct transport_tx *trans, struct link_device *dev)
static survey_orientation_t survey_orientation
#define RECTANGLE_SURVEY_HEADING_WE
static struct EnuCoor_f survey_from survey_to
void nav_survey_rectangle_rotorcraft_init(void)
#define USE_INTERLEAVE
#define SurveyGoingNorth()
static bool nav_survey_rectangle_active
bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
#define LINE_START_FUNCTION
#define SurveyGoingEast()
#define SurveyGoingWest()
static bool nav_survey_send
#define SurveyGoingSouth()
#define RECTANGLE_SURVEY_HEADING_NS
Automatic survey of a rectangle for rotorcraft.
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
struct RotorcraftNavigation nav
Definition: navigation.c:51
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:342
Rotorcraft navigation functions.
#define NAV_HORIZONTAL_MODE_ROUTE
Definition: navigation.h:86
navigation_approaching nav_approaching
Definition: navigation.h:152
navigation_route nav_route
Definition: navigation.h:151
struct EnuCoor_f target
final target position (in meters)
Definition: navigation.h:126
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98