mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-24 22:05:58 +08:00
[flight_plan] Outback Flightplan (#1934)
This commit is contained in:
Christophe De Wagter
committed by
Gautier Hattenberger
Gautier Hattenberger
parent
3b52a30681
commit
c8299abd7c
@@ -258,7 +258,7 @@
|
||||
airframe="airframes/TUDELFT/tudelft_outback.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/TUDELFT/tudelft_outback.xml"
|
||||
flight_plan="flight_plans/TUDELFT/tudelft_delft_outback.xml"
|
||||
flight_plan="flight_plans/TUDELFT/obc2016/obc2016.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml"
|
||||
settings_modules="modules/imu_common.xml modules/gps.xml modules/ahrs_int_cmpl_quat.xml modules/opa_controller.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml"
|
||||
gui_color="#ffffdffac31f"
|
||||
|
||||
@@ -0,0 +1,408 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../../flight_plan.dtd">
|
||||
<!--
|
||||
Australian UAV Outback Challenge of 2014.
|
||||
|
||||
* TU Delft University of Technology, L&R http://www.tudelft.nl
|
||||
* MAVLab - http://mavlab.info
|
||||
|
||||
-->
|
||||
|
||||
<procedure>
|
||||
<!-- ******************************** HEADERS ****************************** -->
|
||||
<header>
|
||||
#include "autopilot.h"
|
||||
#include "guidance/guidance_hybrid.h"
|
||||
#include "guidance/guidance_v.h"
|
||||
#include "guidance/guidance_h.h"
|
||||
#include "subsystems/electrical.h"
|
||||
#include "guidance/guidance_indi.h"
|
||||
#include "subsystems/navigation/waypoints.h"
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
#include "modules/sensors/temp_adc.h"
|
||||
// States
|
||||
#define AircraftIsBooting() LessThan(nav_block,4)
|
||||
|
||||
</header>
|
||||
<!-- ******************************* WAYPOINTS ***************************** -->
|
||||
<waypoints>
|
||||
<waypoint name="CLIMB" x="100" y="300"/>
|
||||
<waypoint name="STDBY" x="30" y="30"/>
|
||||
<waypoint name="HOVER" x="268.5" y="27.1" height="60"/>
|
||||
<waypoint name="LANDSPOT" x="268.5" y="127.1" height="40"/>
|
||||
<waypoint name="LANDING" x="-4800.1" y="-9409.5" height="40"/>
|
||||
<waypoint name="JOE" x="268.5" y="127.1" height="40"/>
|
||||
</waypoints>
|
||||
|
||||
<!-- ******************************** SECTORS ****************************** -->
|
||||
|
||||
|
||||
<!-- ****************************** EXCEPTIONS ***************************** -->
|
||||
<exceptions>
|
||||
|
||||
<!-- Geofence -->
|
||||
<exception cond="(!InsideSafety(GetPosX(), GetPosY()) &&
|
||||
!(AircraftIsBooting()) &&
|
||||
!(nav_block == IndexOfBlock('Geo_init')) &&
|
||||
!(nav_block == IndexOfBlock('HoldingPoint')) &&
|
||||
!(nav_block == IndexOfBlock('failsafe')) &&
|
||||
!(nav_block == IndexOfBlock('SLAMDunk_init')) )" deroute="failsafe"/>
|
||||
|
||||
<!-- GPS loss -->
|
||||
<exception cond="(!GpsFixValid() &&
|
||||
!(IndexOfBlock('Takeoff') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('landed')) &&
|
||||
!(nav_block == IndexOfBlock('GpsLoss')) &&
|
||||
!(nav_block == IndexOfBlock('Descent_noGPS')) &&
|
||||
!(nav_block == IndexOfBlock('Descent_slow_noGPS')) &&
|
||||
!(nav_block == IndexOfBlock('landing_moment_noGPS')) &&
|
||||
!(nav_block == IndexOfBlock('failsafe')) &&
|
||||
(autopilot_in_flight == true) )" deroute="GpsLoss"/>
|
||||
|
||||
<!-- Datalink loss -->
|
||||
<exception cond="(datalink_time > 60 &&
|
||||
!(datalink_time > 120) &&
|
||||
!(IndexOfBlock('Takeoff') > nav_block) &&
|
||||
!(nav_block >= IndexOfBlock('landed')) &&
|
||||
!(nav_block == IndexOfBlock('GpsLoss')) &&
|
||||
!(nav_block == IndexOfBlock('inert')) &&
|
||||
!(nav_block == IndexOfBlock('failsafe')) &&
|
||||
(autopilot_in_flight == true) )" deroute="DatalinkLoss"/>
|
||||
|
||||
<!-- Stall detect -->
|
||||
<exception cond="((outback_hybrid_mode == HB_FORWARD) &&
|
||||
(12.0 > stateGetAirspeed_f()) &&
|
||||
has_transitioned &&
|
||||
(autopilot_in_flight == true) &&
|
||||
!(nav_block == IndexOfBlock('Hover_here')) )" deroute="Hover_here"/>
|
||||
|
||||
</exceptions>
|
||||
|
||||
|
||||
|
||||
<!-- *********************************************************************** -->
|
||||
<!-- ********************** FLIGHTPLAN STARTINGPOINT *********************** -->
|
||||
<!-- *********************************************************************** -->
|
||||
|
||||
<!-- *********** Wait for GPS fix, 3D by default *********** -->
|
||||
<blocks>
|
||||
|
||||
<!-- Initialization (No Geofence check, No datalink check, No RPM check, No stall check) -->
|
||||
<block name="Wait_GPS">
|
||||
<set value="true" var="opa_controller_vision_power"/> <!-- Power on the Kalamos -->
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<call fun="nav_throttle_curve_set(0)" loop="false"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo_init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<call fun="nav_throttle_curve_set(0)" loop="false"/>
|
||||
<call fun="NavSetAltitudeReferenceHere()"/>
|
||||
<call fun="nav_set_heading_current()" loop="false"/>
|
||||
</block>
|
||||
<block name="SLAMDunk_init">
|
||||
<manual pitch="0" roll="0" throttle="0" until="getKalamosReady()" vmode="throttle" yaw="0"/> <!-- Check if Kalamos is booted -->
|
||||
<call fun="enableKalamosAttCalib(true)" until="stage_time>15"/> <!-- Enable Kalamos calibration for 15 seconds -->
|
||||
<call fun="enableKalamosAttCalib(false)" loop="false"/> <!-- Disable Kalamos calibration -->
|
||||
<set value="false" var="opa_controller_vision_power"/> <!-- Poweroff the Kalamos -->
|
||||
</block>
|
||||
<block name="HoldingPoint">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<call fun="nav_throttle_curve_set(0)" loop="false"/>
|
||||
<call fun="set_wind_heading_to_current90()" loop="false"/> <!-- Set heading based on wind -->
|
||||
<manual pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle" yaw="0"/>
|
||||
</block>
|
||||
|
||||
<!-- Takeoff (No RPM check, No stall check) -->
|
||||
<block group="TO" name="Start_Engine" strip_button="Start_Engine">
|
||||
<call fun="nav_throttle_curve_set(0)" loop="false"/>
|
||||
<call fun="nav_heli_spinup_setup(3, 0.07)" loop="false"/>
|
||||
<call fun="NavResurrect()"/>
|
||||
<call fun="nav_heli_spinup_run()"/>
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<manual pitch="0" roll="0" throttle="0" until="stage_time>6" vmode="throttle" yaw="0"/>
|
||||
</block>
|
||||
<block name="Hold_Attitude">
|
||||
<exception cond="1200 > throttle_curve.rpm_meas" deroute="Start_Engine"/>
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<call fun="nav_set_heading_current()" loop="false"/>
|
||||
<manual pitch="0" roll="0" throttle="0" until="stage_time>1" vmode="throttle" yaw="0"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
|
||||
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Climb"/>
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<attitude pitch="0" roll="0" throttle="0.83" until="stage_time>2" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Climb">
|
||||
<exception cond="stateGetPositionEnu_f()->z > 50.0" deroute="decide_there_or_back"/>
|
||||
<call fun="NavSetWaypointHere(WP_CLIMB)" loop="false"/>
|
||||
<call fun="nav_set_heading_current()" loop="false"/>
|
||||
<set value="RadOfDeg(35)" var="guidance_indi_max_bank"/>
|
||||
<call fun="GuidanceVSetRef(stateGetPositionNed_i()->z - POS_BFP_OF_REAL(5.0), -SPEED_BFP_OF_REAL(3.0), 0)"/>
|
||||
<stay precall="PrecallModeHover()" climb="nav_climb_vspeed" vmode="climb" wp="CLIMB" until="stateGetPositionEnu_f()->z > 20.0"/>
|
||||
<set value="GUIDANCE_H_MAX_BANK" var="guidance_indi_max_bank"/>
|
||||
<stay precall="PrecallModeHover()"climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
|
||||
</block>
|
||||
|
||||
<!-- Decide where the takeoff was used for -->
|
||||
<block name="decide_there_or_back">
|
||||
<exception cond="6 > last_wp_reached_in_route" deroute="line_p1_p2"/> <!-- Deroute back corridor to joe -->
|
||||
<exception cond="last_wp_reached_in_route > 5" deroute="line_p9_p8"/> <!-- Deroute back corridor to home -->
|
||||
<call fun="NavSetWaypointHere(WP_HOVER)" loop="false"/>
|
||||
<stay precall="PrecallModeHover()"wp="HOVER" until="stage_time > 1"/>
|
||||
</block>
|
||||
|
||||
<!-- Safety blocks -->
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<set value="GUIDANCE_H_MAX_BANK" var="guidance_indi_max_bank"/>
|
||||
<stay precall="PrecallModeHover()"wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Hover_here">
|
||||
<call fun="waypoint_set_alt(WP_HOVER,stateGetPositionEnu_f()->z)" loop="false"/>
|
||||
<call fun="NavSetWaypointHere(WP_HOVER)" loop="false"/>
|
||||
<stay precall="PrecallModeHover()"wp="HOVER"/>
|
||||
</block>
|
||||
|
||||
<!-- Flightpath to joe -->
|
||||
<block group="to_joe" name="line_p1_p2">
|
||||
<go precall="PrecallModeForward()" from="p1" hmode="route" wp="intermediate_wp"/>
|
||||
<go precall="PrecallModeForward()" from="p1" hmode="route" wp="p2"/>
|
||||
<set value="2" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p2_p3"/>
|
||||
</block>
|
||||
<block group="to_joe" name="line_p2_p3">
|
||||
<go precall="PrecallModeForward()" from="p2" hmode="route" wp="p3"/>
|
||||
<set value="3" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p3_p4"/>
|
||||
</block>
|
||||
<block group="to_joe" name="line_p3_p4">
|
||||
<go precall="PrecallModeForward()" from="p3" hmode="route" wp="p4"/>
|
||||
<set value="4" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p4_p5"/>
|
||||
</block>
|
||||
<block group="to_joe" name="line_p4_p5">
|
||||
<go precall="PrecallModeForward()" from="p4" hmode="route" wp="p5"/>
|
||||
<set value="5" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p5_p6"/>
|
||||
</block>
|
||||
<block group="to_joe" name="line_p5_p6">
|
||||
<go precall="PrecallModeForward()" from="p5" hmode="route" wp="p6"/>
|
||||
<set value="6" var="last_wp_reached_in_route"/>
|
||||
<set value="true" var="opa_controller_vision_power"/>
|
||||
<deroute block="line_p6_p7"/>
|
||||
</block>
|
||||
<block group="to_joe" name="line_p6_p7">
|
||||
<go precall="PrecallModeForward()" from="p6" hmode="route" wp="p7"/>
|
||||
<set value="7" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p7_p8"/>
|
||||
</block>
|
||||
<block group="to_joe" name="line_p7_p8">
|
||||
<go precall="PrecallModeForward()" from="p7" hmode="route" wp="p8"/>
|
||||
<set value="8" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p8_p9"/>
|
||||
</block>
|
||||
<block group="to_joe" name="line_p8_p9">
|
||||
<go precall="PrecallModeForward()" from="p8" hmode="route" wp="p9"/>
|
||||
<set value="9" var="last_wp_reached_in_route"/>
|
||||
<deroute block="search_joe"/>
|
||||
</block>
|
||||
|
||||
<!-- Flightpath from Joe -->
|
||||
<block group="return" name="line_p9_p8">
|
||||
<exception cond="9 > last_wp_reached_in_route" deroute="line_p8_p7"/>
|
||||
<go precall="PrecallModeForward()" from="p9" hmode="route" wp="p8"/>
|
||||
<set value="8" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p8_p7"/>
|
||||
</block>
|
||||
<block group="return" name="line_p8_p7">
|
||||
<exception cond="8 > last_wp_reached_in_route" deroute="line_p7_p6"/>
|
||||
<go precall="PrecallModeForward()" from="p8" hmode="route" wp="p7"/>
|
||||
<set value="7" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p7_p8"/>
|
||||
</block>
|
||||
<block group="return" name="line_p7_p6">
|
||||
<exception cond="7 > last_wp_reached_in_route" deroute="line_p6_p5"/>
|
||||
<go precall="PrecallModeForward()" from="p7" hmode="route" wp="p6"/>
|
||||
<set value="6" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p6_p5"/>
|
||||
</block>
|
||||
<block group="return" name="line_p6_p5">
|
||||
<exception cond="6 > last_wp_reached_in_route" deroute="line_p5_p4"/>
|
||||
<go precall="PrecallModeForward()" from="p6" hmode="route" wp="p5"/>
|
||||
<set value="5" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p5_p4"/>
|
||||
</block>
|
||||
<block group="return" name="line_p5_p4">
|
||||
<exception cond="5 > last_wp_reached_in_route" deroute="line_p4_p3"/>
|
||||
<go precall="PrecallModeForward()" from="p5" hmode="route" wp="p4"/>
|
||||
<set value="4" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p4_p3"/>
|
||||
</block>
|
||||
<block group="return" name="line_p4_p3">
|
||||
<exception cond="4 > last_wp_reached_in_route" deroute="line_p3_p2"/>
|
||||
<go precall="PrecallModeForward()" from="p4" hmode="route" wp="p3"/>
|
||||
<set value="3" var="last_wp_reached_in_route"/>
|
||||
<deroute block="line_p3_p2"/>
|
||||
</block>
|
||||
<block group="return" name="line_p3_p2">
|
||||
<exception cond="3 > last_wp_reached_in_route" deroute="line_p2_p1"/>
|
||||
<go precall="PrecallModeForward()" from="p3" hmode="route" wp="p2"/>
|
||||
<set value="2" var="last_wp_reached_in_route"/>
|
||||
<set value="true" var="opa_controller_vision_power"/>
|
||||
<deroute block="line_p2_p1"/>
|
||||
</block>
|
||||
<block group="return" name="line_p2_p1">
|
||||
<go precall="PrecallModeForward()" from="p2" hmode="route" wp="p1"/>
|
||||
<set value="1" var="last_wp_reached_in_route"/>
|
||||
<deroute block="Descent"/>
|
||||
</block>
|
||||
|
||||
<!-- Landing at base-->
|
||||
<block name="Descent">
|
||||
<exception cond="12.0 > stateGetPositionEnu_f()->z" deroute="Descent_slow"/>
|
||||
<call fun="NavSetWaypointHere(WP_TD)" loop="false"/>
|
||||
<call fun="enableKalamosDescent(true)" loop="false"/>
|
||||
<call fun="GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0)"/>
|
||||
<stay precall="PrecallModeHover()"climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
|
||||
</block>
|
||||
<block name="Descent_slow">
|
||||
<exception cond="((0.6 + fabs(stateGetNedToBodyEulers_f()->phi)/2) > k2p_package.height ) && (k2p_package.height > 0.0) && getKalamosReady()" deroute="landing_moment"/>
|
||||
<exception cond="(fabs(stateGetNedToBodyEulers_f()->theta) > 0.6) && ( 2.5 > k2p_package.height) && (!(last_block==IndexOfBlock('Descent_pause')))" deroute="Descent_pause"/>
|
||||
<stay precall="PrecallModeHover()"climb="-0.5" vmode="climb" wp="TD"/>
|
||||
</block>
|
||||
<block name="Descent_pause">
|
||||
<exception cond="(0.3 > fabs(stateGetNedToBodyEulers_f()->theta) )" deroute="Descent_pause"/>
|
||||
<stay precall="PrecallModeHover()"climb="0.0" vmode="climb" wp="TD" until="stage_time>30"/>
|
||||
<deroute block="Descent_slow"/>
|
||||
</block>
|
||||
<block name="landing_moment">
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<attitude pitch="0" roll="0" throttle="0.5" until="stage_time>2" vmode="throttle"/>
|
||||
<deroute block="landed"/>
|
||||
</block>
|
||||
|
||||
<!-- Landing without GPS -->
|
||||
<block name="Descent_noGPS">
|
||||
<set value="true" var="opa_controller_vision_power"/> <!-- Vision power when no gps auto landing -->
|
||||
<call fun="enableKalamosDescent(true)" loop="false"/>
|
||||
<exception cond="10.0 > stateGetPositionEnu_f()->z" deroute="Descent_slow_noGPS"/>
|
||||
<call fun="nav_set_heading_current()" loop="false"/>
|
||||
<call fun="NavSetWaypointHere(WP_TD)" loop="false"/>
|
||||
<call fun="GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0)"/>
|
||||
<attitude pitch="0" roll="0" climb="nav_descend_vspeed" vmode="climb"/>
|
||||
</block>
|
||||
<block name="Descent_slow_noGPS">
|
||||
<exception cond="((0.6 + fabs(stateGetNedToBodyEulers_f()->phi)/2) > k2p_package.height) && (k2p_package.height > 0.0) && getKalamosReady()" deroute="landing_moment"/>
|
||||
<attitude pitch="0" roll="0" climb="-0.5" vmode="climb"/>
|
||||
</block>
|
||||
<block name="landing_moment_noGPS">
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<attitude pitch="0" roll="0" throttle="0.5" until="stage_time>2" vmode="throttle"/>
|
||||
<deroute block="landed"/>
|
||||
</block>
|
||||
<!-- Search for joe -->
|
||||
<block name="search_joe">
|
||||
<call fun="enableKalamosLandingspotSearch(false)" loop="false"/>
|
||||
<call fun="enableKalamosFindJoe(false)" loop="false"/>
|
||||
<call fun="enableKalamosDescent(false)" loop="false"/>
|
||||
<go precall="PrecallModeForward()" from="p9" hmode="route" wp="j1"/>
|
||||
<stay precall="PrecallModeHover()"wp="j1" until="stage_time>4"/>
|
||||
<call fun="enableKalamosFindJoe(true)" loop="false"/>
|
||||
<go precall="PrecallModeHover()" wp="j2" hmode="route" from="j1"/>
|
||||
<stay precall="PrecallModeHover()"wp="j2" until="stage_time>10"/>
|
||||
<call fun="enableKalamosFindJoe(false)" loop="false"/>
|
||||
<go precall="PrecallModeHover()" wp="LANDING"/>
|
||||
<stay precall="PrecallModeHover()"wp="LANDING" until="stage_time > 30"/>
|
||||
<deroute block="Do_vision_landing"/>
|
||||
</block>
|
||||
|
||||
<!-- Circle in case necessary, to be commanded by operator-->
|
||||
<block name="Circle_around">
|
||||
<go precall="PrecallModeForward()" from="j2" hmode="route" wp="j1"/>
|
||||
<go precall="PrecallModeForward()" from="j1" hmode="route" wp="j3"/>
|
||||
<go precall="PrecallModeForward()" from="j3" hmode="route" wp="j4"/>
|
||||
<go precall="PrecallModeForward()" from="j4" hmode="route" wp="j5"/>
|
||||
<go precall="PrecallModeForward()" from="j5" hmode="route" wp="j1"/>
|
||||
</block>
|
||||
|
||||
<!-- Vision based landing -->
|
||||
<block name="Do_vision_landing">
|
||||
<exception cond="6.0 > stateGetPositionEnu_f()->z" deroute="Descent_slow_remote"/>
|
||||
<call fun="enableKalamosFindJoe(false)" loop="false"/>
|
||||
<call fun="enableKalamosLandingspotSearch(false)" loop="false"/>
|
||||
<call fun="enableKalamosDescent(true)" loop="false"/>
|
||||
<stay precall="PrecallModeHover()"climb="land_cmd.z" vmode="climb" wp="LANDING"/>
|
||||
</block>
|
||||
<block name="Descent_slow_remote">
|
||||
<exception cond="((0.6 + fabs(stateGetNedToBodyEulers_f()->phi)/2) > k2p_package.height ) && (k2p_package.height > 0.0) && getKalamosReady()" deroute="landing_moment_remote"/>
|
||||
<exception cond="(fabs(stateGetNedToBodyEulers_f()->theta) > 0.6) && ( 2.5 > k2p_package.height) && (!(last_block==IndexOfBlock('Descent_pause_remote')))" deroute="Descent_pause_remote"/>
|
||||
<stay precall="PrecallModeHover()"climb="-0.5" vmode="climb" wp="LANDING"/>
|
||||
</block>
|
||||
<block name="Descent_pause_remote">
|
||||
<exception cond="(0.3 > fabs(stateGetNedToBodyEulers_f()->theta) )" deroute="Descent_slow_remote"/>
|
||||
<stay precall="PrecallModeHover()"climb="0.0" vmode="climb" wp="LANDING" until="stage_time>30"/>
|
||||
<deroute block="Descent_slow_remote"/>
|
||||
</block>
|
||||
<block name="landing_moment_remote">
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<attitude pitch="0" roll="0" throttle="0.5" until="stage_time>2" vmode="throttle"/>
|
||||
<deroute block="landed"/>
|
||||
</block>
|
||||
<block name="landed">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<call fun="nav_throttle_curve_set(0)" loop="false"/>
|
||||
<set value="false" var="opa_controller_vision_power"/> <!-- Vision power off at remote landing -->
|
||||
<attitude pitch="0" roll="0" throttle="0" until="stage_time>5" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
<!-- Inert and wait minute -->
|
||||
<block name="inert">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<call fun="NavOpaDisarm(true)"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="stage_time>2" vmode="throttle"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="electrical.vsupply>220" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="wait_minute">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="(stage_time>60) && GpsFixValid()" vmode="throttle"/>
|
||||
<deroute block="Start_Engine"/>
|
||||
</block>
|
||||
|
||||
<!-- Emergency modes -->
|
||||
<block name="GpsLoss">
|
||||
<!-- Less than 2 meters goto failsafe -->
|
||||
<exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="landing_moment_noGPS"/>
|
||||
<!-- Deroute to hover here if below 20 meters and GPS fix becomes valid -->
|
||||
<exception cond="GpsFixValid() && (20.0 > stateGetPositionEnu_f()->z)" deroute="Hover_here"/>
|
||||
<!-- Goto descent without GPS after 20 seconds without GPS -->
|
||||
<exception cond="block_time>20" deroute="Descent_noGPS"/>
|
||||
<!-- Set throttle curve 1 (hover) and descend with vspeed -->
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<attitude climb="nav_descend_vspeed" pitch="0" roll="0" until="GpsFixValid() && (stateGetPositionEnu_f()->z > 20.0)" vmode="climb"/>
|
||||
<!-- If GPS fix becomes valid and we are above 20 meters return to previous block -->
|
||||
<return reset_stage="1"/>
|
||||
</block>
|
||||
<block name="DatalinkLoss">
|
||||
<!-- If we have another 60 seconds datalink loss fly home through corridor -->
|
||||
<exception cond="block_time>120" deroute="line_p9_p8"/> <!-- fly back home -->
|
||||
<!-- If motor temperature becomes higher 100 degrees goto forwared and fly to home in corridor -->
|
||||
<exception cond="get_temp(1) > 100" deroute="line_p9_p8"/> <!-- fly back home -->
|
||||
<!-- Set Hover waypoint to current positions and set height -->
|
||||
<call fun="NavSetWaypointHere(WP_HOVER)" loop="false"/>
|
||||
<call fun="waypoint_set_alt(WP_HOVER,stateGetPositionEnu_f()->z + 20)" loop="false"/>
|
||||
<stay precall="PrecallModeHover()"wp="HOVER" until="60 > datalink_time"/>
|
||||
<!-- Return to previous block if datalink is OK -->
|
||||
<return reset_stage="1"/>
|
||||
<!-- if the function above did not work, keep in this block -->
|
||||
<stay precall="PrecallModeHover()"wp="HOVER"/>
|
||||
</block>
|
||||
<block name="failsafe">
|
||||
<!-- Ultimate KILL MODE -->
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<call fun="NavSetFailsafe"/>
|
||||
<call fun="INTERMCU_SET_CMD_STATUS(INTERMCU_CMD_FAILSAFE)" loop="false"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
</blocks>
|
||||
</procedure>
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../../flight_plan.dtd">
|
||||
|
||||
<procedure>
|
||||
|
||||
<sectors>
|
||||
<sector name="MissionBoundary" color="yellow" type="dynamic">
|
||||
<corner name="_MB1"/>
|
||||
<corner name="_MB2"/>
|
||||
<corner name="_MB3"/>
|
||||
<corner name="_MB4"/>
|
||||
<corner name="_MB5"/>
|
||||
<corner name="_MB6"/>
|
||||
<corner name="_MB7"/>
|
||||
<corner name="_MB8"/>
|
||||
<corner name="_MB9"/>
|
||||
<corner name="_MB10"/>
|
||||
<corner name="_MB11"/>
|
||||
<corner name="_MB12"/>
|
||||
<corner name="_MB13"/>
|
||||
<corner name="_MB14"/>
|
||||
<corner name="_MB15"/>
|
||||
<corner name="_MB16"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
|
||||
<!-- ****************************** EXCEPTIONS ***************************** -->
|
||||
<exceptions>
|
||||
<!-- RC-Loss -->
|
||||
<!-- Handled by FTD-Radio-Control -->
|
||||
|
||||
<!-- Datalink-Loss: -->
|
||||
<!-- Handled in Mission Navigation -->
|
||||
|
||||
<!-- GPS-Failure: -->
|
||||
<!-- Handled in Mission Navigation -->
|
||||
|
||||
<!-- Baro-Failure: -->
|
||||
<!-- Inform Operator, Fly lower, keep flying on GPS -->
|
||||
|
||||
<!-- Hard GeoFence -->
|
||||
<!-- TODO Maybe ADD aditional exception with 2 sec timer here just to be sure to avoid position GPS spikes or some other jitter issues -->
|
||||
<!--exception cond="((!InsideMissionBoundary(GetPosX(), GetPosY())) && gps_has_been_good()" deroute="GeoFence"/-->
|
||||
|
||||
<!-- Failure of Autopilot -->
|
||||
<!-- FTD will failsafe if no commands arrive during 100ms -->
|
||||
|
||||
<!-- Failure of Geofence detection processor -->
|
||||
<!-- FTD will failsafe if no heartbeat arrives from AP (with GPS) during 100ms -->
|
||||
|
||||
<!-- Soft Altitude GeoFence 2800ft MSL-->
|
||||
<!-- <exception cond="MoreThan(GetAmsl(), 2800) && gps_has_been_good()" deroute="AlmostHeightViolation" />-->
|
||||
|
||||
<!-- Hard Altitude GeoFence 3000ft MSL -->
|
||||
<!-- <exception cond="MoreThan(GetAmsl(), 3000) && gps_has_been_good()" deroute="HeightViolation" /-->
|
||||
|
||||
<!-- Case of loss of GPS AND loss of datalink -->
|
||||
<!-- exception cond="And(datalink_time > 10, !GpsFixValid())" deroute="DatalinkAndGpsFailure"/-->
|
||||
</exceptions>
|
||||
|
||||
<blocks>
|
||||
<!-- ******* Activate the irreversible Forced Crash Command to the Flight Termination Device ******** -->
|
||||
|
||||
<block name="GeoFence">
|
||||
<!--call fun="ForceCrash()"/-->
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
|
||||
<block name="AlmostHeightViolation">
|
||||
<!--call fun="NavKillThrottle()"/-->
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
|
||||
<block name="HeightViolation">
|
||||
<!--call fun="ForceCrash()"/-->
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
|
||||
<block name="DatalinkAndGpsFailure">
|
||||
<!--call fun="ForceCrash()"/-->
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
|
||||
<block name="ManualTerminateRequest" strip_button="FlightTermination" strip_icon="downdownend.png">
|
||||
<!--call fun="ForceCrash()"/-->
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
|
||||
</blocks>
|
||||
</procedure>
|
||||
|
||||
@@ -0,0 +1,71 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="423.8" ground_alt="343.8" home_mode_height="120" lat0="-27.273900" lon0="151.290039" max_dist_from_home="12038" name="OBC2016" qfu="170" security_height="10">
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint lat="-27.274012" lon="151.290038" name="TD"/>
|
||||
<waypoint lat="-27.3581" lon="151.240624" name="REPORT_JOE"/>
|
||||
<waypoint lat="-27.358357" lon="151.239732" name="j1" height="40"/>
|
||||
<waypoint lat="-27.357885" lon="151.241427" name="j2" height="40"/>
|
||||
<waypoint lat="-27.360014" lon="151.240522" name="j3" height="40"/>
|
||||
<waypoint lat="-27.357827" lon="151.247775" name="j4" height="40"/>
|
||||
<waypoint lat="-27.356253" lon="151.246831" name="j5" height="40"/>
|
||||
<waypoint lat="-27.282360" lon="151.289117" name="intermediate_wp" height="50"/>
|
||||
<waypoint lat="-27.270220" lon="151.295874" name="_MB1"/>
|
||||
<waypoint lat="-27.279114" lon="151.294158" name="_MB2"/>
|
||||
<waypoint lat="-27.319091" lon="151.286441" name="_MB3"/>
|
||||
<waypoint lat="-27.317933" lon="151.278923" name="_MB4"/>
|
||||
<waypoint lat="-27.277956" lon="151.286643" name="_MB5"/>
|
||||
<waypoint lat="-27.277580" lon="151.284204" name="_MB6"/>
|
||||
<waypoint lat="-27.276210" lon="151.275317" name="_MB7"/>
|
||||
<waypoint lat="-27.325115" lon="151.257217" name="_MB8"/>
|
||||
<waypoint lat="-27.358508" lon="151.255387" name="_MB9"/>
|
||||
<waypoint lat="-27.361077" lon="151.246946" name="_MB10"/>
|
||||
<waypoint lat="-27.363744" lon="151.237290" name="_MB11"/>
|
||||
<waypoint lat="-27.355123" lon="151.234302" name="_MB12"/>
|
||||
<waypoint lat="-27.352455" lon="151.243958" name="_MB13"/>
|
||||
<waypoint lat="-27.350364" lon="151.250673" name="_MB14"/>
|
||||
<waypoint lat="-27.324265" lon="151.252104" name="_MB15"/>
|
||||
<waypoint lat="-27.266754" lon="151.273390" name="_MB16"/>
|
||||
<waypoint lat="-27.268686" lon="151.285921" name="_MB17"/>
|
||||
<waypoint lat="-27.273826" lon="151.290156" name="p1" height="60"/>
|
||||
<waypoint lat="-27.316738" lon="151.281871" name="p2" height="120"/>
|
||||
<waypoint lat="-27.317045" lon="151.283862" name="p3" height="120"/>
|
||||
<waypoint lat="-27.278580" lon="151.291288" name="p4" height="120"/>
|
||||
<waypoint lat="-27.273606" lon="151.290506" name="p5" height="120"/>
|
||||
<waypoint lat="-27.271136" lon="151.274482" name="p6" height="120"/>
|
||||
<waypoint lat="-27.324690" lon="151.254661" name="p7" height="120"/>
|
||||
<waypoint lat="-27.354436" lon="151.253030" name="p8" height="120"/>
|
||||
<waypoint lat="-27.358100" lon="151.240624" name="p9" height="50"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="red" name="Safety" type="dynamic">
|
||||
<corner name="_MB1"/>
|
||||
<corner name="_MB2"/>
|
||||
<corner name="_MB3"/>
|
||||
<corner name="_MB4"/>
|
||||
<corner name="_MB5"/>
|
||||
<corner name="_MB6"/>
|
||||
<corner name="_MB7"/>
|
||||
<corner name="_MB8"/>
|
||||
<corner name="_MB9"/>
|
||||
<corner name="_MB10"/>
|
||||
<corner name="_MB11"/>
|
||||
<corner name="_MB12"/>
|
||||
<corner name="_MB13"/>
|
||||
<corner name="_MB14"/>
|
||||
<corner name="_MB15"/>
|
||||
<corner name="_MB16"/>
|
||||
<corner name="_MB17"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<modules>
|
||||
<module name="nav" type="heli_spinup"/>
|
||||
</modules>
|
||||
<includes>
|
||||
<include name="Mission" procedure="include_obc2016_mission.xml"/>
|
||||
</includes>
|
||||
<blocks>
|
||||
<block name="Boot"/>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -1,92 +0,0 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="5" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="60" name="Delft Basic" security_height="2">
|
||||
<header>
|
||||
#include "autopilot.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="TD" x="5.6" y="-10.9"/>
|
||||
</waypoints>
|
||||
<modules>
|
||||
<!-- extra navigation routines -->
|
||||
<module name="nav" type="heli_spinup"/>
|
||||
</modules>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<call fun="NavSetGroundReferenceHere()"/>
|
||||
<call fun="nav_set_heading_current()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<manual pitch="0" roll="0" yaw="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Start Engine">
|
||||
<call fun="nav_throttle_curve_set(0)" loop="false"/>
|
||||
<call fun="nav_heli_spinup_setup(7, 0.15)" loop="false"/>
|
||||
<call fun="NavResurrect()"/>
|
||||
<call fun="nav_heli_spinup_run()"/>
|
||||
<call fun="nav_throttle_curve_set(1)" loop="false"/>
|
||||
<manual pitch="0" roll="0" yaw="0" throttle="0.15" until="stage_time>7" vmode="throttle"/>
|
||||
<call fun="nav_throttle_curve_set(2)" loop="false"/>
|
||||
<manual pitch="0" roll="0" yaw="0" throttle="0.15" until="stage_time>3" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Hold Attitude">
|
||||
<manual pitch="0" roll="0" yaw="0" throttle="0.15" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
|
||||
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
|
||||
<call fun="nav_set_heading_current()"/>
|
||||
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
|
||||
<stay vmode="climb" climb="nav_climb_vspeed" 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 wp="p2" until="stage_time>10"/>
|
||||
<go from="p2" hmode="route" wp="p1"/>
|
||||
<deroute block="stay_p1"/>
|
||||
</block>
|
||||
<block name="route">
|
||||
<go from="p1" 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="land here" strip_button="Land Here" strip_icon="land-right.png">
|
||||
<call 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 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>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
Reference in New Issue
Block a user