mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-30 11:37:06 +08:00
[imav] airframes and flight plans from IMAV2019
This commit is contained in:
Gautier Hattenberger
@@ -0,0 +1,304 @@
|
||||
<!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 "subsystems/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" type="dynamic">
|
||||
<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" type="dynamic">
|
||||
<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) &&
|
||||
!(IndexOfBlock('Takeoff') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
|
||||
(autopilot.launch == true) )" deroute="EmergencyLanding"/>
|
||||
<exception cond="(datalink_time > 10 &&
|
||||
!(IndexOfBlock('Takeoff') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
|
||||
(autopilot.launch == true) )" deroute="EmergencyLanding"/>
|
||||
<exception cond="(delay_test_gf(!InsideFlyZone(GetPosX(), GetPosY()),10) &&
|
||||
!(IndexOfBlock('Takeoff') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
|
||||
(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="stateGetPositionEnu_f()->z > 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="stateGetPositionEnu_f()->z > 2.0"/>
|
||||
<stay wp="CLIMB" until="GetPosAlt() > 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 > 3"/>
|
||||
<go wp="BLUE" from="RED" hmode="route"/>
|
||||
<stay wp="BLUE" until="stage_time > 3"/>
|
||||
<go wp="YELLOW" from="BLUE"/>
|
||||
<stay wp="YELLOW" until="stage_time > 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="stateGetPositionEnu_f()->z > 2.0"/>
|
||||
<stay wp="CLIMB" until="GetPosAlt() > 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="stateGetPositionEnu_f()->z > 2.0"/>
|
||||
<stay wp="CLIMB" until="GetPosAlt() > 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="stateGetPositionEnu_f()->z > 2.0"/>
|
||||
<stay wp="CLIMB" until="GetPosAlt() > 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="stateGetPositionEnu_f()->z > 2.0"/>
|
||||
<stay wp="CLIMB" until="GetPosAlt() > 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
Block a user