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
cartography.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Vandeportaele Bertrand
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
30 #include "state.h"
31 #include "stdio.h"
32 
34 #include "generated/flight_plan.h"
35 
36 #include "std.h" //macros pas mal dans sw/include
37 
39 //for fast debbuging, the simulation can be accelerated using the gaia software from an xterm console
40 // /home/bvdp/paparazzi3/sw/simulator/gaia
42 // for explanations about debugging macros:
43 //http://gcc.gnu.org/onlinedocs/cpp/Stringification.html#Stringification
44 
45 // Be carefull not to use printf function in ap compilation, only use it in sim compilation
46 // the DEBUG_PRINTF should be defined only in the sim part of the makefile airframe file
47 #ifdef DEBUG_PRINTF
48 int CPTDEBUG = 0;
49 #define PRTDEB(TYPE,EXP) \
50  printf("%5d: " #EXP ": %"#TYPE"\n",CPTDEBUG,EXP);fflush(stdout);CPTDEBUG++;
51 #define PRTDEBSTR(EXP) \
52  printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++;
53 #else
54 #define PRTDEB(TYPE,EXP) \
55  ;
56 
57 #define PRTDEBSTR(EXP) \
58  ;
59 #endif
60 
61 /*
62  exemple of use for theese macros
63  PRTDEBSTR(Init polysurvey)
64  PRTDEB(u,SurveySize)
65 
66  PRTDEB(lf,PolygonCenter.x)
67  PRTDEB(lf,PolygonCenter.y)
68  */
70 
71 #define DISTXY(P1X,P1Y,P2X,P2Y)\
72  (sqrt( ( (P2X-P1X) * (P2X-P1X) ) + ( (P2Y-P1Y) * (P2Y-P1Y) ) ) )
73 
74 #define NORMXY(P1X,P1Y)\
75  (sqrt( ( (P1X) * (P1X) ) + ( (P1Y) * (P1Y) ) ) )
76 
77 
78 //max distance between the estimator position and an objective points to consider that the objective points is atteined
79 
80 #define DISTLIMIT 30
81 
83 
85  1; //used to count the number of rails the plane has achieved since the boot, to number the sequences of saved images
86 //the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences
87 
89 #define USE_ONBOARD_CAMERA 1
90 
91 #if USE_ONBOARD_CAMERA
93 #endif
94 
95 
96 
97 
99 bool survey_losange_uturn;//this flag indicates if the aircraft is turning between 2 rails (1) or if it is flying straight forward in the rail direction (0)
100 
101 int railnumber; //indicate the number of the rail being acquired
103 
104 float distrail; //distance between adjacent rails in meters, the value is set in the init function
105 float distplus; //distance that the aircraft should travel before and after a rail before turning to the next rails in meters, the value is set in the init function
106 
108  60; //a cheangable value that can be set interactively in the GCS, not used at that time, it can be used to choose the right value while the aircraft is flying
109 
110 
111 static struct point point1, point2, point3; // 3 2D points used for navigation
112 static struct point pointA, pointB, pointC; // 3 2D points used for navigation
113 static struct point vec12, vec13;
114 float norm12, norm13; // norms of 12 & 13 vectors
115 
116 
117 
118 
119 float tempx, tempy; //temporary points for exchanges
120 float angle1213; //angle between 12 & 13 vectors
121 float signforturn; //can be +1 or -1, it is used to compute the direction of the UTURN between rails
122 
123 float tempcircleradius;// used to compute the radius of the UTURN after a rail
124 float circleradiusmin = 40;
125 
128 
129 
132 
134 
135 
136 
137 
139 #include "generated/modules.h"
140 
142 #include "mcu_periph/uart.h"
143 #include "std.h"
144 
145 
147 
148 void init_carto(void)
149 {
150 }
151 
153 {
154  static uint16_t dummy_id = 0;
155  static uint8_t dummy_state = 0;
156  static uint8_t snapshot_valid = 1;
157  static float dummy_temp = NAN;
158  DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,
159  &dummy_d,
160  &dummy_state,
162  &snapshot_valid,
163  &dummy_temp);
164 }
165 
166 void start_carto(void)
167 {
168 }
169 
170 void stop_carto(void)
171 {
172 }
173 
174 
175 
178 {
180  return false;
181 }
184 {
186  PRTDEBSTR(SNAPSHOT)
187  cartography_periodic_downlink_carto_status = MODULES_START;
188  return false;
189 
190 }
193 {
195  PRTDEBSTR(SNAPSHOT)
196  cartography_periodic_downlink_carto_status = MODULES_START;
197  return true;
198 
199 }
202 {
204  PRTDEBSTR(STOP SNAPSHOT)
205  cartography_periodic_downlink_carto_status = MODULES_START;
206  return false;
207 
208 }
210 
212 {
213  waypoints[wp4].x = waypoints[wp2].x + waypoints[wp3].x - waypoints[wp1].x;
214  waypoints[wp4].y = waypoints[wp2].y + waypoints[wp3].y - waypoints[wp1].y;
215 
217  PRTDEB(f, waypoints[wp4].x)
218  PRTDEB(f, waypoints[wp4].y)
219  return false;
220 }
221 
223 bool nav_survey_ComputeProjectionOnLine(struct point pointAf, struct point pointBf, float pos_xf, float pos_yf,
224  float *normAMf, float *normBMf, float *distancefromrailf);
225 
226 
227 bool nav_survey_ComputeProjectionOnLine(struct point pointAf, struct point pointBf, float pos_xf, float pos_yf,
228  float *normAMf, float *normBMf, float *distancefromrailf)
229 //return if the projection of the estimator on the AB line is located inside the AB interval
230 {
231  float a, b, c, xa, xb, xc, ya, yb, yc;
232  float f;
233  float AA1;
234  float BB1;
235  float YP;
236  float XP;
237 
238  float AMx, AMy, BMx, BMy;
239  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
240 
241 
242 
243  xb = pointAf.x;
244  yb = pointAf.y;
245 
246  xc = pointBf.x;
247  yc = pointBf.y;
248 
249  xa = pos_xf;
250  ya = pos_yf;
251 
252  //calcul des parametres de la droite pointAf pointBf
253  a = yc - yb;
254  b = xb - xc;
255  c = (yb - yc) * xb + (xc - xb) * yb ;
256 
257  //calcul de la distance de la droite à l'avion
258 
259 
260  if (fabs(a) > 1e-10) {
261  *distancefromrailf = fabs((a * xa + b * ya + c) / sqrt(a * a + b *
262  b)); //denominateur =0 iniquement si a=b=0 //peut arriver si 2 waypoints sont confondus
263  } else {
264  return 0;
265  }
266 
267  PRTDEB(f, a)
268  PRTDEB(f, b)
269  PRTDEB(f, c)
270  PRTDEB(f, *distancefromrailf)
271 
272 
273  // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC)
274  AA1 = (xc - xb);
275  BB1 = (yc - yb);
276  if (fabs(AA1) > 1e-10) {
277  f = (b - (a * BB1 / AA1));
278  if (fabs(f) > 1e-10) {
279  YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f;
280  } else {
281  return 0;
282  }
283  } else {
284  return 0;
285  }
286 
287 
288 
289 
290  XP = (-c - b * YP) / a ; //a !=0 deja testé avant
291  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
292  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
293  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
294 
295  PRTDEB(f, AA1)
296  PRTDEB(f, BB1)
297  PRTDEB(f, YP)
298  PRTDEB(f, XP)
299 
300  AMx = XP - pointAf.x;
301  AMy = YP - pointAf.y;
302  BMx = XP - pointBf.x;
303  BMy = YP - pointBf.y;
304 
305  *normAMf = NORMXY(AMx, AMy);
306  *normBMf = NORMXY(BMx, BMy);
307 
308  PRTDEB(f, *normAMf)
309  PRTDEB(f, *normBMf)
310 
311  if (((*normAMf) + (*normBMf)) > 1.05 * DISTXY(pointBf.x, pointBf.y, pointAf.x, pointAf.y)) {
312  PRTDEBSTR(NOT INSIDE)
313  return 0;
314  } else {
315  PRTDEBSTR(INSIDE)
316  return 1;
317  }
318 }
320 //if distrailinit = 0, the aircraft travel from wp1 -> wp2 then do the inverse travel passing through the wp3,
321 //This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly
322 bool nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
323 {
324  //PRTDEBSTR(nav_survey_losange_carto_init)
325  survey_losange_uturn = false;
326 
327 
328  point1.x = waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type
329  point1.y = waypoints[wp1].y;
330  point2.x = waypoints[wp2].x;
331  point2.y = waypoints[wp2].y;
332  point3.x = waypoints[wp3].x;
333  point3.y = waypoints[wp3].y;
334 
335  PRTDEB(u, wp1)
336  PRTDEB(f, point1.x)
337  PRTDEB(f, point1.y)
338 
339  PRTDEB(u, wp2)
340  PRTDEB(f, point2.x)
341  PRTDEB(f, point2.y)
342 
343  PRTDEB(u, wp3)
344  PRTDEB(f, point3.x)
345  PRTDEB(f, point3.y)
346 
347 
348 
349  vec12.x = point2.x - point1.x;
350  vec12.y = point2.y - point1.y;
351  PRTDEB(f, vec12.x)
352  PRTDEB(f, vec12.y)
353 
354  //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0
355  norm12 = NORMXY(vec12.x, vec12.y);
356 
357  PRTDEB(f, norm12)
358 
359 
360  vec13.x = point3.x - point1.x;
361  vec13.y = point3.y - point1.y;
362  PRTDEB(f, vec13.x)
363  PRTDEB(f, vec13.y)
364 
365  norm13 = NORMXY(vec13.x, vec13.y);
366  PRTDEB(f, norm13)
367 
368  //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13
369  // return false;
370 
371 
372  if (fabs(distrailinit) <= 1) {
373  //is distrailinit==0, then the aircraft should do 2 passes to register the bands
374  distrail = norm13;
375  numberofrailtodo = 1;
376  } else {
377  //no, so normal trajectory
378  distrail = fabs(distrailinit);
379  numberofrailtodo = ceil(norm13 / distrail); //round to the upper integer
380  }
381 
382  distplus = fabs(distplusinit);
383 
384 
385  PRTDEB(f, distrail)
386  PRTDEB(f, distplus)
388  PRTDEB(d, railnumber)
390 
391  railnumber = -1; // the state is before the first rail, which is numbered 0
392 
393  if (norm12 < 1e-15) {
394  return false;
395  }
396  if (norm13 < 1e-15) {
397  return false;
398  }
399 
400 
401  angle1213 = (180 / 3.14159) * acos((((vec12.x * vec13.x) + (vec12.y * vec13.y))) / (norm12 *
402  norm13)); //oriented angle between 12 and 13 vectors
403 
404  angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y, vec12.x);
405  while (angle1213 >= M_PI) { angle1213 -= 2 * M_PI; }
406  while (angle1213 <= -M_PI) { angle1213 += 2 * M_PI; }
407 
408  PRTDEB(f, angle1213)
409 
410  if (angle1213 > 0) {
411  signforturn = -1;
412  } else {
413  signforturn = 1;
414  }
415 
416 
417  return false; //Init function must return false, so that the next function in the flight plan is automatically executed
418  //dans le flight_plan.h
419  // if (! (nav_survey_losange_carto()))
420  // NextStageAndBreak();
421 }
424 {
425  //test pour modifier en vol la valeur distrail
426 
427  //distrail=distrailinteractif;
428 
429 
430  //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT,
431  //if the aircraft is inside the region to map, camera_snapshot_image_number will be equal to the number of rail since the last boot (not since the nav_survey_losange_carto_init, in order to get different values for differents calls to the cartography function (this number is used to name the images on the hard drive
433 
434 
435  PRTDEB(f, distrail)
436 
437 
438 
440  PRTDEB(d, railnumber)
441 
443 
444  //PRTDEB(f,stateGetPositionEnu_f()->x)
445  //PRTDEB(f,stateGetPositionEnu_f()->y)
446 
447  //sortir du bloc si données abhérantes
448  if (norm13 < 1e-15) {
449  PRTDEBSTR(norm13 < 1e-15)
450  return false;
451  }
452  if (norm12 < 1e-15) {
453  PRTDEBSTR(norm13 < 1e-15)
454  return false;
455  }
456  if (distrail < 1e-15) {
457  PRTDEBSTR(distrail < 1e-15)
458  return false;
459  }
460 
461  if (survey_losange_uturn == FALSE) {
462 
463  if (railnumber == -1) {
464  //se diriger vers le début du 1°rail
465  PRTDEBSTR(approche debut rail 0)
466  pointA.x = point1.x - (vec12.x / norm12) * distplus * 1.2; //on prend une marge plus grande pour arriver en ce point
467  pointA.y = point1.y - (vec12.y / norm12) * distplus * 1.2; //car le virage n'est pas tres bien géré
468 
469 
470  pointB.x = point2.x + (vec12.x / norm12) * distplus * 1.2; //on prend une marge plus grande pour arriver en ce point
471  pointB.y = point2.y + (vec12.y / norm12) * distplus * 1.2; //car le virage n'est pas tres bien géré
472 
473  //PRTDEB(f,pointA.x)
474  //PRTDEB(f,pointA.y)
475 
476 
477  //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
478  //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
479  //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
480 
481 
484 
485  if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointA.x, pointA.y) > 2 * DISTLIMIT)
486  || (normBM < (DISTXY(pointB.x, pointB.y, pointA.x, pointA.y)))) {
487  nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointA.x, pointA.y);
488  //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);
489  } else {
490  PRTDEBSTR(debut rail 0)
491  //un fois arrivé, on commence le 1° rail;
492  railnumber = 0;
494 
495  }
496  }
497 
498 
499  if (railnumber >= 0) {
500  pointA.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
501  pointA.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
502 
503  pointB.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
504  pointB.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
505 
506 
507 
508 
509 
510  if ((railnumber % 2) == 0) { //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2
511  //rien a faire
512  } else { //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1
513  //echange pointA et B
514  tempx = pointA.x;
515  tempy = pointA.y;
516  pointA.x = pointB.x;
517  pointA.y = pointB.y;
518  pointB.x = tempx;
519  pointB.y = tempy;
520 
521  }
522 
523  // PRTDEB(f,pointA.x)
524  // PRTDEB(f,pointA.y)
525  // PRTDEB(f,pointB.x)
526  // PRTDEB(f,pointB.y)
529 
530 
531 
532  // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) &&
533  // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
534 
535 
536  if (!((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT)
537  || (ProjectionInsideLimitOfRail && (normAM > (DISTXY(pointB.x, pointB.y, pointA.x, pointA.y))))))
538  // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
539  {
540  nav_route_xy(pointA.x, pointA.y, pointB.x, pointB.y);
541  PRTDEBSTR(NAVROUTE)
542 
543 
544  //est ce que l'avion est dans la zone ou il doit prendre des images?
545  //DEJA APPELE AVANT LE IF
546  // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail);
547 
548  if ((normAM > distplus) && (normBM > distplus) && (distancefromrail < distrail / 2)) {
549  //CAMERA_SNAPSHOT_REQUIERED=true;
550  //camera_snapshot_image_number++;
552  PRTDEBSTR(SNAPSHOT)
553  }
554 
555  }
556 
557  else { // virage
558  //PRTDEBSTR(debut rail suivant)
559  railnumber++;
561 
562  PRTDEB(d, railnumber)
564  //CAMERA_SNAPSHOT_REQUIERED=true;
565  //camera_snapshot_image_number++;
566 
568  survey_losange_uturn = true;
569 
570  }
571 
574  return false; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false
575  }
576 
577  }
578  } else { // (survey_losange_uturn==TRUE)
579 
580 
581  if (distrail < 200) {
582  //tourne autour d'un point à mi chemin entre les 2 rails
583 
584  //attention railnumber a été incrémenté en fin du rail précédent
585 
586  if ((railnumber % 2) == 1) { //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
587  PRTDEBSTR(UTURN - IMPAIR)
588  //fin du rail précédent
589  pointA.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
590  pointA.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
591  //début du rail suivant
592  pointB.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
593  pointB.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
594  //milieu
595  waypoints[0].x = (pointA.x + pointB.x) / 2;
596  waypoints[0].y = (pointA.y + pointB.y) / 2;
597 
598  tempcircleradius = distrail / 2;
601  }
602 
603 
604  //fin du rail suivant
605  pointC.x = (point1.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
606  pointC.y = (point1.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
607 
608 
609  course_next_rail = atan2(pointC.x - pointB.x, pointC.y - pointB.y);
612 
614  while (angle_between > M_PI) { angle_between -= 2 * M_PI; }
615  while (angle_between < -M_PI) { angle_between += 2 * M_PI; }
616 
617  angle_between = DegOfRad(angle_between);
619  //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
620 
622  if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT)
623  || (angle_between > -10 && angle_between < 10)) {
624  //l'avion fait le rail suivant
625  survey_losange_uturn = false;
626  PRTDEBSTR(FIN UTURN - IMPAIR)
627  }
628  } else { //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
629  PRTDEBSTR(UTURN - PAIR)
630  //fin du rail précédent
631  pointA.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
632  pointA.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
633  //début du rail suivant
634  pointB.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
635  pointB.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
636  //milieu
637  waypoints[0].x = (pointA.x + pointB.x) / 2;
638  waypoints[0].y = (pointA.y + pointB.y) / 2;
639 
640  tempcircleradius = distrail / 2;
643  }
644 
645 
646 
647 
648  //fin du rail suivant
649  pointC.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
650  pointC.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
651 
652 
653  course_next_rail = atan2(pointC.x - pointB.x, pointC.y - pointB.y);
656 
658  while (angle_between > M_PI) { angle_between -= 2 * M_PI; }
659  while (angle_between < -M_PI) { angle_between += 2 * M_PI; }
660 
661  angle_between = DegOfRad(angle_between);
663  //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
664 
666  if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT)
667  || (angle_between > -10 && angle_between < 10)) {
668  //l'avion fait le rail suivant
669  survey_losange_uturn = false;
670  PRTDEBSTR(FIN UTURN - PAIR)
671  }
672  }
673  } else {
674  //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion
675 
676  if ((railnumber % 2) == 1) { //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
677  PRTDEBSTR(TRANSIT - IMPAIR)
678  //fin du rail précédent
679  pointA.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
680  pointA.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
681  //début du rail suivant
682  pointB.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
683  pointB.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
684  nav_route_xy(pointA.x, pointA.y, pointB.x, pointB.y);
685  if (DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT) {
686  //l'avion fait le rail suivant
687  survey_losange_uturn = false;
688  PRTDEBSTR(FIN TRANSIT - IMPAIR)
689  }
690  } else { //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
691  PRTDEBSTR(TRANSIT - PAIR)
692  //fin du rail précédent
693  pointA.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
694  pointA.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
695  //début du rail suivant
696  pointB.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
697  pointB.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
698  nav_route_xy(pointA.x, pointA.y, pointB.x, pointB.y);
699  if (DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT) {
700  //l'avion fait le rail suivant
701  survey_losange_uturn = false;
702  PRTDEBSTR(FIN TRANSIT - PAIR)
703  }
704 
705  }
706 
707  }
708  }
709 
710 
712  //NavVerticalAutoThrottleMode(0.); /* No pitch */
713  //NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
714 
715 
716 
717  cartography_periodic_downlink_carto_status = MODULES_START;
718 
719  return true; //apparament pour les fonctions de tache=> true
720 }
722 
723 
unsigned short uint16_t
Definition: types.h:16
float norm13
Definition: cartography.c:114
float distancefromrail
Definition: cartography.c:127
#define PRTDEB(TYPE, EXP)
Definition: cartography.c:54
Definition: sdio_arch.c:45
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define PRTDEBSTR(EXP)
Definition: cartography.c:57
float x
Definition: common_nav.h:40
float normAM
Definition: cartography.c:127
#define DISTXY(P1X, P1Y, P2X, P2Y)
Definition: cartography.c:71
float distrail
Definition: cartography.c:104
float tempcircleradius
Definition: cartography.c:123
float norm12
Definition: cartography.c:114
static struct point point1 point2 point3
Definition: cartography.c:111
static struct point vec12 vec13
Definition: cartography.c:113
float signforturn
Definition: cartography.c:121
float tempx
Definition: cartography.c:119
bool nav_survey_Snapshoot(void)
Definition: cartography.c:183
void start_carto(void)
Definition: cartography.c:166
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
uint16_t railnumberSinceBoot
Definition: cartography.c:84
#define FALSE
Definition: std.h:5
float circleradiusmin
Definition: cartography.c:124
bool nav_survey_Inc_railnumberSinceBoot(void)
Definition: cartography.c:177
float y
Definition: common_nav.h:41
float normBM
Definition: cartography.c:127
bool nav_survey_StopSnapshoot(void)
Definition: cartography.c:201
bool nav_survey_losange_carto(void)
Definition: cartography.c:423
float angle1213
Definition: cartography.c:120
void stop_carto(void)
Definition: cartography.c:170
float distrailinteractif
Definition: cartography.c:107
bool survey_losange_uturn
Definition: cartography.c:99
void init_carto(void)
Automatic survey of an oriented rectangle (defined by three points)
Definition: cartography.c:148
int railnumber
Definition: cartography.c:101
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static struct point pointA pointB pointC
Definition: cartography.c:112
unsigned char uint8_t
Definition: types.h:14
int numberofrailtodo
Definition: cartography.c:102
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
float angle_between
Definition: cartography.c:131
#define NORMXY(P1X, P1Y)
Definition: cartography.c:74
bool ProjectionInsideLimitOfRail
Definition: cartography.c:133
void periodic_downlink_carto(void)
Definition: cartography.c:152
float a
Definition: common_nav.h:42
#define DISTLIMIT
Definition: cartography.c:80
bool nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
Definition: cartography.c:322
bool nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4)
Definition: cartography.c:211
bool nav_survey_ComputeProjectionOnLine(struct point pointAf, struct point pointBf, float pos_xf, float pos_yf, float *normAMf, float *normBMf, float *distancefromrailf)
Definition: cartography.c:227
bool nav_survey_Snapshoot_Continu(void)
Definition: cartography.c:192
uint16_t camera_snapshot_image_number
Definition: cartography.c:92
float tempy
Definition: cartography.c:119
float distplus
Definition: cartography.c:105
float course_next_rail
Definition: cartography.c:130