<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">

<flight_plan alt="1000" ground_alt="840" lat0="65.007419" lon0="-18.895916" max_dist_from_home="4500" name="Ingolfsskali" qfu="0" security_height="160">
  <header>
#include "subsystems/datalink/datalink.h"
</header>
  <waypoints>
    <waypoint name="HOME" x="15.0" y="5.0"/>
    <waypoint name="CLIMB" x=".669853874" y="213.9946379811"/>
    <waypoint name="1" x="-181.785735614" y="-34.9692539535"/>
    <waypoint name="2" x="130.143368186" y="-90.3661855655"/>
    <waypoint name="CR" x="0.0885126006" y="-73.7359798392"/>
    <waypoint name="S1" x="-200" y="-200"/>
    <waypoint name="S2" x="300" y="400"/>
  </waypoints>
  <exceptions>
    <exception cond="GetPosAlt() > 3600"  deroute="wait"/>
    <exception cond="datalink_time > 30"  deroute="wait"/>
    <exception cond="10 > PowerVoltage()" deroute="wait"/>
  </exceptions>
  <blocks>
    <block name="start">
      <set value="10" var="my_nav_roll"/>
      <set value="-10" var="my_nav_pitch"/>
      <attitude pitch="5" roll="0" throttle="0.7" until="(autopilot.flight_time > 5)" vmode="throttle"/>
      <go wp="1"/>
    </block>
    <block name="east">
      <go approaching_time="2" from="1" hmode="route" target="2" wp="2"/>
      <deroute block="west"/>
    </block>
    <block name="west">
      <go approaching_time="2" from="2" hmode="route" target="1" wp="1"/>
      <deroute block="east"/>
    </block>
    <block name="circles">
      <for from="1" to="5" var="i">
        <set value="$i*750*cos(RadOfDeg(30))" var="waypoints[WP_2].x"/>
        <set value="$i*750*sin(RadOfDeg(30))+nav_radius" var="waypoints[WP_2].y"/>
        <go hmode="route" wp="2"/>
        <set value="$i*750*sin(RadOfDeg(30))" var="waypoints[WP_2].y"/>
        <circle radius="nav_radius" until="NavCircleCount()>1" wp="2"/>
      </for>
      <deroute block="west"/>
    </block>

    <block name="clock">
      <circle radius="nav_radius" wp="CR"/>
    </block>
    <block name="counter">
      <circle radius="-90" wp="CR"/>
    </block>
    <block name="survey">
      <survey_rectangle grid="150" wp1="S1" wp2="S2"/>
    </block>
   <block name="survey WE">
      <survey_rectangle grid="150" orientation="WE" wp1="S1" wp2="S2"/>
    </block>
    <block name="profile">
      <for from="1" to="5" var="i">
        <circle alt="GetAltRef()+50* $i" radius="nav_radius" until="stage_time>120" wp="1"/>
      </for>
    </block>
    <block name="climb">
      <circle radius="nav_radius" wp="CLIMB"/>
    </block>
    <block name="descent">
      <circle radius="nav_radius" wp="CLIMB" vmode="throttle" throttle="0" pitch="RadOfDeg(my_nav_pitch)"  until="(GetAltRef()+150 > GetPosAlt())"/>
    <deroute block="wait"/>
    </block>
    <block name="wind">
      <attitude pitch="RadOfDeg(my_nav_pitch)" roll="my_nav_roll" throttle="0.0" until="(GetAltRef()+150 > GetPosAlt())" vmode="throttle"/>
      <deroute block="wait"/>
    </block>
    <block name="wait" strip_button="Wait">
      <circle radius="nav_radius" wp="CLIMB" alt="GetAltRef()+200"/>
    </block>
    <block name="roll">
      <set value="TRUE" var="h_ctl_disabled"/>
      <set value="(-0.75*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
      <while cond="3.> stage_time && (stateGetNedToBodyEulers_f()->phi > 0 || -M_PI_2 > stateGetNedToBodyEulers_f()->phi)"/>
      <set value="FALSE" var="h_ctl_disabled"/>
      <return/>
    </block>
  </blocks>
</flight_plan>
