mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-27 08:55:51 +08:00
Fabien-B
87a0526f37
* [Flightplans] Replace ">" by "@GT". * [Flightplan] Replace "&&" by "@AND". * Add GetPosHeight() macro. * [FlightPlan] Replace "stateGetPositionEnu_f()->z" by the GetPosHeight() macro.
156 lines
9.3 KiB
XML
156 lines
9.3 KiB
XML
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
|
|
|
<flight_plan alt="200." ground_alt="42" lat0="37.7776225" lon0="23.9184623" max_dist_from_home="4000" name="lagonisi_beach_survey" qfu="180" security_height="100.">
|
|
<header>
|
|
</header>
|
|
<waypoints>
|
|
<waypoint height="0" name="HOME" x="14.2" y="3.2"/>
|
|
<waypoint height="0" lat="37.777399" lon="23.918696" name="TD"/>
|
|
<waypoint height="0" name="CAM_POINT" x="11.6" y="34.3"/>
|
|
<waypoint height="50" name="STANDBY" x="23.3" y="-67.7"/>
|
|
<waypoint height="200" name="RIGHT_HERE" x="138.7" y="-28.0"/>
|
|
<waypoint height="30" name="AF" x="36.5" y="-265.3"/>
|
|
<waypoint height="30" name="BASELEG" x="101.5" y="246.0"/>
|
|
<waypoint height="200" name="MOB" x="137.3" y="3.5"/>
|
|
<waypoint height="30" lat="37.774872" lon="23.918757" name="AF_SOUTH"/>
|
|
<waypoint height="30" lat="37.779789" lon="23.918472" name="AF_NORTH"/>
|
|
<waypoint height="30" name="RELEASE" x="78.7" y="-22.2"/>
|
|
<waypoint alt="200" lat="37.772633" lon="23.906762" name="WP1"/>
|
|
<waypoint alt="200" lat="37.770300" lon="23.899190" name="WP2"/>
|
|
<waypoint alt="200" lat="37.774123" lon="23.895975" name="WP3"/>
|
|
<waypoint alt="200" lat="37.771894" lon="23.885380" name="WP4"/>
|
|
<waypoint alt="200" lat="37.774628" lon="23.882086" name="WP5"/>
|
|
<waypoint alt="200" lat="37.783286" lon="23.884851" name="WP6"/>
|
|
<waypoint alt="200" lat="37.786916" lon="23.880351" name="TARGET"/>
|
|
</waypoints>
|
|
<sectors/>
|
|
<variables/>
|
|
<exceptions>
|
|
<exception cond="(imcu_get_radio(RADIO_YAW) @GT MAX_PPRZ/2)@AND(autopilot.mode == AP_MODE_AUTO2) @AND (GetPosAlt() @GT GetAltRef()+20)" deroute="Target"/>
|
|
<exception cond="(MIN_PPRZ/2 @GT imcu_get_radio(RADIO_YAW))@AND(autopilot.mode == AP_MODE_AUTO2) @AND (GetPosAlt() @GT GetAltRef()+20)" deroute="Return Home"/>
|
|
</exceptions>
|
|
<blocks>
|
|
<block name="Wait Gps">
|
|
<set value="false" var="h_ctl_disabled"/>
|
|
<set value="true" var="autopilot.kill_throttle"/>
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
|
|
<while cond="!GpsFixValid()"/>
|
|
</block>
|
|
<block name="Geo Init">
|
|
<while cond="LessThan(NavBlockTime(), 10)"/>
|
|
<call_once fun="NavSetGroundReferenceHere()"/>
|
|
<call_once fun="NavSetWaypointHere(WP_HOME)"/>
|
|
</block>
|
|
<block name="Holding Point">
|
|
<exception cond="((imcu_get_radio(RADIO_THROTTLE) @GT (MAX_PPRZ-(MAX_PPRZ/10)))@AND(autopilot.mode == AP_MODE_AUTO2))" deroute="Takeoff"/>
|
|
<set value="true" var="autopilot.kill_throttle"/>
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)"/>
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
|
|
<attitude roll="0" throttle="0" until="((sqrt(dist2_to_home) @GT 10)@AND (700 @GT gps.pdop))" vmode="throttle"/>
|
|
<deroute block="Takeoff"/>
|
|
</block>
|
|
<block name="Takeoff" strip_button="Takeoff (wp AF_NORTH)" strip_icon="takeoff.png">
|
|
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
|
|
<set value="true" var="autopilot.launch"/>
|
|
<set value="false" var="h_ctl_disabled"/>
|
|
<set value="false" var="autopilot.kill_throttle"/>
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
|
|
<attitude pitch="20" roll="0.0" throttle="1.0" until="GetPosAlt() @GT GetAltRef()+20" vmode="throttle"/>
|
|
<deroute block="Standby"/>
|
|
</block>
|
|
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
|
<exception cond="electrical.bat_low@AND(autopilot.mode == AP_MODE_AUTO2)@AND(GetPosAlt() @GT GetAltRef()+20)" deroute="Land"/>
|
|
<exception cond="radio_control.status == RC_REALLY_LOST@AND(autopilot.mode == AP_MODE_AUTO2)@AND(GetPosAlt() @GT GetAltRef()+20)" deroute="Land"/>
|
|
<set value="false" var="h_ctl_disabled"/>
|
|
<set value="false" var="autopilot.kill_throttle"/>
|
|
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
|
|
<circle height="30" radius="nav_radius" wp="STANDBY"/>
|
|
</block>
|
|
<block name="Target" strip_button="Go to TARGET" strip_icon="kill.png">
|
|
<exception cond="electrical.bat_low@AND(autopilot.mode == AP_MODE_AUTO2)@AND(GetPosAlt() @GT GetAltRef()+20)" deroute="Return Home NOW"/>
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
|
|
<go from="HOME" height="200" hmode="route" wp="WP1"/>
|
|
<go from="WP1" height="200" hmode="route" wp="WP2"/>
|
|
<go from="WP2" height="200" hmode="route" wp="WP3"/>
|
|
<go from="WP3" height="200" hmode="route" wp="WP4"/>
|
|
<go from="WP4" height="200" hmode="route" wp="WP5"/>
|
|
<go from="WP5" height="200" hmode="route" wp="WP6"/>
|
|
<go alt="200+ground_alt" from="WP6" hmode="route" wp="TARGET"/>
|
|
<circle height="200" radius="(nav_radius*2)" until="NavCircleCount() @GT 2" wp="TARGET"/>
|
|
<deroute block="Return Home"/>
|
|
</block>
|
|
<block name="Return Home" strip_button="Return Home" strip_icon="home.png">
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
|
|
<go from="TARGET" height="200" hmode="route" wp="WP6"/>
|
|
<go from="WP6" height="200" hmode="route" wp="WP5"/>
|
|
<go from="WP5" height="200" hmode="route" wp="WP4"/>
|
|
<go from="WP4" height="200" hmode="route" wp="WP3"/>
|
|
<go from="WP3" height="200" hmode="route" wp="WP2"/>
|
|
<go from="WP2" height="200" hmode="route" wp="WP1"/>
|
|
<go from="WP1" height="200" hmode="route" wp="HOME"/>
|
|
<deroute block="Standby"/>
|
|
</block>
|
|
<block name="Return Home NOW" strip_button="Return Home" strip_icon="home.png">
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
|
|
<call fun="NavSetWaypointHere(WP_RIGHT_HERE)"/>
|
|
<set value="GetPosAlt()" var="WaypointAlt(WP_RIGHT_HERE)"/>
|
|
<go from="RIGHT_HERE" height="200" hmode="route" wp="HOME"/>
|
|
<deroute block="Standby"/>
|
|
</block>
|
|
<block name="Land" strip_button="AUTO LAND">
|
|
<call fun="calculate_wind_no_airspeed(WP_TD, (nav_radius*2), 50)"/>
|
|
<deroute block="Land1"/>
|
|
</block>
|
|
<block name="Land1">
|
|
<exception cond="airborne_wind_dir @GT 270. || 90. @GT airborne_wind_dir" deroute="Land From South"/>
|
|
<exception cond="airborne_wind_dir @GT 90. @AND 270. @GT airborne_wind_dir" deroute="Land From North"/>
|
|
</block>
|
|
<block name="Land From North" strip_button="Land FROM NORTH (wp AF_NORTH-TD)" strip_icon="land-right.png">
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
|
|
<call_once fun="nav_compute_baseleg(WP_AF_NORTH, WP_TD, WP_BASELEG, nav_radius)"/>
|
|
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="BASELEG"/>
|
|
<set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
|
|
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND 10 @GT fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
|
|
</block>
|
|
<block name="Final North">
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
|
|
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="Flare North"/>
|
|
<go from="AF_NORTH" hmode="route" vmode="glide" wp="TD"/>
|
|
</block>
|
|
<block name="Flare North">
|
|
<go approaching_time="0" from="AF_NORTH" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
|
|
<attitude roll="0.0" throttle="0.0" until="false" vmode="throttle"/>
|
|
</block>
|
|
<block name="Land From South" strip_button="Land FROM SOUTH (wp AF_SOUTH-TD)" strip_icon="land-left.png">
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
|
|
<call_once fun="nav_compute_baseleg(WP_AF_SOUTH, WP_TD, WP_BASELEG, nav_radius)"/>
|
|
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="BASELEG"/>
|
|
<set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
|
|
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND 10>fabs(GetPosAlt()-WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
|
|
</block>
|
|
<block name="final South">
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
|
|
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="Flare South"/>
|
|
<go from="AF_SOUTH" hmode="route" vmode="glide" wp="TD"/>
|
|
</block>
|
|
<block name="Flare South">
|
|
<go approaching_time="0" from="AF_SOUTH" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
|
|
<attitude roll="0.0" throttle="0.0" until="false" vmode="throttle"/>
|
|
</block>
|
|
<block name="Recover Uav" strip_button="Recover" strip_icon="parachute.png">
|
|
<call fun="LockParachute()"/>
|
|
<call fun="calculate_wind_no_airspeed(WP_TD, (nav_radius*2), 50)"/>
|
|
<call fun="ParachuteComputeApproach(WP_BASELEG, WP_RELEASE, WP_TD)"/>
|
|
<circle alt="(ground_alt+30)" radius="nav_radius" until="NavQdrCloseTo(DegOfRad(parachute_start_qdr)-(nav_radius/fabs(nav_radius))*10) @AND 10>fabs(GetPosAlt()-(ground_alt+30))" wp="BASELEG"/>
|
|
<call fun="NavSetWaypointHere(WP_MOB)"/>
|
|
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
|
|
<go from="MOB" hmode="route" wp="RELEASE"/>
|
|
<attitude pitch="10" roll="0.0" throttle="0" until="MINIMUM_AIRSPEED @GT stateGetHorizontalSpeedNorm_f()" vmode="throttle"/>
|
|
<call fun="DeployParachute()"/>
|
|
<set value="true" var="autopilot.kill_throttle"/>
|
|
<circle height="70." radius="nav_radius" wp="HOME"/>
|
|
</block>
|
|
</blocks>
|
|
</flight_plan>
|