Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity
Files
master
paparazzi/conf/flight_plans/competitions/IMAV2019_drop.xml
T
Gautier Hattenberger 28ef30ed80 Nav rotorcraft rework (#2964) 
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>
2023-03-02 22:15:23 +01:00

305 lines
14 KiB
XML
Raw Permalink Blame History

<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="790" ground_alt="775" home_mode_height="15" lat0="40.1579607" lon0="-3.3813882" max_dist_from_home="400" name="IMAV2019 Drop" qfu="250." security_height="10">
<header>
#include "modules/datalink/datalink.h"
static inline bool delay_test_rc(bool test, int delay) {
static int nb = 0;
if (test) {
nb++;
if (nb == delay) {
nb = 0;
return true;
}
return false;
} else {
nb = 0;
return false;
}
}
static inline bool delay_test_gf(bool test, int delay) {
static int nb = 0;
if (test) {
nb++;
if (nb == delay) {
nb = 0;
return true;
}
return false;
} else {
nb = 0;
return false;
}
}
#ifdef SITL
#define SwitchServoOff(_x) {}
#ifdef NAV_C
#define jevois_stream(_x) {}
#endif
#endif
</header>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="STDBY" x="92.7" y="25.3"/>
<waypoint name="TD" x="100.2" y="36.1"/>
<waypoint name="CLIMB" x="89.9" y="14.1"/>
<waypoint name="_NFZ1" x="238.6" y="74.6"/>
<waypoint name="_NFZ2" x="259.1" y="-43.6"/>
<waypoint name="_NFZ3" x="57.2" y="-204.1"/>
<waypoint name="_NFZ4" x="-138.0" y="-237.3"/>
<waypoint name="_NFZ5" x="-202.5" y="-133.7"/>
<waypoint name="_NFZ6" x="-127.3" y="10.6"/>
<waypoint name="S1" x="-101.1" y="-7.9"/>
<waypoint name="S2" x="29.2" y="-7.1"/>
<waypoint name="S3" x="28.6" y="-73.1"/>
<waypoint name="S4" x="-101.4" y="-74.0"/>
<waypoint name="RED" x="-41.2" y="-56.6"/>
<waypoint name="BLUE" x="-83.5" y="-50.8"/>
<waypoint name="YELLOW" x="-8.2" y="-51.9"/>
<waypoint name="HOUSE" x="7.5" y="-26.7"/>
<waypoint name="_HERE" x="86.0" y="-117.2"/>
</waypoints>
<sectors>
<sector color="red" name="FlyZone">
<corner name="_NFZ1"/>
<corner name="_NFZ2"/>
<corner name="_NFZ3"/>
<corner name="_NFZ4"/>
<corner name="_NFZ5"/>
<corner name="_NFZ5"/>
<corner name="_NFZ6"/>
</sector>
<sector color="orange" name="Search">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
<corner name="S4"/>
</sector>
</sectors>
<variables>
<variable init="30." max="60." min="10." step="1." var="map_alt"/>
</variables>
<modules>
<module name="cv" type="target_localization"/>
<module name="nav" type="survey_rectangle_rotorcraft"/>
</modules>
<!--exceptions>
<exception cond="(delay_test_rc(RCLost(),20) @AND
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('Land Right AF-TD')) @AND
(autopilot.launch == true) )" deroute="EmergencyLanding"/>
<exception cond="(datalink_time @GT 10 @AND
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('Land Right AF-TD')) @AND
(autopilot.launch == true) )" deroute="EmergencyLanding"/>
<exception cond="(delay_test_gf(!InsideFlyZone(GetPosX(), GetPosY()),10) @AND
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('Land Right AF-TD')) @AND
(autopilot.launch == true) )" deroute="EmergencyLanding"/>
</exceptions-->
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
<block name="Start Engine" strip_button="Start Engine" strip_icon="resurrect.png" group="home">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav.climb_vspeed" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<call_once fun="jevois_stream(false)"/>
<stay wp="STDBY"/>
</block>
<block name="Mission Survey" strip_button="Mission Survey" group="survey">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time>2"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav.climb_vspeed" wp="CLIMB" until="GetPosHeight() @GT 2.0"/>
<stay wp="CLIMB" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_CLIMB) - 5.0"/>
</block>
<block name="Survey-WE SET" strip_button="Survey-WE" strip_icon="survey_we.png" group="survey">
<call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S3, sweep, WE)"/>
<deroute block="Survey RUN"/>
</block>
<block name="Survey RUN">
<exception cond="rectangle_survey_sweep_num >= 1" deroute="land"/>
<call_once fun="jevois_stream(true)"/>
<call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S3)"/>
</block>
<block name="Visite Mailbox" strip_button="Visit WP" group="survey">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(10.)"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<go wp="RED" from="_HERE" hmode="route"/>
<stay wp="RED" until="stage_time @GT 3"/>
<go wp="BLUE" from="RED" hmode="route"/>
<stay wp="BLUE" until="stage_time @GT 3"/>
<go wp="YELLOW" from="BLUE"/>
<stay wp="YELLOW" until="stage_time @GT 3"/>
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_STDBY))"/>
<go wp="STDBY" from="YELLOW"/>
<deroute block="Standby"/>
</block>
<block name="Mission RED" strip_button="Start RED" group="start drop">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time>2"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav.climb_vspeed" wp="CLIMB" until="GetPosHeight() @GT 2.0"/>
<stay wp="CLIMB" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_CLIMB) - 5.0"/>
</block>
<block name="Drop RED" strip_button="RED" group="drop">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_RED))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<!--call_once fun="jevois_stream(true)"/-->
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_RED)-5.0"/>
<go from="_HERE" hmode="route" wp="RED"/>
<!--set value="true" var="target_localization_update_wp"/-->
<stay wp="RED" climb="-1.0" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 10.0"/>
<stay wp="RED" climb="-0.5" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 3.0"/>
<set value="false" var="target_localization_update_wp"/>
<stay wp="RED" alt="3.0" until="stage_time @GT 3"/>
<call_once fun="SwitchServoOff()"/>
<stay wp="RED" alt="5.0" until="stage_time @GT 2"/>
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_TD))"/>
<call_once fun="jevois_stream(false)"/>
<deroute block="land"/>
</block>
<block name="Stay RED">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_RED))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_RED)-5.0"/>
<go from="_HERE" hmode="route" wp="RED"/>
<stay wp="RED"/>
</block>
<block name="Mission BLUE" strip_button="Start BLUE" group="start drop">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time>2"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav.climb_vspeed" wp="CLIMB" until="GetPosHeight() @GT 2.0"/>
<stay wp="CLIMB" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_CLIMB) - 5.0"/>
</block>
<block name="Drop BLUE" strip_button="BLUE" group="drop">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_BLUE))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<!--call_once fun="jevois_stream(true)"/-->
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_BLUE)-5.0"/>
<go from="_HERE" hmode="route" wp="BLUE"/>
<!--set value="true" var="target_localization_update_wp"/-->
<stay wp="BLUE" climb="-1.0" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 10.0"/>
<stay wp="BLUE" climb="-0.5" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 3.0"/>
<set value="false" var="target_localization_update_wp"/>
<stay wp="BLUE" alt="3.0" until="stage_time @GT 3"/>
<call_once fun="SwitchServoOff()"/>
<stay wp="BLUE" alt="5.0" until="stage_time @GT 2"/>
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_TD))"/>
<call_once fun="jevois_stream(false)"/>
<deroute block="land"/>
</block>
<block name="Stay BLUE">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_BLUE))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_BLUE)-5.0"/>
<go from="_HERE" hmode="route" wp="BLUE"/>
<stay wp="BLUE"/>
</block>
<block name="Mission YELLOW" strip_button="Start YELLOW" group="start drop">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time>2"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav.climb_vspeed" wp="CLIMB" until="GetPosHeight() @GT 2.0"/>
<stay wp="CLIMB" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_CLIMB) - 5.0"/>
</block>
<block name="Drop YELLOW" strip_button="YELLOW" group="drop">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_YELLOW))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<!--call_once fun="jevois_stream(true)"/-->
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_YELLOW)-5.0"/>
<go from="_HERE" hmode="route" wp="YELLOW"/>
<!--set value="true" var="target_localization_update_wp"/-->
<stay wp="YELLOW" climb="-1.0" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 10.0"/>
<stay wp="YELLOW" climb="-0.5" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 3.0"/>
<set value="false" var="target_localization_update_wp"/>
<stay wp="YELLOW" alt="3.0" until="stage_time @GT 3"/>
<call_once fun="SwitchServoOff()"/>
<stay wp="YELLOW" alt="5.0" until="stage_time @GT 2"/>
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_TD))"/>
<call_once fun="jevois_stream(false)"/>
<deroute block="land"/>
</block>
<block name="Stay YELLOW">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_YELLOW))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_YELLOW)-5.0"/>
<go from="_HERE" hmode="route" wp="YELLOW"/>
<stay wp="YELLOW"/>
</block>
<block name="Mission HOUSE" strip_button="Start HOUSE" group="start drop">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time>2"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav.climb_vspeed" wp="CLIMB" until="GetPosHeight() @GT 2.0"/>
<stay wp="CLIMB" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_CLIMB) - 5.0"/>
</block>
<block name="Drop HOUSE" strip_button="HOUSE" group="drop">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_HOUSE))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_HOUSE)-5.0"/>
<go from="_HERE" hmode="route" wp="HOUSE"/>
<stay wp="HOUSE" climb="-1.0" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 10.0"/>
<stay wp="HOUSE" climb="-0.5" vmode="climb" until="GetPosAlt() @LT GetAltRef() + 1.5"/>
<call_once fun="SwitchServoOff()"/>
<stay wp="HOUSE" alt="5.0" until="stage_time @GT 4"/>
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_TD))"/>
<deroute block="land"/>
</block>
<block name="Stay HOUSE">
<set var="nav_flight_altitude" value="POS_BFP_OF_REAL(WaypointAlt(WP_HOUSE))"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<stay wp="_HERE" until="GetPosAlt() @GT GetAltRef() + WaypointAlt(WP_YELLOW)-5.0"/>
<go from="_HERE" hmode="route" wp="HOUSE"/>
<stay wp="HOUSE"/>
</block>
<block name="land here">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="jevois_stream(false)"/>
<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>
<block name="EmergencyLanding">
<stay throttle="0.1" vmode="throttle" wp="HOME"/>
</block>
<block name="SetLandHere">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
<return/>
</block>
</blocks>
</flight_plan>
Reference in New Issue View Git Blame Copy Permalink