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