paparazzi/conf/flight_plans/tudelft/oneloop_cyberzoo.xml

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

<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Safe TU Delft Cyberzoo" security_height="0.4">
  <header>
    #include "autopilot.h"
    #include "modules/datalink/datalink.h"
    #include "modules/energy/electrical.h"
    #include "modules/radio_control/radio_control.h"
    #include "modules/ahrs/ahrs.h"
    #include "modules/gps/gps.h"
    <!-- #include "sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h" -->
</header>
  <waypoints>
    <waypoint lat="51.990631" lon="4.376796" name="HOME"/>
    <waypoint name="CLIMB" x="1.9" y="1.0"/>
    <waypoint name="STDBY" x="0.5" y="0.3"/>
    <waypoint name="p5" x="0.2" y="0.3"/>
    <waypoint name="TD" x="0.8" y="-1.7"/>
    <waypoint lat="51.9905979" lon="4.3767279" name="CZ1"/>
    <waypoint lat="51.9906685" lon="4.3767327" name="CZ2"/>
    <waypoint lat="51.9906652" lon="4.3768481" name="CZ3"/>
    <waypoint lat="51.9905941" lon="4.3768441" name="CZ4"/>
    <waypoint lat="51.9906028" lon="4.3767375" name="p1"/>
    <waypoint lat="51.9906633" lon="4.3767411" name="p2"/>
    <waypoint lat="51.9906601" lon="4.3768415" name="p3"/>
    <waypoint lat="51.9905979" lon="4.3768386" name="p4"/>
  </waypoints>
  <sectors>
    <sector color="blue" name="CyberZoo">
      <corner name="CZ1"/>
      <corner name="CZ2"/>
      <corner name="CZ3"/>
      <corner name="CZ4"/>
    </sector>
    <sector color="red" name="Flyzone">
      <corner name="p1"/>
      <corner name="p2"/>
      <corner name="p3"/>
      <corner name="p4"/>
    </sector>
  </sectors>
  <exceptions>
    <!--Soft Geofencing (go back to Standby)-->
    <exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT 20.0) @AND
     !(nav_block == IndexOfBlock('Wait GPS')) @AND
     !(nav_block == IndexOfBlock('Geo init'))" deroute="safe"/>
    <!-- Hard Geofencing (Kill) -->
    <exception cond="(Or(!InsideCyberZoo(GetPosX(), GetPosY()), GetPosAlt() @GT 80.0) @AND
      !(IndexOfBlock('Holding point') @GT nav_block) @AND
      !(nav_block >= IndexOfBlock('land here')) @AND
      (autopilot_in_flight() == true) )" deroute="Landed"/>
    <!-- RC lost -->
    <exception cond="((radio_control.status == RC_REALLY_LOST) @AND
      !(IndexOfBlock('Holding point') @GT nav_block) @AND
      !(nav_block >= IndexOfBlock('land here')) @AND
      (autopilot_in_flight() == true) )" deroute="Landed"/>
    <!-- Datalink lost  -->
    <exception cond="((datalink_time @GT 5) @AND
      !(IndexOfBlock('Holding point') @GT nav_block) @AND
      !(nav_block >= IndexOfBlock('land here')) @AND
      (autopilot_in_flight() == true) )" deroute="Landed"/>
    <!-- Bat low (constant RPM descent)-->
    <exception cond="(electrical.bat_low @AND
      !(IndexOfBlock('Holding point') @GT nav_block) @AND
      !(nav_block >= IndexOfBlock('land here')) @AND
      (autopilot_in_flight() == true) )" deroute="land here"/>
    <!-- Bat critical (constant RPM no stabilization)-->
    <exception cond="(electrical.bat_critical @AND
      !(IndexOfBlock('Holding point') @GT nav_block) @AND
      !(nav_block >= IndexOfBlock('land here')) @AND
      (autopilot_in_flight() == true) )" deroute="land here"/>
  </exceptions>
  <blocks>
    <block name="Wait GPS">
      <call_once fun="NavKillThrottle()"/>
      <while cond="!GpsFixValid()"/>
    </block>
    <block name="Geo init">
      <while cond="LessThan(NavBlockTime(), 2)"/>
      <call_once fun="NavSetAltitudeReferenceHere()"/>
    </block>
    <block name="Holding point">
      <call_once fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
    </block>
    <block key="r" name="Start Engine">
      <call_once fun="NavResurrect()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
      <call_once fun="NavResurrect()"/>
    </block>
    <block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
      <exception cond="GetPosAlt() @GT 0.8" deroute="Standby"/>
      <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
      <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
    </block>
    <block key="s" name="Standby" strip_button="Standby" strip_icon="home.png">
      <stay wp="STDBY"/>
    </block>
    <block name="go_p5">
      <stay wp="p5"/>
    </block>
    <block name="safe">
      <stay wp="p5"/>
      <exception cond="NavBlockTime() @GT 5" deroute="go_p5"/>
    </block>
    <block key="l" name="land here" strip_button="land here" strip_icon="land-right.png">
      <call_once fun="NavSetWaypointHere(WP_TD)"/>
      <go wp="TD"/>
      <deroute block="Flare"/>
    </block>
    <block name="Land">
      <go wp="TD"/>
      <deroute block="Flare"/>
    </block>
    <block name="Flare">
      <exception cond="NavDetectGround()" deroute="Holding point"/>
      <exception cond="!nav_is_in_flight()" deroute="Landed"/>
      <exception cond="0.10 @GT GetPosAlt()" deroute="Landed"/>
      <call_once fun="NavStartDetectGround()"/>
      <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
    </block>
    <block name="Safe landing">
      <exception cond="!nav_is_in_flight()" deroute="Manual"/>
      <exception cond="0.10 @GT GetPosAlt()" deroute="Manual"/>
      <call_once fun="NavStartDetectGround()"/>
      <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
    </block>
    <block name="Manual">
      <set value="AP_MODE_ATTITUDE_DIRECT" var="autopilot_mode_auto2"/>
      <stay wp="STDBY"/>
    </block>
    <block name="Landed">
      <call_once fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
    </block>
  </blocks>
</flight_plan>