<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">

<flight_plan alt="240" ground_alt="190" lat0="43.4622" lon0="1.2729" max_dist_from_home="1200" name="Versatile" qfu="270" security_height="60">

<!--<flight_plan alt="240" ground_alt="190" lat0="43.224622" lon0="1.2729" max_dist_from_home="900" name="Versatile" qfu="270" security_height="25"> -->
  <header>
#include "datalink.h"
/*
#ifndef VARDECLARED
#define VARDECLARED
float varsweepsize=110;
#endif
*/
</header>
  <waypoints>
    <waypoint name="HOME" x="0" y="0"/>
    <waypoint name="STDBY" x="20" y="80"/>
    <waypoint name="1" x="44.8" y="102.2"/>
    <waypoint name="2" x="-63.5" y="122.9"/>
    <waypoint name="MOB" x="-11.5" y="-21.2"/>
    <waypoint name="S1" x="-177.6" y="11.6"/>
    <waypoint name="S2" x="173.9" y="-41.3"/>
    <waypoint name="S3" x="-60.6" y="310.9"/>
    <waypoint name="S4" x="-204.7" y="17.0"/>
    <waypoint alt="30" name="AF" x="200" y="-10"/>
    <waypoint alt="0" name="TD" x="80.0" y="20.0"/>
    <waypoint name="BASELEG" x="26.9" y="-23.0"/>
    <waypoint name="_1" x="-100" y="0"/>
    <waypoint name="_2" x="-100" y="200"/>
    <waypoint name="_3" x="100" y="200"/>
    <waypoint name="_4" x="100" y="0"/>
    <waypoint name="CLIMB" x="-122.5" y="35.4"/>
    <waypoint name="SP1" x="-57.2" y="9.8"/>
    <waypoint name="SP2" x="54.2" y="-41.7"/>
    <waypoint name="SP3" x="-15.7" y="88.6"/>
    <waypoint name="SP4" x="97.5" y="37.8"/>
  </waypoints>
<!--  <sectors>
    <sector name="PolySectorS">
      <corner name="S1"/>
      <corner name="S2"/>
      <corner name="S3"/>
      <corner name="S4"/>
    </sector>
  </sectors>-->
  <exceptions/>
  <blocks>
    <block name="Wait GPS">
      <set value="1" var="kill_throttle"/>
      <while cond="!GpsFixValid()"/>
    </block>
    <block name="Holding point">
      <set value="1" var="kill_throttle"/>
      <set value="NAV_MODE_ROLL" var="nav_mode"/>
      <attitude roll="0" throttle="0" vmode="throttle"/>
    </block>
    <block name="Geo init">
      <while cond="LessThan(NavBlockTime(), 10)"/>
      <call fun="NavSetGroundReferenceHere()"/>
      <deroute block="Holding point"/>
    </block>

    <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
      <set value="0" var="kill_throttle"/>
      <go wp="CLIMB"/>
    </block>
    <block name="Standby" strip_button="Standby" strip_icon="home.png">
      <circle radius="nav_radius" wp="STDBY"/>
    </block>



    <block name="IncRailNumber" strip_button="Standby" strip_icon="home.png">
		    <call fun="nav_survey_Inc_railnumberSinceBoot()"/>
	    <circle radius="nav_radius" wp="STDBY"/>
    </block>

    <block name="Snapshoot" strip_button="Standby" strip_icon="home.png">
		    <call fun="nav_survey_Snapshoot()"/>
	    <circle radius="nav_radius" wp="STDBY"/>
    </block>

    <block name="SnapshootContinu" strip_button="Standby" strip_icon="home.png">
		    <call fun="nav_survey_Snapshoot_Continu()"/>
	    <circle radius="nav_radius" wp="STDBY"/>
    </block>

    <block name="StopSnapshoot" strip_button="Standby" strip_icon="home.png">
		    <call fun="nav_survey_StopSnapshoot()"/>
	    <circle radius="nav_radius" wp="STDBY"/>
    </block>




    <block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png">
      <oval p1="1" p2="2" radius="nav_radius"/>
    </block>
    <block name="MOB" strip_button="Turn around here" strip_icon="mob.png">
      <call fun="NavSetWaypointHere(WP_MOB)"/>
      <circle radius="100" wp="MOB"/>
    </block>



  <block name="init_display_carto">
      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
    </block>

    <block name="Monter">
      <call fun="NavSetWaypointHere(WP_MOB)"/>
      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
      <circle alt="ground_alt+105" radius="nav_radius" wp="MOB"/>
      <exception cond="estimator_z > ground_alt+100" deroute="Carto survey S1-S2-S3"/>
    </block>


    <block name="Carto survey S1-S2-S3" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
      <call fun="nav_survey_losange_carto_init(WP_S1,WP_S2,WP_S3, 50,150)"/>
      <call fun="nav_survey_losange_carto()"/>
    </block>
    <block name="Carto survey S1-S3-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
      <call fun="nav_survey_losange_carto_init(WP_S1,WP_S3,WP_S2, 0,150)"/>
      <call fun="nav_survey_losange_carto()"/>
    </block>
    <block name="Fin Carto" strip_button="Standby" strip_icon="home.png">
      <circle radius="nav_radius" wp="STDBY"/>
    </block>


  <block name="init_display_carto Fin">
      <call fun="nav_survey_computefourth_corner(WP_SP1,WP_SP2,WP_SP3,WP_SP4)"/>
    </block>


    <block name="Descendre">
      <call fun="NavSetWaypointHere(WP_MOB)"/>
      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
      <circle alt="ground_alt+35" radius="nav_radius" wp="MOB"/>
      <exception cond="ground_alt+40 > estimator_z   " deroute="Carto survey SP1-SP2-SP3 fin"/>
    </block>

    <block name="Carto survey SP1-SP2-SP3 fin" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
      <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP2,WP_SP3, 20,150)"/>
      <call fun="nav_survey_losange_carto()"/>
    </block>
    <block name="Carto survey SP1-SP3-SP2 fin " strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
      <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP3,WP_SP2, 0,150)"/>
      <call fun="nav_survey_losange_carto()"/>
    </block>
    <block name="Fin Carto fin " strip_button="Standby" strip_icon="home.png">
      <circle radius="nav_radius" wp="STDBY"/>
    </block>


    <block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
      <survey_rectangle grid="150" wp1="S1" wp2="S2"/>
    </block>
  </blocks>
</flight_plan>