<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="61" ground_alt="49" lat0="45.564133" lon0="-122.622467" max_dist_from_home="125" name="Rotorcraft Multiflight (Portland)" security_height="2">
  <header>
#include "autopilot.h"
</header>
  <waypoints>
    <waypoint name="HOME" x="-0.2" y="-7.1"/>
    <waypoint name="CLIMB" x="-4.9" y="-3.4"/>
    <waypoint name="STDBY" x="2.5" y="-0.2"/>
    <waypoint name="p1" x="-0.2" y="36.2"/>
    <waypoint name="p2" x="40.7" y="9.4"/>
    <waypoint name="p3" x="40.9" y="37.5"/>
    <waypoint name="p4" x="-0.3" y="10.1"/>
    <waypoint name="HOV" x="20.0" y="15.0"/>
    <waypoint name="CAM" x="30.0" y="15.0"/>
    <waypoint name="S1" x="-8.4" y="39.2"/>
    <waypoint name="S2" x="49.3" y="5.9"/>
    <waypoint name="TD" x="8.4" y="-8.0"/>
  </waypoints>
  <blocks>
    <block name="Wait GPS">
      <call_once fun="NavKillThrottle()"/>
      <while cond="!GpsFixValid()"/>
    </block>
    <block name="Geo init">
      <while cond="LessThan(NavBlockTime(), 10)"/>
      <call_once fun="NavSetGroundReferenceHere()"/>
    </block>
    <block name="Holding point">
      <call_once fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
    </block>
    <block group="init_control" name="Start Motors" strip_button="Start Motors" strip_icon="start_motors.png">
      <call_once fun="NavResurrect()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
    </block>
    <block group="init_control" name="Takeoff" strip_button="Takeoff" strip_icon="launch.png">
      <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
      <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
      <stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
    </block>
    <block group="waypoint_control_col1" name="Standby" strip_button="Standby" strip_icon="standby.png">
      <stay wp="STDBY"/>
    </block>
    <block group="waypoint_control_col1" name="stay_p1" strip_button="Waypoint 1" strip_icon="waypoint_one.png">
      <stay wp="p1"/>
    </block>
    <block group="waypoint_control_col2" name="stay_p2" strip_button="Waypoint 2" strip_icon="waypoint_two.png">
      <stay wp="p2"/>
    </block>
    <block group="waypoint_control_col2" name="stay_p3" strip_button="Waypoint 3" strip_icon="waypoint_three.png">
      <stay wp="p3"/>
    </block>
    <block group="waypoint_control_col2" name="stay_p4" strip_button="Waypoint 4" strip_icon="waypoint_four.png">
      <stay wp="p4"/>
    </block>
    <block name="p1_nav">
      <call_once fun="nav_set_heading_towards_waypoint(WP_p1)"/>
      <go wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="p2_nav">
      <call_once fun="nav_set_heading_towards_waypoint(WP_p2)"/>
      <go wp="p2"/>
      <deroute block="stay_p2"/>
    </block>
    <block name="stay_HOV">
      <stay wp="HOV"/>
    </block>
    <block group="pano_control" name="go_HOV_0" strip_button="Go Pano" strip_icon="pano_hov.png">
      <call_once fun="nav_set_heading_deg(0)"/>
      <deroute block="stay_HOV"/>
    </block>
    <block name="go_HOV_90">
      <call_once fun="nav_set_heading_deg(90)"/>
      <deroute block="stay_HOV"/>
    </block>
    <block name="go_HOV_180">
      <call_once fun="nav_set_heading_deg(180)"/>
      <deroute block="stay_HOV"/>
    </block>
    <block name="go_HOV_270">
      <call_once fun="nav_set_heading_deg(270)"/>
      <deroute block="stay_HOV"/>
    </block>
    <block group="pano_control" name="go_HOV_Pano" strip_button="CW Pano" strip_icon="pano_run.png">
      <call_once fun="NavSetWaypointHere(WP_HOV)"/>
      <for var="i" from="1" to="12">
        <heading alt="WaypointAlt(WP_HOV)" course="30 * $i" until="stage_time > 5"/>
      </for>
      <deroute block="go_HOV_0"/>
    </block>
    <block group="spin_control" name="go_HOV_spin_clockwise" strip_button="Spin Clockwise" strip_icon="circle-right.png">
        <call_once fun="NavSetWaypointHere(WP_HOV)"/>
        <for var="i" from="1" to="3">
            <heading alt="WaypointAlt(WP_HOV)" course="120 * $i" until="stage_time > 0"/>
        </for>
        <deroute block="go_HOV_0"/>
    </block>
    <block group="spin_control" name="go_HOV_spin_counterclockwise" strip_button="Spin Counterclockwise" strip_icon="circle-left.png">
        <call_once fun="NavSetWaypointHere(WP_HOV)"/>
        <for var="i" from="1" to="3">
            <heading alt="WaypointAlt(WP_HOV)" course="-120 * $i" until="stage_time > 0"/>
        </for>
        <deroute block="go_HOV_0"/>
    </block>
    <block name="line_p1_p2">
      <go from="p1" hmode="route" wp="p2"/>
      <stay until="stage_time>10" wp="p2"/>
      <go from="p2" hmode="route" wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="route_tri">
      <go from="p1" hmode="route" wp="p3"/>
      <go from="p3" hmode="route" wp="p4"/>
      <go from="p4" hmode="route" wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="route_quad">
      <go from="p1" hmode="route" wp="p3"/>
      <go from="p3" hmode="route" wp="p2"/>
      <go from="p2" hmode="route" wp="p4"/>
      <go from="p4" hmode="route" wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="path_rev_quad">
      <path wpts="p1, p4, p2, p3, p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="route_x">
      <go approaching_time="0" from="p1" hmode="route" wp="p2"/>
      <go approaching_time="0" from="p2" hmode="route" wp="p3"/>
      <go approaching_time="0" from="p3" hmode="route" wp="p4"/>
      <go approaching_time="0" from="p4" hmode="route" wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="route_x_nav">
      <call_once fun="nav_set_heading_towards_waypoint(WP_p2)"/>
      <go approaching_time="0" from="p1" hmode="route" wp="p2"/>
      <call_once fun="nav_set_heading_towards_waypoint(WP_p3)"/>
      <go approaching_time="0" from="p2" hmode="route" wp="p3"/>
      <call_once fun="nav_set_heading_towards_waypoint(WP_p4)"/>
      <go approaching_time="0" from="p3" hmode="route" wp="p4"/>
      <call_once fun="nav_set_heading_towards_waypoint(WP_p1)"/>
      <go approaching_time="0" from="p4" hmode="route" wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="survey_S1_S2_NS">
      <survey_rectangle grid="10" orientation="NS" wp1="S1" wp2="S2"/>
    </block>
    <block name="survey_S1_S2_WE">
      <survey_rectangle grid="10" orientation="WE" wp1="S1" wp2="S2"/>
    </block>
    <block group="extra_pattern" name="Survey S1-S2 Sweep NS SET" strip_button="SvySweep-NS" strip_icon="survey_rect_ns.png">
      <call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S2, sweep, NS)"/>
      <deroute block="Survey RECTANGLE RUN"/>
    </block>
    <block group="extra_pattern" name="Survey S1-S2 Sweep WE SET" strip_button="SvySweep-WE" strip_icon="survey_rect_we.png">
      <call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S2, sweep, WE)"/>
      <deroute block="Survey RECTANGLE RUN"/>
    </block>
    <block group="extra_pattern" name="Survey RECTANGLE RUN" strip_button="SvySweep CONT" strip_icon="survey_rect_run.png">
      <exception cond="rectangle_survey_sweep_num == 1" deroute="Standby"/>
      <call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S2)"/>
    </block>
    <block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
      <circle radius="nav_radius" wp="CAM"/>
    </block>
    <block group="land_pattern" name="land here" strip_button="Land Here" strip_icon="land_here.png">
      <call_once fun="NavSetWaypointHere(WP_TD)"/>
    </block>
    <block group="land_pattern" name="land" strip_button="Land" strip_icon="land.png">
      <go wp="TD"/>
    </block>
    <block name="flare">
      <exception cond="NavDetectGround()" deroute="Holding point"/>
      <exception cond="!nav_is_in_flight()" deroute="landed"/>
      <call_once fun="NavStartDetectGround()"/>
      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
    </block>
    <block name="landed">
      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
    </block>
  </blocks>
</flight_plan>
