Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity
Files
master
paparazzi/conf/flight_plans/tudelft/rotwing_troia.xml
T

310 lines
16 KiB
XML
Raw Permalink Blame History

<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="115" ground_alt="15" lat0="38.474472222222225" lon0="-8.871222222222222" max_dist_from_home="80000" name="Rotating wing Troia" security_height="30">
<waypoints>
<waypoint lat="38.4874248" lon="-8.8691705" name="1-1"/>
<waypoint lat="38.4734375" lon="-8.8541523" name="1-2"/>
<waypoint lat="38.4690399" lon="-8.8614329" name="1-3"/>
<waypoint lat="38.4630730" lon="-8.8715400" name="1-4"/>
<waypoint lat="38.4701413" lon="-8.8785666" name="1-5"/>
<waypoint lat="38.4779321" lon="-8.8854282" name="1-6"/>
<waypoint lat="38.4796216" lon="-8.8819603" name="1-7"/>
<waypoint lat="38.4821792" lon="-8.8775063" name="1-8"/>
<waypoint lat="38.4831320" lon="-8.8691903" name="1-1_soft"/>
<waypoint lat="38.4742608" lon="-8.8598735" name="1-2_soft"/>
<waypoint lat="38.4710995" lon="-8.8649587" name="1-3_soft"/>
<waypoint lat="38.4676940" lon="-8.8708800" name="1-4_soft"/>
<waypoint lat="38.4767258" lon="-8.8799233" name="1-5_soft"/>
<waypoint lat="38.486388" lon="-8.868333" name="sado_e_1"/>
<waypoint lat="38.511111" lon="-8.84444" name="sado_e_2"/>
<waypoint lat="38.485" lon="-8.834166" name="sado_e_3"/>
<waypoint lat="38.47305555" lon="-8.83027777" name="sado_e_4"/>
<waypoint lat="38.47305555" lon="-8.85555555" name="sado_e_5"/>
<waypoint lat="38.4863888" lon="-8.8683333" name="sado_e_6"/>
<!-- <waypoint lat="38.468493" lon="-8.886115" name="2-1_soft"
38.4863888888889 -8.86833333333333
38.5111111111111 -8.84444444444445
38.485 -8.83416666666667
38.4730555555556 -8.83027777777778
38.4730555555556 -8.85555555555556
38.4863888888889 -8.86833333333333
/>
<waypoint lat="38.467498" lon="-8.884860" name="2-2_soft"/>
<waypoint lat="38.475621" lon="-8.874463" name="2-4_soft"/>
<waypoint lat="38.468610" lon="-8.877984" name="2-3_soft"/> -->
<!-- <waypoint lat="38.499227" lon="-8.858617" name="3-1"/>
<waypoint lat="38.476154" lon="-8.833249" name="3-2"/>
<waypoint lat="38.469040" lon="-8.861433" name="3-3"/>
<waypoint lat="38.467811" lon="-8.879136" name="3-4"/>
<waypoint lat="38.464900" lon="-8.884918" name="3-5"/>
<waypoint lat="38.471784" lon="-8.899161" name="3-6"/>
<waypoint lat="38.478446" lon="-8.885780" name="3-7"/>
<waypoint lat="38.480130" lon="-8.886261" name="3-8"/>
<waypoint lat="38.493038" lon="-8.858636" name="3-1_soft"/>
<waypoint lat="38.478620" lon="-8.845008" name="3-2_soft"/>
<waypoint lat="38.470562" lon="-8.867834" name="3-3_soft"/>
<waypoint lat="38.476390" lon="-8.887219" name="3-5_soft"/>
<waypoint lat="38.468803" lon="-8.880064" name="3-4_soft"/> -->
<waypoint lat="38.4" lon="-9.608888888888888" name="6-1"/>
<waypoint lat="38.409166666666664" lon="-9.196944444444444" name="6-2"/>
<waypoint lat="38.434444444444445" lon="-9.116666666666667" name="6-3"/>
<waypoint lat="38.43361111111111" lon="-9.054722222222223" name="6-4"/>
<waypoint lat="38.48777777777778" lon="-8.93361111111111" name="6-5"/>
<waypoint lat="38.46722222222222" lon="-8.885555555555555" name="6-6"/>
<waypoint lat="38.44444444444444" lon="-8.854722222222222" name="6-7"/>
<waypoint lat="38.30027777777778" lon="-8.854722222222222" name="6-8"/>
<waypoint lat="38.30027777777778" lon="-8.850833333333334" name="6-9"/>
<waypoint lat="38.215833333333336" lon="-8.850277777777778" name="6-10"/>
<waypoint lat="38.16833333333334" lon="-9.001666666666667" name="6-11"/>
<waypoint lat="38.16694444444445" lon="-9.198888888888888" name="6-12"/>
<waypoint lat="38.000277777777775" lon="-9.200555555555555" name="6-13"/>
<waypoint lat="38.000277777777775" lon="-9.450277777777778" name="6-14"/>
<waypoint name="HOME" x="111.225" y="-35.761"/>
<waypoint name="CLIMB" x="137.087" y="156.759"/>
<waypoint name="STDBY" lat="38.4746" lon="-8.86917"/>
<waypoint name="circ" lat="38.4751" lon="-8.86715"/>
<waypoint name="TD" x="111.497" y="-30.473" height="30"/>
<!-- <waypoint name="APP" x="70" y="-25"/> -->
<!-- <waypoint name="FOLLOW" x="300" y="80"/> -->
<waypoint name="p1" x="171.84" y="83.954"/>
<waypoint name="p2" x="419.479" y="282.12"/>
<waypoint name="p3" x="641.263" y="14.065"/>
<waypoint name="p4" x="385.379" y="-204.223"/>
<waypoint name="Downwind" x="170.2" y="209.1" height="50"/>
<waypoint name="Base" x="534.285" y="-58.323" height="40"/>
<waypoint name="Final" x="257.543" y="-180.466" height="30"/>
<waypoint name="ML_global_target" x="397.8" y="156.359"/>
</waypoints>
<sectors>
<sector color="red" name="A1">
<corner name="1-1"/>
<corner name="1-2"/>
<corner name="1-3"/>
<corner name="1-4"/>
<corner name="1-5"/>
<corner name="1-6"/>
<corner name="1-7"/>
<corner name="1-8"/>
</sector>
<sector color="orange" name="A1_soft">
<corner name="1-1_soft"/>
<corner name="1-2_soft"/>
<corner name="1-3_soft"/>
<corner name="1-4_soft"/>
<corner name="1-5_soft"/>
</sector>
<sector color="red" name="A3">
<corner name="6-1"/>
<corner name="6-2"/>
<corner name="6-3"/>
<corner name="6-4"/>
<corner name="6-5"/>
<corner name="6-6"/>
<corner name="6-7"/>
<corner name="6-8"/>
<corner name="6-9"/>
<corner name="6-10"/>
<corner name="6-11"/>
<corner name="6-12"/>
<corner name="6-13"/>
<corner name="6-14"/>
</sector>
<sector color="blue" name="SADO_EAST">
<corner name="sado_e_1"/>
<corner name="sado_e_2"/>
<corner name="sado_e_3"/>
<corner name="sado_e_4"/>
<corner name="sado_e_5"/>
<corner name="sado_e_6"/>
</sector><!--
<sector color="orange" name="A2_soft">
<corner name="3-1_soft"/>
<corner name="3-2_soft"/>
<corner name="3-3_soft"/>
<corner name="3-4_soft"/>
<corner name="3-5_soft"/>
</sector> -->
</sectors>
<variables>
<variable var="liftoff_pitch"/>
<variable var="liftoff_roll"/>
<variable var="stage_timer_msec"/>
</variables>
<exceptions>
<!-- Hard geofence (red -> Holding point) -->
<exception cond="And( Or(!InsideA1(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 300.0), Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 300.0)) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<!-- Soft geofence (orange -> GO Standby) -->
<exception cond="And( Or(!InsideA1_soft(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 250.0), Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 250.0)) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby_free"/>
<!-- Datalink loss (25 seconds -> GO STANDBY) -->
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby_free"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block>
<block name="Wait takeoff">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 5.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 5.0) @AND (stage_time > 2)" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/>
<set var="liftoff_roll" value="DegOfRad(stateGetNedToBodyEulers_f()->phi)"/>
<set var="liftoff_pitch" value="DegOfRad(stateGetNedToBodyEulers_f()->theta)"/>
<attitude pitch="liftoff_pitch" roll="liftoff_roll" throttle="0.75" until="(get_sys_time_msec()-stage_timer_msec)>250" vmode="throttle"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/>
</block>
<block name="Standby_free">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/>
</block>
<block name="mavlink_guided">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<stay wp="ML_global_target"/>
</block>
<!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> -->
<!-- <block name="follow_module">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> -->
<!-- <block name="route">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/>
</block> -->
<block name="small_route_p1" strip_button="Route_P1" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p1"/>
</block>
<block name="small_route_p2" strip_button="Route_P2" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p2"/>
</block>
<block name="small_route_p3" strip_button="Route P3" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p3"/>
</block>
<block name="small_route_p4" strip_button="Route P4" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p4"/>
<deroute block="small_route_p1"/>
</block>
<block name="stay_p1" strip_button="Stay P1" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p1"/>
</block>
<block name="stay_p2">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/>
</block>
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="nav.radius" wp="circ"/>
</block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-nav.radius" wp="circ"/>
</block>
<block name="Circle_Auto_fwd" strip_button="CircleAuto" strip_icon="circle-left.png">
<!-- <call_once var="circle_direction" fun="rotwing_state_choose_circle_direction(WP_circ)"/> -->
<!-- <circle radius="-nav.radius" wp="circ"/> -->
<exception cond="!rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CW_fwd"/>
<exception cond="rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CCW_fwd"/>
</block>
<!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> -->
<block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="Downwind" strip_button="Downwind" strip_icon="land-right.png">
<go wp="Downwind"/>
</block>
<block name="Base">
<go wp="Base"/>
</block>
<block name="Final">
<go wp="Final"/>
<deroute block="land"/>
</block>
<block name="land here">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="TD"/>
</block>
<block name="descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block>
<block name="flare_low">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/>
<!-- <call_once fun="NavStartDetectGround()"/> -->
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
Reference in New Issue View Git Blame Copy Permalink