diff --git a/conf/airframes/LAAS/mmlaas_N1.xml b/conf/airframes/LAAS/mmlaas_N1.xml
index 470ae5aa94..59cacc5ea6 100644
--- a/conf/airframes/LAAS/mmlaas_N1.xml
+++ b/conf/airframes/LAAS/mmlaas_N1.xml
@@ -42,7 +42,12 @@
-
+
+
+
+
+
+
diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml
new file mode 100644
index 0000000000..21b8968ffe
--- /dev/null
+++ b/conf/flight_plans/versatile_carto_fixe_muret.xml
@@ -0,0 +1,160 @@
+
+
+
+
+
+
+#include "datalink.h"
+/*
+#ifndef VARDECLARED
+#define VARDECLARED
+float varsweepsize=110;
+#endif
+*/
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/cartography.xml b/conf/modules/cartography.xml
new file mode 100644
index 0000000000..04e3d9d4de
--- /dev/null
+++ b/conf/modules/cartography.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+#Exemple of RAW makefile part
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
new file mode 100644
index 0000000000..bd5febc023
--- /dev/null
+++ b/sw/airborne/modules/cartography/cartography.c
@@ -0,0 +1,745 @@
+/*
+ * $Id: cartography.c 2011-04-20 10:43:13Z $
+ *
+ * Copyright (C) 2011 Vandeportaele Bertrand
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/** \file cartography.c
+ * \brief Navigation functions for cartography of the ground
+ *
+ */
+
+
+
+
+#include "estimator.h"
+#include "stdio.h"
+
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+
+#include "std.h" //macros pas mal dans sw/include
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+//for fast debbuging, the simulation can be accelerated using the gaia software from an xterm console
+// /home/bvdp/paparazzi3/sw/simulator/gaia
+////////////////////////////////////////////////////////////////////////////////////////////////
+// for explanations about debugging macros:
+//http://gcc.gnu.org/onlinedocs/cpp/Stringification.html#Stringification
+
+// Be carefull not to use printf function in ap compilation, only use it in sim compilation
+// the DEBUG_PRINTF should be defined only in the sim part of the makefile airframe file
+#ifdef DEBUG_PRINTF
+int CPTDEBUG=0;
+#define PRTDEB(TYPE,EXP) \
+printf("%5d: " #EXP ": %"#TYPE"\n",CPTDEBUG,EXP);fflush(stdout);CPTDEBUG++;
+#define PRTDEBSTR(EXP) \
+printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++;
+#else
+#define PRTDEB(TYPE,EXP) \
+;
+
+#define PRTDEBSTR(EXP) \
+;
+#endif
+
+/*
+ exemple of use for theese macros
+ PRTDEBSTR(Init polysurvey)
+ PRTDEB(u,SurveySize)
+
+ PRTDEB(lf,PolygonCenter.x)
+ PRTDEB(lf,PolygonCenter.y)
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define DISTXY(P1X,P1Y,P2X,P2Y)\
+(sqrt( ( (P2X-P1X) * (P2X-P1X) ) + ( (P2Y-P1Y) * (P2Y-P1Y) ) ) )
+
+#define NORMXY(P1X,P1Y)\
+(sqrt( ( (P1X) * (P1X) ) + ( (P1Y) * (P1Y) ) ) )
+
+
+//max distance between the estimator position and an objective points to consider that the objective points is atteined
+
+#define DISTLIMIT 30
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+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
+//the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+#define USE_ONBOARD_CAMERA
+
+#ifdef USE_ONBOARD_CAMERA
+bool_t CAMERA_SNAPSHOT_REQUIERED=FALSE; //declared in main_ap.c
+uint16_t camera_snapshot_image_number=0; //declared in main_ap.c
+#endif
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+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)
+
+int railnumber; //indicate the number of the rail being acquired
+int numberofrailtodo;
+
+float distrail; //distance between adjacent rails in meters, the value is set in the init function
+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
+
+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
+
+
+static struct point point1,point2,point3; // 3 2D points used for navigation
+static struct point pointA,pointB,pointC; // 3 2D points used for navigation
+static struct point vec12,vec13;
+float norm12,norm13; // norms of 12 & 13 vectors
+
+
+
+
+float tempx,tempy; //temporary points for exchanges
+float angle1213; //angle between 12 & 13 vectors
+float signforturn; //can be +1 or -1, it is used to compute the direction of the UTURN between rails
+
+float tempcircleradius;// used to compute the radius of the UTURN after a rail
+float circleradiusmin=40;
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+float normBM,normAM,distancefromrail;
+
+
+float course_next_rail;
+float angle_between;
+
+bool_t ProjectionInsideLimitOfRail;
+
+
+
+
+#include "modules/cartography/cartography.h"
+#include "led.h"
+
+#include "downlink.h"
+#include "mcu_periph/uart.h"
+#include "std.h"
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+void init_carto(void) {
+ // this part is already done by led_init in fact
+ LED_INIT(DEMO_MODULE_LED);
+ LED_OFF(DEMO_MODULE_LED);
+}
+
+void periodic_1Hz_carto(void) {
+ /*uint8_t toto=3;
+ float lat=3.14159;
+ float lon=1.234;
+ LED_TOGGLE(DEMO_MODULE_LED);
+ DOWNLINK_SEND_MARK(DefaultChannel,&toto,&lat,&lon);
+ */
+}
+
+void periodic_10Hz_carto(void) {
+ /* LED_TOGGLE(DEMO_MODULE_LED);
+ */
+}
+
+void start_carto(void) {
+ /* LED_ON(DEMO_MODULE_LED);
+ */
+}
+
+void stop_carto(void) {
+ /* LED_OFF(DEMO_MODULE_LED);
+ */
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Inc_railnumberSinceBoot(void)
+{
+ railnumberSinceBoot++;
+ return FALSE;
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Snapshoot(void)
+{
+ camera_snapshot_image_number=railnumberSinceBoot;
+ PRTDEBSTR(SNAPSHOT)
+ CAMERA_SNAPSHOT_REQUIERED=TRUE;
+ return FALSE;
+
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Snapshoot_Continu(void)
+{
+ camera_snapshot_image_number=railnumberSinceBoot;
+ PRTDEBSTR(SNAPSHOT)
+ CAMERA_SNAPSHOT_REQUIERED=TRUE;
+ return TRUE;
+
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_StopSnapshoot(void)
+{
+ camera_snapshot_image_number=0;
+ PRTDEBSTR(STOP SNAPSHOT)
+ CAMERA_SNAPSHOT_REQUIERED=TRUE;
+ return FALSE;
+
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 )
+{
+ waypoints[wp4].x=waypoints[wp2].x+waypoints[wp3].x-waypoints[wp1].x;
+ waypoints[wp4].y=waypoints[wp2].y+waypoints[wp3].y-waypoints[wp1].y;
+
+ PRTDEBSTR(nav_survey_computefourth_corner)
+ PRTDEB(f,waypoints[wp4].x)
+ PRTDEB(f,waypoints[wp4].y)
+ return FALSE;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf);
+
+
+bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf)
+//return if the projection of the estimator on the AB line is located inside the AB interval
+{
+ float a,b,c,xa,xb,xc,ya,yb,yc;
+ float f;
+ float AA1;
+ float BB1;
+ float YP;
+ float XP;
+
+ float AMx,AMy,BMx,BMy;
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+
+
+
+ xb=pointAf.x;
+ yb=pointAf.y;
+
+ xc=pointBf.x;
+ yc=pointBf.y;
+
+ xa=estimator_xf;
+ ya=estimator_yf;
+
+ //calcul des parametres de la droite pointAf pointBf
+ a = yc - yb;
+ b = xb - xc;
+ c = (yb - yc) * xb + (xc - xb) * yb ;
+
+ //calcul de la distance de la droite à l'avion
+
+
+ if (fabs(a)>1e-10)
+ *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
+ else
+ return 0;
+
+ PRTDEB(f,a)
+ PRTDEB(f,b)
+ PRTDEB(f,c)
+ PRTDEB(f,*distancefromrailf)
+
+
+ // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC)
+ AA1 = (xc - xb);
+ BB1 = (yc - yb);
+ if (fabs(AA1)>1e-10)
+ {
+ f=(b - (a * BB1 / AA1));
+ if (fabs(f)>1e-10)
+ YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f;
+ else
+ return 0;
+ }
+ else
+ return 0;
+
+
+
+
+ XP = (-c - b * YP) / a ; //a !=0 deja testé avant
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+
+ PRTDEB(f,AA1)
+ PRTDEB(f,BB1)
+ PRTDEB(f,YP)
+ PRTDEB(f,XP)
+
+ AMx=XP-pointAf.x;
+ AMy=YP-pointAf.y;
+ BMx=XP-pointBf.x;
+ BMy=YP-pointBf.y;
+
+ *normAMf=NORMXY(AMx,AMy);
+ *normBMf=NORMXY(BMx,BMy);
+
+ PRTDEB(f,*normAMf)
+ PRTDEB(f,*normBMf)
+
+ if ( ( (*normAMf) + (*normBMf) ) >1.05*DISTXY(pointBf.x,pointBf.y,pointAf.x,pointAf.y))
+ {
+ PRTDEBSTR(NOT INSIDE)
+ return 0;
+ }
+ else
+ {
+ PRTDEBSTR(INSIDE)
+ return 1;
+ }
+}
+///////////////////////////////////////////////////////////////////////////
+//if distrailinit = 0, the aircraft travel from wp1 -> wp2 then do the inverse travel passing through the wp3,
+//This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly
+bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
+{
+ //PRTDEBSTR(nav_survey_losange_carto_init)
+ survey_losange_uturn=FALSE;
+
+
+ point1.x=waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type
+ point1.y=waypoints[wp1].y;
+ point2.x=waypoints[wp2].x;
+ point2.y=waypoints[wp2].y;
+ point3.x=waypoints[wp3].x;
+ point3.y=waypoints[wp3].y;
+
+ PRTDEB(u,wp1)
+ PRTDEB(f,point1.x)
+ PRTDEB(f,point1.y)
+
+ PRTDEB(u,wp2)
+ PRTDEB(f,point2.x)
+ PRTDEB(f,point2.y)
+
+ PRTDEB(u,wp3)
+ PRTDEB(f,point3.x)
+ PRTDEB(f,point3.y)
+
+
+
+ vec12.x=point2.x-point1.x;
+ vec12.y=point2.y-point1.y;
+ PRTDEB(f,vec12.x)
+ PRTDEB(f,vec12.y)
+
+ //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0
+ norm12=NORMXY(vec12.x,vec12.y);
+
+ PRTDEB(f,norm12)
+
+
+ vec13.x=point3.x-point1.x;
+ vec13.y=point3.y-point1.y;
+ PRTDEB(f,vec13.x)
+ PRTDEB(f,vec13.y)
+
+ norm13=NORMXY(vec13.x,vec13.y);
+ PRTDEB(f,norm13)
+
+ //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13
+ // return FALSE;
+
+
+ if (fabs(distrailinit)<=1)
+ { //is distrailinit==0, then the aircraft should do 2 passes to register the bands
+ distrail=norm13;
+ numberofrailtodo=1;
+ }
+ else
+ {//no, so normal trajectory
+ distrail=fabs(distrailinit);
+ numberofrailtodo=ceil( norm13 / distrail);//round to the upper integer
+ }
+
+ distplus=fabs(distplusinit);
+
+
+ PRTDEB(f,distrail)
+ PRTDEB(f,distplus)
+ PRTDEB(d,numberofrailtodo)
+ PRTDEB(d,railnumber)
+ PRTDEB(d,railnumberSinceBoot)
+
+ railnumber=-1; // the state is before the first rail, which is numbered 0
+
+ if (norm12<1e-15)
+ return FALSE;
+ if (norm13<1e-15)
+ return FALSE;
+
+
+ angle1213=(180/3.14159) * acos( ( ((vec12.x*vec13.x ) + (vec12.y*vec13.y) ))/(norm12*norm13));//oriented angle between 12 and 13 vectors
+
+ angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y,vec12.x);
+ while ( angle1213 >= M_PI ) angle1213 -= 2*M_PI;
+ while ( angle1213 <= -M_PI ) angle1213 += 2*M_PI;
+
+ PRTDEB(f,angle1213)
+
+ if (angle1213 >0)
+ signforturn=-1;
+ else
+ signforturn=1;
+
+
+ return FALSE; //Init function must return false, so that the next function in the flight plan is automatically executed
+ //dans le flight_plan.h
+ // if (! (nav_survey_losange_carto()))
+ // NextStageAndBreak();
+}
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_losange_carto(void)
+{
+ //test pour modifier en vol la valeur distrail
+
+ //distrail=distrailinteractif;
+
+
+ //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT,
+ //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
+ camera_snapshot_image_number=0;
+
+
+ PRTDEB(f,distrail)
+
+
+
+ PRTDEBSTR(nav_survey_losange_carto)
+ PRTDEB(d,railnumber)
+
+ PRTDEB(d,railnumberSinceBoot)
+
+ //PRTDEB(f,estimator_x)
+ //PRTDEB(f,estimator_y)
+
+ //sortir du bloc si données abhérantes
+ if (norm13<1e-15)
+ {
+ PRTDEBSTR(norm13<1e-15)
+ return FALSE;
+ }
+ if (norm12<1e-15)
+ {
+ PRTDEBSTR(norm13<1e-15)
+ return FALSE;
+ }
+ if (distrail<1e-15)
+ {
+ PRTDEBSTR(distrail<1e-15)
+ return FALSE;
+ }
+
+ if (survey_losange_uturn==FALSE)
+ {
+
+ if (railnumber==-1)
+ { //se diriger vers le début du 1°rail
+ PRTDEBSTR(approche debut rail 0)
+ pointA.x=point1.x-(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
+ pointA.y=point1.y-(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
+
+
+ pointB.x=point2.x+(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
+ pointB.y=point2.y+(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
+
+ //PRTDEB(f,pointA.x)
+ //PRTDEB(f,pointA.y)
+
+
+ //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
+ //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT)
+ //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT)
+
+
+ nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+
+ if ((DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+ {
+ nav_route_xy(estimator_x, estimator_y,pointA.x,pointA.y);
+ //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);
+ }
+ else
+ {
+ PRTDEBSTR(debut rail 0)
+ //un fois arrivé, on commence le 1° rail;
+ railnumber=0;
+ railnumberSinceBoot++;
+
+ }
+ }
+
+
+ if (railnumber>=0)
+ {
+ pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+
+ pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+
+
+
+
+
+ if ((railnumber %2)==0) //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2
+ {
+ //rien a faire
+ }
+ else //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1
+ {
+ //echange pointA et B
+ tempx=pointA.x;
+ tempy=pointA.y;
+ pointA.x=pointB.x;
+ pointA.y=pointB.y;
+ pointB.x=tempx;
+ pointB.y=tempy;
+
+ }
+
+ // PRTDEB(f,pointA.x)
+ // PRTDEB(f,pointA.y)
+ // PRTDEB(f,pointB.x)
+ // PRTDEB(f,pointB.y)
+ ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+
+
+
+ // if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) >DISTLIMIT) &&
+ // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+
+
+ if (! ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))))
+ // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+ {
+ nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
+ PRTDEBSTR(NAVROUTE)
+
+
+ //est ce que l'avion est dans la zone ou il doit prendre des images?
+ //DEJA APPELE AVANT LE IF
+ // nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+
+ if ( (normAM> distplus) && (normBM> distplus) && (distancefromrailnumberofrailtodo)
+ {
+ PRTDEBSTR(fin nav_survey_losange_carto)
+ return FALSE; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false
+ }
+
+ }
+ }
+ else // (survey_losange_uturn==TRUE)
+ {
+
+
+ if (distrail<200)
+ {
+ //tourne autour d'un point à mi chemin entre les 2 rails
+
+ //attention railnumber a été incrémenté en fin du rail précédent
+
+ if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
+ {
+ PRTDEBSTR(UTURN-IMPAIR)
+ //fin du rail précédent
+ pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+ pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+ //début du rail suivant
+ pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+ //milieu
+ waypoints[0].x=(pointA.x+pointB.x)/2;
+ waypoints[0].y=(pointA.y+pointB.y)/2;
+
+ tempcircleradius=distrail/2;
+ if(tempcircleradius M_PI) angle_between -= 2 * M_PI;
+ while (angle_between < -M_PI) angle_between += 2 * M_PI;
+
+ angle_between= DegOfRad(angle_between);
+ PRTDEB(f,angle_between )
+ //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
+
+ NavCircleWaypoint(0,signforturn*tempcircleradius);
+ if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) )
+ {
+ //l'avion fait le rail suivant
+ survey_losange_uturn=FALSE;
+ PRTDEBSTR(FIN UTURN-IMPAIR)
+ }
+ }
+ else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
+ {
+ PRTDEBSTR(UTURN-PAIR)
+ //fin du rail précédent
+ pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+ pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+ //début du rail suivant
+ pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+ //milieu
+ waypoints[0].x=(pointA.x+pointB.x)/2;
+ waypoints[0].y=(pointA.y+pointB.y)/2;
+
+ tempcircleradius=distrail/2;
+ if(tempcircleradius M_PI) angle_between -= 2 * M_PI;
+ while (angle_between < -M_PI) angle_between += 2 * M_PI;
+
+ angle_between= DegOfRad(angle_between);
+ PRTDEB(f,angle_between )
+ //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
+
+ NavCircleWaypoint(0,signforturn*(-1)*tempcircleradius);
+ if (( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) )
+ {
+ //l'avion fait le rail suivant
+ survey_losange_uturn=FALSE;
+ PRTDEBSTR(FIN UTURN-PAIR)
+ }
+ }
+ }
+ else
+ { //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion
+
+ if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
+ {
+ PRTDEBSTR(TRANSIT-IMPAIR)
+ //fin du rail précédent
+ pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+ pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+ //début du rail suivant
+ pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+ nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
+ if ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) true
+}
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+
diff --git a/sw/airborne/modules/cartography/cartography.h b/sw/airborne/modules/cartography/cartography.h
new file mode 100644
index 0000000000..2ba83d9acf
--- /dev/null
+++ b/sw/airborne/modules/cartography/cartography.h
@@ -0,0 +1,52 @@
+/** Automatic survey of an oriented rectangle (defined by three points) */
+
+#ifndef CARTOGRAPHY_H
+#define CARTOGRAPHY_H
+
+
+
+
+void init_carto(void);
+void periodic_1Hz_carto(void);
+void periodic_10Hz_carto(void);
+void start_carto(void);
+void stop_carto(void);
+
+/*
+ typedef enum {NS, WE} survey_orientation_t;
+ */
+
+
+#ifdef USE_ONBOARD_CAMERA
+extern bool_t CAMERA_SNAPSHOT_REQUIERED;
+extern uint16_t camera_snapshot_image_number;
+#endif
+
+extern float distrailinteractif; //pour exporter la variable et pouvoir la changer depuis settings
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+
+extern bool_t nav_survey_Inc_railnumberSinceBoot(void);
+extern bool_t nav_survey_Snapshoot(void);
+bool_t nav_survey_Snapshoot_Continu(void);
+extern bool_t nav_survey_StopSnapshoot(void);
+extern bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 );
+
+extern bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrail, float distplus);
+
+extern bool_t nav_survey_losange_carto(void); // !!!! important il faut mettre void en parametres d'entrée, sinon le compilo dit: attention : function declaration isn»t a prototype
+
+//(uint8_t wp1, uint8_t wp2, uint8_t wp3);
+
+/*
+ #define NavSurveylosange_cartoInit(_wp1, _wp2, _grid, _distplus) nav_survey_losange_init(_wp1, _wp2, _wp3, _grid, _distplus)
+ #define NavSurveylosange_carto nav_survey_losange
+ */
+
+
+
+#endif
+
\ No newline at end of file