mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-28 18:07:25 +08:00
Gautier Hattenberger
28ef30ed80
This is a complete rework of the navigation for rotorcraft with the key features: - all nav functions and interface in float - nav function are registered (decoupling between nav API and implementation of standard pattern) - submodes to specify if setpoint is a position (legacy), speed or accel (better integration of algorithms like GVF) - guidance (H and V) is reorganized with default function to implement (run_pos, run_speed, run_accel) - guidance control (the old default PID) is separated from the guidance logic --------- Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
133 lines
5.0 KiB
XML
133 lines
5.0 KiB
XML
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
|
|
|
<flight_plan alt="10" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="1000" name="Delft Basic" security_height="2">
|
|
<header>
|
|
#include "autopilot.h"
|
|
#include "modules/datalink/datalink.h"
|
|
#include "modules/energy/electrical.h"
|
|
#include "modules/radio_control/radio_control.h"
|
|
</header>
|
|
<waypoints>
|
|
<waypoint name="HOME" x="0.0" y="0.0"/>
|
|
<waypoint name="CLIMB" x="0.0" y="5.0"/>
|
|
<waypoint name="STDBY" x="-2.0" y="-5.0"/>
|
|
<waypoint name="p1" x="-19.9" y="14.3"/>
|
|
<waypoint name="p2" x="-0.6" y="21.6"/>
|
|
<waypoint name="p3" x="22.2" y="-26.5"/>
|
|
<waypoint name="p4" x="4.9" y="-34.4"/>
|
|
<waypoint name="CAM" x="14.2" y="-29.4"/>
|
|
<waypoint name="TD" x="5.6" y="-10.9"/>
|
|
<waypoint name="_G1" x="-31.4" y="27.3"/>
|
|
<waypoint name="_G2" x="0.5" y="37.0"/>
|
|
<waypoint name="_G3" x="35.1" y="-33.6"/>
|
|
<waypoint name="_G4" x="2.5" y="-46.9"/>
|
|
</waypoints>
|
|
<sectors>
|
|
<sector color="red" name="Flight_Area">
|
|
<corner name="_G1"/>
|
|
<corner name="_G2"/>
|
|
<corner name="_G3"/>
|
|
<corner name="_G4"/>
|
|
</sector>
|
|
</sectors>
|
|
<exceptions>
|
|
<exception cond="!InsideFlight_Area(GetPosX(), GetPosY())" deroute="Holding point"/>
|
|
<exception cond="((radio_control.status == RC_REALLY_LOST) @AND
|
|
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
|
|
!(nav_block >= IndexOfBlock('land here')) @AND
|
|
(autopilot_in_flight() == true) )" deroute="descend att 0"/>
|
|
<exception cond="(datalink_time @GT 60 @AND
|
|
!(datalink_time @GT 120) @AND
|
|
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
|
|
!(nav_block >= IndexOfBlock('land here')) @AND
|
|
(autopilot_in_flight() == true) )" deroute="ComeBackAndLand"/>
|
|
<exception cond="(electrical.bat_low @AND
|
|
!(IndexOfBlock('Holding point') @GT nav_block) @AND
|
|
!(nav_block >= IndexOfBlock('land here')) @AND
|
|
(autopilot_in_flight() == true) )" deroute="ComeBackAndLand"/>
|
|
<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(), 10)"/>
|
|
<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 name="Start Engine">
|
|
<call_once fun="NavResurrect()"/>
|
|
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
|
</block>
|
|
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
|
|
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
|
|
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
|
|
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
|
|
</block>
|
|
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
|
<stay wp="STDBY"/>
|
|
</block>
|
|
<block name="stay_p1">
|
|
<stay wp="p1"/>
|
|
</block>
|
|
<block name="go_p2">
|
|
<go wp="p2"/>
|
|
<deroute block="stay_p1"/>
|
|
</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">
|
|
<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="stay_p1"/>
|
|
</block>
|
|
<block name="test yaw">
|
|
<go wp="p1"/>
|
|
<for from="1" to="16" var="i">
|
|
<heading alt="WaypointAlt(WP_p1)" course="90 * $i" until="stage_time @GT 3"/>
|
|
</for>
|
|
<deroute block="Standby"/>
|
|
</block>
|
|
<block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
|
|
<circle radius="nav.radius" wp="CAM"/>
|
|
</block>
|
|
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
|
|
<call_once fun="NavSetWaypointHere(WP_TD)"/>
|
|
</block>
|
|
<block name="land">
|
|
<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" until="FALSE" vmode="throttle"/>
|
|
</block>
|
|
<block name="ComeBackAndLand">
|
|
<go wp="HOME"/>
|
|
<deroute block="land here"/>
|
|
</block>
|
|
<block name="descend att 0">
|
|
<attitude pitch="0" roll="0" throttle="0.40" until="stage_time @GT 30" vmode="throttle"/>
|
|
<deroute block="Holding point"/>
|
|
</block>
|
|
</blocks>
|
|
</flight_plan>
|