Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
33 #include "subsystems/nav.h"
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 
84 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
85 //the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences
86 
88 #define USE_ONBOARD_CAMERA 1
89 
90 #if USE_ONBOARD_CAMERA
92 #endif
93 
94 
95 
96 
98 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)
99 
100 int railnumber; //indicate the number of the rail being acquired
102 
103 float distrail; //distance between adjacent rails in meters, the value is set in the init function
104 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
105 
106 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
107 
108 
109 static struct point point1,point2,point3; // 3 2D points used for navigation
110 static struct point pointA,pointB,pointC; // 3 2D points used for navigation
111 static struct point vec12,vec13;
112 float norm12,norm13; // norms of 12 & 13 vectors
113 
114 
115 
116 
117 float tempx,tempy; //temporary points for exchanges
118 float angle1213; //angle between 12 & 13 vectors
119 float signforturn; //can be +1 or -1, it is used to compute the direction of the UTURN between rails
120 
121 float tempcircleradius;// used to compute the radius of the UTURN after a rail
123 
126 
127 
130 
132 
133 
134 
135 
137 #include "generated/modules.h"
138 
139 #ifndef DOWNLINK_DEVICE
140 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
141 #endif
143 #include "mcu_periph/uart.h"
144 #include "std.h"
145 
146 
148 
149 void init_carto(void) {
150 }
151 
153  DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,&camera_snapshot_image_number);
154 }
155 
156 void start_carto(void) {
157 }
158 
159 void stop_carto(void) {
160 }
161 
162 
163 
166 {
168  return FALSE;
169 }
172 {
174  PRTDEBSTR(SNAPSHOT)
175  cartography_periodic_downlink_carto_status = MODULES_START;
176  return FALSE;
177 
178 }
181 {
183  PRTDEBSTR(SNAPSHOT)
184  cartography_periodic_downlink_carto_status = MODULES_START;
185  return TRUE;
186 
187 }
190 {
192  PRTDEBSTR(STOP SNAPSHOT)
193  cartography_periodic_downlink_carto_status = MODULES_START;
194  return FALSE;
195 
196 }
198 
200 {
201  waypoints[wp4].x=waypoints[wp2].x+waypoints[wp3].x-waypoints[wp1].x;
202  waypoints[wp4].y=waypoints[wp2].y+waypoints[wp3].y-waypoints[wp1].y;
203 
205  PRTDEB(f,waypoints[wp4].x)
206  PRTDEB(f,waypoints[wp4].y)
207  return FALSE;
208 }
209 
211 bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf);
212 
213 
214 bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf)
215 //return if the projection of the estimator on the AB line is located inside the AB interval
216 {
217  float a,b,c,xa,xb,xc,ya,yb,yc;
218  float f;
219  float AA1;
220  float BB1;
221  float YP;
222  float XP;
223 
224  float AMx,AMy,BMx,BMy;
225  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
226 
227 
228 
229  xb=pointAf.x;
230  yb=pointAf.y;
231 
232  xc=pointBf.x;
233  yc=pointBf.y;
234 
235  xa=pos_xf;
236  ya=pos_yf;
237 
238  //calcul des parametres de la droite pointAf pointBf
239  a = yc - yb;
240  b = xb - xc;
241  c = (yb - yc) * xb + (xc - xb) * yb ;
242 
243  //calcul de la distance de la droite à l'avion
244 
245 
246  if (fabs(a)>1e-10)
247  *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
248  else
249  return 0;
250 
251  PRTDEB(f,a)
252  PRTDEB(f,b)
253  PRTDEB(f,c)
254  PRTDEB(f,*distancefromrailf)
255 
256 
257  // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC)
258  AA1 = (xc - xb);
259  BB1 = (yc - yb);
260  if (fabs(AA1)>1e-10)
261  {
262  f=(b - (a * BB1 / AA1));
263  if (fabs(f)>1e-10)
264  YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f;
265  else
266  return 0;
267  }
268  else
269  return 0;
270 
271 
272 
273 
274  XP = (-c - b * YP) / a ; //a !=0 deja testé avant
275  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
276  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
277  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
278 
279  PRTDEB(f,AA1)
280  PRTDEB(f,BB1)
281  PRTDEB(f,YP)
282  PRTDEB(f,XP)
283 
284  AMx=XP-pointAf.x;
285  AMy=YP-pointAf.y;
286  BMx=XP-pointBf.x;
287  BMy=YP-pointBf.y;
288 
289  *normAMf=NORMXY(AMx,AMy);
290  *normBMf=NORMXY(BMx,BMy);
291 
292  PRTDEB(f,*normAMf)
293  PRTDEB(f,*normBMf)
294 
295  if ( ( (*normAMf) + (*normBMf) ) >1.05*DISTXY(pointBf.x,pointBf.y,pointAf.x,pointAf.y))
296  {
297  PRTDEBSTR(NOT INSIDE)
298  return 0;
299  }
300  else
301  {
302  PRTDEBSTR(INSIDE)
303  return 1;
304  }
305 }
307 //if distrailinit = 0, the aircraft travel from wp1 -> wp2 then do the inverse travel passing through the wp3,
308 //This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly
309 bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
310 {
311  //PRTDEBSTR(nav_survey_losange_carto_init)
313 
314 
315  point1.x=waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type
316  point1.y=waypoints[wp1].y;
317  point2.x=waypoints[wp2].x;
318  point2.y=waypoints[wp2].y;
319  point3.x=waypoints[wp3].x;
320  point3.y=waypoints[wp3].y;
321 
322  PRTDEB(u,wp1)
323  PRTDEB(f,point1.x)
324  PRTDEB(f,point1.y)
325 
326  PRTDEB(u,wp2)
327  PRTDEB(f,point2.x)
328  PRTDEB(f,point2.y)
329 
330  PRTDEB(u,wp3)
331  PRTDEB(f,point3.x)
332  PRTDEB(f,point3.y)
333 
334 
335 
336  vec12.x=point2.x-point1.x;
337  vec12.y=point2.y-point1.y;
338  PRTDEB(f,vec12.x)
339  PRTDEB(f,vec12.y)
340 
341  //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0
342  norm12=NORMXY(vec12.x,vec12.y);
343 
344  PRTDEB(f,norm12)
345 
346 
347  vec13.x=point3.x-point1.x;
348  vec13.y=point3.y-point1.y;
349  PRTDEB(f,vec13.x)
350  PRTDEB(f,vec13.y)
351 
353  PRTDEB(f,norm13)
354 
355  //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13
356  // return FALSE;
357 
358 
359  if (fabs(distrailinit)<=1)
360  { //is distrailinit==0, then the aircraft should do 2 passes to register the bands
363  }
364  else
365  {//no, so normal trajectory
366  distrail=fabs(distrailinit);
367  numberofrailtodo=ceil( norm13 / distrail);//round to the upper integer
368  }
369 
370  distplus=fabs(distplusinit);
371 
372 
373  PRTDEB(f,distrail)
374  PRTDEB(f,distplus)
376  PRTDEB(d,railnumber)
378 
379  railnumber=-1; // the state is before the first rail, which is numbered 0
380 
381  if (norm12<1e-15)
382  return FALSE;
383  if (norm13<1e-15)
384  return FALSE;
385 
386 
387  angle1213=(180/3.14159) * acos( ( ((vec12.x*vec13.x ) + (vec12.y*vec13.y) ))/(norm12*norm13));//oriented angle between 12 and 13 vectors
388 
389  angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y,vec12.x);
390  while ( angle1213 >= M_PI ) angle1213 -= 2*M_PI;
391  while ( angle1213 <= -M_PI ) angle1213 += 2*M_PI;
392 
393  PRTDEB(f,angle1213)
394 
395  if (angle1213 >0)
396  signforturn=-1;
397  else
398  signforturn=1;
399 
400 
401  return FALSE; //Init function must return false, so that the next function in the flight plan is automatically executed
402  //dans le flight_plan.h
403  // if (! (nav_survey_losange_carto()))
404  // NextStageAndBreak();
405 }
408 {
409  //test pour modifier en vol la valeur distrail
410 
411  //distrail=distrailinteractif;
412 
413 
414  //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT,
415  //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
417 
418 
419  PRTDEB(f,distrail)
420 
421 
422 
424  PRTDEB(d,railnumber)
425 
427 
428  //PRTDEB(f,stateGetPositionEnu_f()->x)
429  //PRTDEB(f,stateGetPositionEnu_f()->y)
430 
431  //sortir du bloc si données abhérantes
432  if (norm13<1e-15)
433  {
434  PRTDEBSTR(norm13<1e-15)
435  return FALSE;
436  }
437  if (norm12<1e-15)
438  {
439  PRTDEBSTR(norm13<1e-15)
440  return FALSE;
441  }
442  if (distrail<1e-15)
443  {
444  PRTDEBSTR(distrail<1e-15)
445  return FALSE;
446  }
447 
449  {
450 
451  if (railnumber==-1)
452  { //se diriger vers le début du 1°rail
453  PRTDEBSTR(approche debut rail 0)
454  pointA.x=point1.x-(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
455  pointA.y=point1.y-(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
456 
457 
458  pointB.x=point2.x+(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
459  pointB.y=point2.y+(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
460 
461  //PRTDEB(f,pointA.x)
462  //PRTDEB(f,pointA.y)
463 
464 
465  //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
466  //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
467  //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
468 
469 
471 
472  if ((DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
473  {
474  nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y,pointA.x,pointA.y);
475  //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);
476  }
477  else
478  {
479  PRTDEBSTR(debut rail 0)
480  //un fois arrivé, on commence le 1° rail;
481  railnumber=0;
483 
484  }
485  }
486 
487 
488  if (railnumber>=0)
489  {
490  pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
491  pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
492 
493  pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
494  pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
495 
496 
497 
498 
499 
500  if ((railnumber %2)==0) //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2
501  {
502  //rien a faire
503  }
504  else //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1
505  {
506  //echange pointA et B
507  tempx=pointA.x;
508  tempy=pointA.y;
509  pointA.x=pointB.x;
510  pointA.y=pointB.y;
511  pointB.x=tempx;
512  pointB.y=tempy;
513 
514  }
515 
516  // PRTDEB(f,pointA.x)
517  // PRTDEB(f,pointA.y)
518  // PRTDEB(f,pointB.x)
519  // PRTDEB(f,pointB.y)
521 
522 
523 
524  // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) &&
525  // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
526 
527 
528  if (! ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT) || ( ProjectionInsideLimitOfRail &&( normAM > (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))))
529  // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
530  {
531  nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
532  PRTDEBSTR(NAVROUTE)
533 
534 
535  //est ce que l'avion est dans la zone ou il doit prendre des images?
536  //DEJA APPELE AVANT LE IF
537  // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail);
538 
539  if ( (normAM> distplus) && (normBM> distplus) && (distancefromrail<distrail/2))
540  {
541  //CAMERA_SNAPSHOT_REQUIERED=TRUE;
542  //camera_snapshot_image_number++;
544  PRTDEBSTR(SNAPSHOT)
545  }
546 
547  }
548 
549  else // virage
550  {
551  //PRTDEBSTR(debut rail suivant)
552  railnumber++;
554 
555  PRTDEB(d,railnumber)
557  //CAMERA_SNAPSHOT_REQUIERED=TRUE;
558  //camera_snapshot_image_number++;
559 
562 
563  }
564 
566  {
568  return FALSE; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false
569  }
570 
571  }
572  }
573  else // (survey_losange_uturn==TRUE)
574  {
575 
576 
577  if (distrail<200)
578  {
579  //tourne autour d'un point à mi chemin entre les 2 rails
580 
581  //attention railnumber a été incrémenté en fin du rail précédent
582 
583  if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
584  {
585  PRTDEBSTR(UTURN-IMPAIR)
586  //fin du rail précédent
587  pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
588  pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
589  //début du rail suivant
590  pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
591  pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
592  //milieu
593  waypoints[0].x=(pointA.x+pointB.x)/2;
594  waypoints[0].y=(pointA.y+pointB.y)/2;
595 
596  tempcircleradius=distrail/2;
599 
600 
601  //fin du rail suivant
602  pointC.x=(point1.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
603  pointC.y=(point1.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
604 
605 
606  course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y);
609 
611  while (angle_between > M_PI) angle_between -= 2 * M_PI;
612  while (angle_between < -M_PI) angle_between += 2 * M_PI;
613 
614  angle_between= DegOfRad(angle_between);
616  //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
617 
619  if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT) || (angle_between> -10 && angle_between< 10) )
620  {
621  //l'avion fait le rail suivant
623  PRTDEBSTR(FIN UTURN-IMPAIR)
624  }
625  }
626  else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
627  {
628  PRTDEBSTR(UTURN-PAIR)
629  //fin du rail précédent
630  pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
631  pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
632  //début du rail suivant
633  pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
634  pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
635  //milieu
636  waypoints[0].x=(pointA.x+pointB.x)/2;
637  waypoints[0].y=(pointA.y+pointB.y)/2;
638 
639  tempcircleradius=distrail/2;
642 
643 
644 
645 
646  //fin du rail suivant
647  pointC.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
648  pointC.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
649 
650 
651  course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y);
654 
656  while (angle_between > M_PI) angle_between -= 2 * M_PI;
657  while (angle_between < -M_PI) angle_between += 2 * M_PI;
658 
659  angle_between= DegOfRad(angle_between);
661  //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
662 
664  if (( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT) || (angle_between> -10 && angle_between< 10) )
665  {
666  //l'avion fait le rail suivant
668  PRTDEBSTR(FIN UTURN-PAIR)
669  }
670  }
671  }
672  else
673  { //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion
674 
675  if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
676  {
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  {
687  //l'avion fait le rail suivant
689  PRTDEBSTR(FIN TRANSIT-IMPAIR)
690  }
691  }
692  else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
693  {
694  PRTDEBSTR(TRANSIT-PAIR)
695  //fin du rail précédent
696  pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
697  pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
698  //début du rail suivant
699  pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
700  pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
701  nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
702  if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT)
703  {
704  //l'avion fait le rail suivant
706  PRTDEBSTR(FIN TRANSIT-PAIR)
707  }
708 
709  }
710 
711  }
712  }
713 
714 
716  //NavVerticalAutoThrottleMode(0.); /* No pitch */
717  //NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
718 
719 
720 
721  cartography_periodic_downlink_carto_status = MODULES_START;
722 
723  return TRUE; //apparament pour les fonctions de tache=> true
724 }
726 
727 
unsigned short uint16_t
Definition: types.h:16
float norm13
Definition: cartography.c:112
float distancefromrail
Definition: cartography.c:125
#define PRTDEB(TYPE, EXP)
Definition: cartography.c:54
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define PRTDEBSTR(EXP)
Definition: cartography.c:57
float normAM
Definition: cartography.c:125
#define DISTXY(P1X, P1Y, P2X, P2Y)
Definition: cartography.c:71
float distrail
Definition: cartography.c:103
float tempcircleradius
Definition: cartography.c:121
float norm12
Definition: cartography.c:112
static struct point point1 point2 point3
Definition: cartography.c:109
static struct point vec12 vec13
Definition: cartography.c:111
float signforturn
Definition: cartography.c:119
float a
Definition: common_nav.h:42
float tempx
Definition: cartography.c:117
void start_carto(void)
Definition: cartography.c:156
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
Definition: cartography.c:309
uint16_t railnumberSinceBoot
Definition: cartography.c:84
#define FALSE
Definition: imu_chimu.h:141
float circleradiusmin
Definition: cartography.c:122
float normBM
Definition: cartography.c:125
bool_t nav_survey_Inc_railnumberSinceBoot(void)
Definition: cartography.c:165
float angle1213
Definition: cartography.c:118
bool_t nav_survey_losange_carto(void)
Definition: cartography.c:407
void stop_carto(void)
Definition: cartography.c:159
bool_t nav_survey_Snapshoot(void)
Definition: cartography.c:171
float distrailinteractif
Definition: cartography.c:106
bool_t nav_survey_Snapshoot_Continu(void)
Definition: cartography.c:180
#define TRUE
Definition: imu_chimu.h:144
void init_carto(void)
Automatic survey of an oriented rectangle (defined by three points)
Definition: cartography.c:149
int railnumber
Definition: cartography.c:100
bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf, struct point pointBf, float pos_xf, float pos_yf, float *normAMf, float *normBMf, float *distancefromrailf)
Definition: cartography.c:214
static struct point pointA pointB pointC
Definition: cartography.c:110
unsigned char uint8_t
Definition: types.h:14
int numberofrailtodo
Definition: cartography.c:101
API to get/set the generic vehicle states.
bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4)
Definition: cartography.c:199
float y
Definition: common_nav.h:41
float angle_between
Definition: cartography.c:129
#define NORMXY(P1X, P1Y)
Definition: cartography.c:74
void periodic_downlink_carto(void)
Definition: cartography.c:152
#define DISTLIMIT
Definition: cartography.c:80
int32_t y
North.
static struct point c
Definition: discsurvey.c:39
bool_t survey_losange_uturn
Definition: cartography.c:98
bool_t nav_survey_StopSnapshoot(void)
Definition: cartography.c:189
float x
Definition: common_nav.h:40
int32_t x
East.
uint16_t camera_snapshot_image_number
Definition: cartography.c:91
float tempy
Definition: cartography.c:117
float distplus
Definition: cartography.c:104
bool_t ProjectionInsideLimitOfRail
Definition: cartography.c:131
float course_next_rail
Definition: cartography.c:128