Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 
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 
140 #include "mcu_periph/uart.h"
141 #include "std.h"
142 
143 
145 
146 void init_carto(void) {
147 }
148 
150  DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,&camera_snapshot_image_number);
151 }
152 
153 void start_carto(void) {
154 }
155 
156 void stop_carto(void) {
157 }
158 
159 
160 
163 {
165  return FALSE;
166 }
169 {
171  PRTDEBSTR(SNAPSHOT)
172  cartography_periodic_downlink_carto_status = MODULES_START;
173  return FALSE;
174 
175 }
178 {
180  PRTDEBSTR(SNAPSHOT)
181  cartography_periodic_downlink_carto_status = MODULES_START;
182  return TRUE;
183 
184 }
187 {
189  PRTDEBSTR(STOP SNAPSHOT)
190  cartography_periodic_downlink_carto_status = MODULES_START;
191  return FALSE;
192 
193 }
195 
197 {
198  waypoints[wp4].x=waypoints[wp2].x+waypoints[wp3].x-waypoints[wp1].x;
199  waypoints[wp4].y=waypoints[wp2].y+waypoints[wp3].y-waypoints[wp1].y;
200 
202  PRTDEB(f,waypoints[wp4].x)
203  PRTDEB(f,waypoints[wp4].y)
204  return FALSE;
205 }
206 
208 bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf);
209 
210 
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 //return if the projection of the estimator on the AB line is located inside the AB interval
213 {
214  float a,b,c,xa,xb,xc,ya,yb,yc;
215  float f;
216  float AA1;
217  float BB1;
218  float YP;
219  float XP;
220 
221  float AMx,AMy,BMx,BMy;
222  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
223 
224 
225 
226  xb=pointAf.x;
227  yb=pointAf.y;
228 
229  xc=pointBf.x;
230  yc=pointBf.y;
231 
232  xa=pos_xf;
233  ya=pos_yf;
234 
235  //calcul des parametres de la droite pointAf pointBf
236  a = yc - yb;
237  b = xb - xc;
238  c = (yb - yc) * xb + (xc - xb) * yb ;
239 
240  //calcul de la distance de la droite à l'avion
241 
242 
243  if (fabs(a)>1e-10)
244  *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
245  else
246  return 0;
247 
248  PRTDEB(f,a)
249  PRTDEB(f,b)
250  PRTDEB(f,c)
251  PRTDEB(f,*distancefromrailf)
252 
253 
254  // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC)
255  AA1 = (xc - xb);
256  BB1 = (yc - yb);
257  if (fabs(AA1)>1e-10)
258  {
259  f=(b - (a * BB1 / AA1));
260  if (fabs(f)>1e-10)
261  YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f;
262  else
263  return 0;
264  }
265  else
266  return 0;
267 
268 
269 
270 
271  XP = (-c - b * YP) / a ; //a !=0 deja testé avant
272  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
273  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
274  //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
275 
276  PRTDEB(f,AA1)
277  PRTDEB(f,BB1)
278  PRTDEB(f,YP)
279  PRTDEB(f,XP)
280 
281  AMx=XP-pointAf.x;
282  AMy=YP-pointAf.y;
283  BMx=XP-pointBf.x;
284  BMy=YP-pointBf.y;
285 
286  *normAMf=NORMXY(AMx,AMy);
287  *normBMf=NORMXY(BMx,BMy);
288 
289  PRTDEB(f,*normAMf)
290  PRTDEB(f,*normBMf)
291 
292  if ( ( (*normAMf) + (*normBMf) ) >1.05*DISTXY(pointBf.x,pointBf.y,pointAf.x,pointAf.y))
293  {
294  PRTDEBSTR(NOT INSIDE)
295  return 0;
296  }
297  else
298  {
299  PRTDEBSTR(INSIDE)
300  return 1;
301  }
302 }
304 //if distrailinit = 0, the aircraft travel from wp1 -> wp2 then do the inverse travel passing through the wp3,
305 //This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly
306 bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
307 {
308  //PRTDEBSTR(nav_survey_losange_carto_init)
310 
311 
312  point1.x=waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type
313  point1.y=waypoints[wp1].y;
314  point2.x=waypoints[wp2].x;
315  point2.y=waypoints[wp2].y;
316  point3.x=waypoints[wp3].x;
317  point3.y=waypoints[wp3].y;
318 
319  PRTDEB(u,wp1)
320  PRTDEB(f,point1.x)
321  PRTDEB(f,point1.y)
322 
323  PRTDEB(u,wp2)
324  PRTDEB(f,point2.x)
325  PRTDEB(f,point2.y)
326 
327  PRTDEB(u,wp3)
328  PRTDEB(f,point3.x)
329  PRTDEB(f,point3.y)
330 
331 
332 
333  vec12.x=point2.x-point1.x;
334  vec12.y=point2.y-point1.y;
335  PRTDEB(f,vec12.x)
336  PRTDEB(f,vec12.y)
337 
338  //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0
339  norm12=NORMXY(vec12.x,vec12.y);
340 
341  PRTDEB(f,norm12)
342 
343 
344  vec13.x=point3.x-point1.x;
345  vec13.y=point3.y-point1.y;
346  PRTDEB(f,vec13.x)
347  PRTDEB(f,vec13.y)
348 
350  PRTDEB(f,norm13)
351 
352  //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13
353  // return FALSE;
354 
355 
356  if (fabs(distrailinit)<=1)
357  { //is distrailinit==0, then the aircraft should do 2 passes to register the bands
360  }
361  else
362  {//no, so normal trajectory
363  distrail=fabs(distrailinit);
364  numberofrailtodo=ceil( norm13 / distrail);//round to the upper integer
365  }
366 
367  distplus=fabs(distplusinit);
368 
369 
370  PRTDEB(f,distrail)
371  PRTDEB(f,distplus)
373  PRTDEB(d,railnumber)
375 
376  railnumber=-1; // the state is before the first rail, which is numbered 0
377 
378  if (norm12<1e-15)
379  return FALSE;
380  if (norm13<1e-15)
381  return FALSE;
382 
383 
384  angle1213=(180/3.14159) * acos( ( ((vec12.x*vec13.x ) + (vec12.y*vec13.y) ))/(norm12*norm13));//oriented angle between 12 and 13 vectors
385 
386  angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y,vec12.x);
387  while ( angle1213 >= M_PI ) angle1213 -= 2*M_PI;
388  while ( angle1213 <= -M_PI ) angle1213 += 2*M_PI;
389 
390  PRTDEB(f,angle1213)
391 
392  if (angle1213 >0)
393  signforturn=-1;
394  else
395  signforturn=1;
396 
397 
398  return FALSE; //Init function must return false, so that the next function in the flight plan is automatically executed
399  //dans le flight_plan.h
400  // if (! (nav_survey_losange_carto()))
401  // NextStageAndBreak();
402 }
405 {
406  //test pour modifier en vol la valeur distrail
407 
408  //distrail=distrailinteractif;
409 
410 
411  //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT,
412  //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
414 
415 
416  PRTDEB(f,distrail)
417 
418 
419 
421  PRTDEB(d,railnumber)
422 
424 
425  //PRTDEB(f,stateGetPositionEnu_f()->x)
426  //PRTDEB(f,stateGetPositionEnu_f()->y)
427 
428  //sortir du bloc si données abhérantes
429  if (norm13<1e-15)
430  {
431  PRTDEBSTR(norm13<1e-15)
432  return FALSE;
433  }
434  if (norm12<1e-15)
435  {
436  PRTDEBSTR(norm13<1e-15)
437  return FALSE;
438  }
439  if (distrail<1e-15)
440  {
441  PRTDEBSTR(distrail<1e-15)
442  return FALSE;
443  }
444 
446  {
447 
448  if (railnumber==-1)
449  { //se diriger vers le début du 1°rail
450  PRTDEBSTR(approche debut rail 0)
451  pointA.x=point1.x-(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
452  pointA.y=point1.y-(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
453 
454 
455  pointB.x=point2.x+(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
456  pointB.y=point2.y+(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
457 
458  //PRTDEB(f,pointA.x)
459  //PRTDEB(f,pointA.y)
460 
461 
462  //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
463  //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
464  //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
465 
466 
468 
469  if ((DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
470  {
471  nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y,pointA.x,pointA.y);
472  //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);
473  }
474  else
475  {
476  PRTDEBSTR(debut rail 0)
477  //un fois arrivé, on commence le 1° rail;
478  railnumber=0;
480 
481  }
482  }
483 
484 
485  if (railnumber>=0)
486  {
487  pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
488  pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
489 
490  pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
491  pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
492 
493 
494 
495 
496 
497  if ((railnumber %2)==0) //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2
498  {
499  //rien a faire
500  }
501  else //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1
502  {
503  //echange pointA et B
504  tempx=pointA.x;
505  tempy=pointA.y;
506  pointA.x=pointB.x;
507  pointA.y=pointB.y;
508  pointB.x=tempx;
509  pointB.y=tempy;
510 
511  }
512 
513  // PRTDEB(f,pointA.x)
514  // PRTDEB(f,pointA.y)
515  // PRTDEB(f,pointB.x)
516  // PRTDEB(f,pointB.y)
518 
519 
520 
521  // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) &&
522  // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
523 
524 
525  if (! ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT) || ( ProjectionInsideLimitOfRail &&( normAM > (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))))
526  // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
527  {
528  nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
529  PRTDEBSTR(NAVROUTE)
530 
531 
532  //est ce que l'avion est dans la zone ou il doit prendre des images?
533  //DEJA APPELE AVANT LE IF
534  // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail);
535 
536  if ( (normAM> distplus) && (normBM> distplus) && (distancefromrail<distrail/2))
537  {
538  //CAMERA_SNAPSHOT_REQUIERED=TRUE;
539  //camera_snapshot_image_number++;
541  PRTDEBSTR(SNAPSHOT)
542  }
543 
544  }
545 
546  else // virage
547  {
548  //PRTDEBSTR(debut rail suivant)
549  railnumber++;
551 
552  PRTDEB(d,railnumber)
554  //CAMERA_SNAPSHOT_REQUIERED=TRUE;
555  //camera_snapshot_image_number++;
556 
559 
560  }
561 
563  {
565  return FALSE; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false
566  }
567 
568  }
569  }
570  else // (survey_losange_uturn==TRUE)
571  {
572 
573 
574  if (distrail<200)
575  {
576  //tourne autour d'un point à mi chemin entre les 2 rails
577 
578  //attention railnumber a été incrémenté en fin du rail précédent
579 
580  if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
581  {
582  PRTDEBSTR(UTURN-IMPAIR)
583  //fin du rail précédent
584  pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
585  pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
586  //début du rail suivant
587  pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
588  pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
589  //milieu
590  waypoints[0].x=(pointA.x+pointB.x)/2;
591  waypoints[0].y=(pointA.y+pointB.y)/2;
592 
593  tempcircleradius=distrail/2;
596 
597 
598  //fin du rail suivant
599  pointC.x=(point1.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
600  pointC.y=(point1.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
601 
602 
603  course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y);
606 
608  while (angle_between > M_PI) angle_between -= 2 * M_PI;
609  while (angle_between < -M_PI) angle_between += 2 * M_PI;
610 
611  angle_between= DegOfRad(angle_between);
613  //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
614 
616  if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT) || (angle_between> -10 && angle_between< 10) )
617  {
618  //l'avion fait le rail suivant
620  PRTDEBSTR(FIN UTURN-IMPAIR)
621  }
622  }
623  else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
624  {
625  PRTDEBSTR(UTURN-PAIR)
626  //fin du rail précédent
627  pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
628  pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
629  //début du rail suivant
630  pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
631  pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
632  //milieu
633  waypoints[0].x=(pointA.x+pointB.x)/2;
634  waypoints[0].y=(pointA.y+pointB.y)/2;
635 
636  tempcircleradius=distrail/2;
639 
640 
641 
642 
643  //fin du rail suivant
644  pointC.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
645  pointC.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
646 
647 
648  course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y);
651 
653  while (angle_between > M_PI) angle_between -= 2 * M_PI;
654  while (angle_between < -M_PI) angle_between += 2 * M_PI;
655 
656  angle_between= DegOfRad(angle_between);
658  //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
659 
661  if (( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT) || (angle_between> -10 && angle_between< 10) )
662  {
663  //l'avion fait le rail suivant
665  PRTDEBSTR(FIN UTURN-PAIR)
666  }
667  }
668  }
669  else
670  { //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion
671 
672  if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
673  {
674  PRTDEBSTR(TRANSIT-IMPAIR)
675  //fin du rail précédent
676  pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
677  pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
678  //début du rail suivant
679  pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
680  pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
681  nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
682  if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) <DISTLIMIT)
683  {
684  //l'avion fait le rail suivant
686  PRTDEBSTR(FIN TRANSIT-IMPAIR)
687  }
688  }
689  else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
690  {
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  {
701  //l'avion fait le rail suivant
703  PRTDEBSTR(FIN TRANSIT-PAIR)
704  }
705 
706  }
707 
708  }
709  }
710 
711 
713  //NavVerticalAutoThrottleMode(0.); /* No pitch */
714  //NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
715 
716 
717 
718  cartography_periodic_downlink_carto_status = MODULES_START;
719 
720  return TRUE; //apparament pour les fonctions de tache=> true
721 }
723 
724 
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:153
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:306
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:162
float angle1213
Definition: cartography.c:118
bool_t nav_survey_losange_carto(void)
Definition: cartography.c:404
void stop_carto(void)
Definition: cartography.c:156
bool_t nav_survey_Snapshoot(void)
Definition: cartography.c:168
float distrailinteractif
Definition: cartography.c:106
bool_t nav_survey_Snapshoot_Continu(void)
Definition: cartography.c:177
#define TRUE
Definition: imu_chimu.h:144
void init_carto(void)
Automatic survey of an oriented rectangle (defined by three points)
Definition: cartography.c:146
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:211
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:196
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:149
#define DISTLIMIT
Definition: cartography.c:80
int32_t y
North.
bool_t survey_losange_uturn
Definition: cartography.c:98
bool_t nav_survey_StopSnapshoot(void)
Definition: cartography.c:186
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