Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Fix flight plans (#2740)

Browse Source 
* [Flightplans] Replace ">" by "@GT".

* [Flightplan] Replace "&&" by "@AND".

* Add GetPosHeight() macro.

* [FlightPlan] Replace "stateGetPositionEnu_f()->z" by the GetPosHeight() macro.
This commit is contained in:
Fabien-B
2021-06-28 13:20:55 +02:00
committed by GitHub
parent 5cfda00673
commit 87a0526f37
74 changed files with 502 additions and 496 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/AGGIEAIR/rotorcraft_opticlow_test.xml
+1 -1
View File
@@ -37,7 +37,7 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 1.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 1.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml
+2 -2
View File
@@ -76,7 +76,7 @@ static inline void obstacle_detection_cb(uint8_t sender_id __attribute__((unused
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Take off" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
<exception cond="stateGetPositionEnu_f()->z @GT 0.6" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 0.6" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="STDBY"/>
</block>
@@ -101,7 +101,7 @@ static inline void obstacle_detection_cb(uint8_t sender_id __attribute__((unused
<go wp="TD"/>
</block>
<block name="Land Target" strip_button="Land Target" group="target">
<exception cond="stateGetPositionEnu_f()->z @LT 0.3" deroute="Ramp down"/>
<exception cond="GetPosHeight() @LT 0.3" deroute="Ramp down"/>
<exception cond="!nav_is_in_flight()" deroute="Kill Engine"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
conf/flight_plans/ENAC/dw1000_basic.xml
+6 -6
View File
@@ -37,7 +37,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -50,8 +50,8 @@
</block>
<block name="Start Shift Tracking">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="Run Shift Tracking" pre_call="shift_tracking_run(&nav_shift)">
<call_once fun="shift_tracking_reset()"/>
@@ -70,11 +70,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/ENAC/fish_outdoor.xml
+6 -6
View File
@@ -44,16 +44,16 @@
<variable init="0.1" var="fp_throttle"/>
</variables>
<exceptions>
<exception cond="(datalink_time > 5 @AND
!(IndexOfBlock('Takeoff') > nav_block) @AND
<exception cond="(datalink_time @GT 5 @AND
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land"/>
<exception cond="(!InsideNet(GetPosX(),GetPosY()) @AND
!(IndexOfBlock('Takeoff') > nav_block) @AND
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('Landed')) @AND
(autopilot_in_flight() == true) )" deroute="EmergencyLanding"/>
<exception cond="(stateGetPositionEnu_f()->z @GT 50 @AND
!(IndexOfBlock('Takeoff') > nav_block) @AND
<exception cond="(GetPosHeight() @GT 50 @AND
!(IndexOfBlock('Takeoff') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('Landed')) @AND
(autopilot_in_flight() == true) )" deroute="EmergencyLanding"/>
</exceptions>
@@ -94,7 +94,7 @@
<go wp="TD"/>
</block>
<block name="flare">
<exception cond="stateGetPositionEnu_f()->z @LT 0.3" deroute="Ramp down"/>
<exception cond="GetPosHeight() @LT 0.3" deroute="Ramp down"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
conf/flight_plans/ENAC/fish_voliere.xml
+1 -1
View File
@@ -78,7 +78,7 @@
<go wp="TD"/>
</block>
<block name="flare">
<exception cond="stateGetPositionEnu_f()->z @LT 0.3" deroute="Ramp down"/>
<exception cond="GetPosHeight() @LT 0.3" deroute="Ramp down"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
conf/flight_plans/HOOPERFLY/hooperfly_gsa_one.xml
+1 -1
View File
@@ -36,7 +36,7 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block group="init_control" name="Takeoff" strip_button="Takeoff" strip_icon="launch.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
</block>
conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_multiflight.xml
+4 -4
View File
@@ -36,7 +36,7 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block group="init_control" name="Takeoff" strip_button="Takeoff" strip_icon="launch.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
</block>
@@ -87,21 +87,21 @@
<block group="pano_control" name="go_HOV_Pano" strip_button="CW Pano" strip_icon="pano_run.png">
<call_once fun="NavSetWaypointHere(WP_HOV)"/>
<for var="i" from="1" to="12">
<heading alt="WaypointAlt(WP_HOV)" course="30 * $i" until="stage_time > 5"/>
<heading alt="WaypointAlt(WP_HOV)" course="30 * $i" until="stage_time @GT 5"/>
</for>
<deroute block="go_HOV_0"/>
</block>
<block group="spin_control" name="go_HOV_spin_clockwise" strip_button="Spin Clockwise" strip_icon="circle-right.png">
<call_once fun="NavSetWaypointHere(WP_HOV)"/>
<for var="i" from="1" to="3">
<heading alt="WaypointAlt(WP_HOV)" course="120 * $i" until="stage_time > 0"/>
<heading alt="WaypointAlt(WP_HOV)" course="120 * $i" until="stage_time @GT 0"/>
</for>
<deroute block="go_HOV_0"/>
</block>
<block group="spin_control" name="go_HOV_spin_counterclockwise" strip_button="Spin Counterclockwise" strip_icon="circle-left.png">
<call_once fun="NavSetWaypointHere(WP_HOV)"/>
<for var="i" from="1" to="3">
<heading alt="WaypointAlt(WP_HOV)" course="-120 * $i" until="stage_time > 0"/>
<heading alt="WaypointAlt(WP_HOV)" course="-120 * $i" until="stage_time @GT 0"/>
</for>
<deroute block="go_HOV_0"/>
</block>
conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_nocturnal.xml
+4 -4
View File
@@ -36,7 +36,7 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block group="init_control" name="Takeoff" strip_button="Takeoff" strip_icon="launch.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
</block>
@@ -87,21 +87,21 @@
<block group="pano_control" name="go_HOV_Pano" strip_button="CW Pano" strip_icon="pano_run.png">
<call fun="NavSetWaypointHere(WP_HOV)"/>
<for var="i" from="1" to="12">
<heading alt="WaypointAlt(WP_HOV)" course="30 * $i" until="stage_time > 5"/>
<heading alt="WaypointAlt(WP_HOV)" course="30 * $i" until="stage_time @GT 5"/>
</for>
<deroute block="go_HOV_0"/>
</block>
<block group="spin_control" name="go_HOV_spin_clockwise" strip_button="Spin Clockwise" strip_icon="circle-right.png">
<call fun="NavSetWaypointHere(WP_HOV)"/>
<for var="i" from="1" to="3">
<heading alt="WaypointAlt(WP_HOV)" course="120 * $i" until="stage_time > 0"/>
<heading alt="WaypointAlt(WP_HOV)" course="120 * $i" until="stage_time @GT 0"/>
</for>
<deroute block="go_HOV_0"/>
</block>
<block group="spin_control" name="go_HOV_spin_counterclockwise" strip_button="Spin Counterclockwise" strip_icon="circle-left.png">
<call fun="NavSetWaypointHere(WP_HOV)"/>
<for var="i" from="1" to="3">
<heading alt="WaypointAlt(WP_HOV)" course="-120 * $i" until="stage_time > 0"/>
<heading alt="WaypointAlt(WP_HOV)" course="-120 * $i" until="stage_time @GT 0"/>
</for>
<deroute block="go_HOV_0"/>
</block>
conf/flight_plans/OPENUAS/include_obc2014_mission.xml
+11 -11
View File
@@ -151,7 +151,7 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
<!-- If datalink is lost for more than 10 seconds and we are in launched state -->
<!-- below for the real SnR --> <!-- TODO: if datalinkloss but still closeby move to commhold? strange better airfiledhome loiter IMHO -->
<exception cond="And((datalink_time/2) > 9, autopilot.launch > 0) && (!AircraftIsBooting())" deroute="CommsHoldLoiter"/> <!-- deroute="AirfieldHomeLoiter"/> -->
<exception cond="And((datalink_time/2) @GT 9, autopilot.launch @GT 0) @AND (!AircraftIsBooting())" deroute="CommsHoldLoiter"/> <!-- deroute="AirfieldHomeLoiter"/> -->
<!-- <exception cond="if Joefoundandretunvalformticket deroute="Waitforjudges"/>-->
@@ -159,10 +159,10 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
<!-- <exception cond="And(launch, gps_lost)" deroute="killengine" or test for heighht 40M or so/> -->
<!-- see if we can get correct AP voltage -->
<!-- not used yet maybe switch it on <exception cond="10.0 > PowerVoltage()" deroute="Standby"/> -->
<!-- not used yet maybe switch it on <exception cond="11.0 > vsupply" deroute="Standby"/> -->
<!-- not used yet maybe switch it on <exception cond="10.0 @GT PowerVoltage()" deroute="Standby"/> -->
<!-- not used yet maybe switch it on <exception cond="11.0 @GT vsupply" deroute="Standby"/> -->
<!-- is less safe and has less chance of AC recovery. It states battery LOW, not battery critical -->
<!-- <exception cond="electrical.bat_low && !(nav_block == IndexOfBlock('Setting_home_location')) && !(nav_block == IndexOfBlock('Holding_point') && !(nav_block == IndexOfBlock('Land')))" deroute="Land"/>-->
<!-- <exception cond="electrical.bat_low @AND !(nav_block == IndexOfBlock('Setting_home_location')) @AND !(nav_block == IndexOfBlock('Holding_point') @AND !(nav_block == IndexOfBlock('Land')))" deroute="Land"/>-->
<!--<TODO: Add somethin like pre_call="if (!InsideRed(GetPosX(), GetPosY())) NavKillThrottle();-->
</exceptions>
@@ -186,7 +186,7 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
<!-- IDEA Close the hatch so not able to insert payload before there is a 3D fix , however maybe not so handy anyhow since we need to wat be fre we can insert-->
<!-- <set value="NavDropCloseHatch" var="unit"/> -->
<!-- if no valid fix or gps accuracy> 15m or no AHRS , it a no-go wait-->
<while cond="!GpsFixValid() || gps.pacc > 1500 || stateIsAttitudeValid()"/>
<while cond="!GpsFixValid() || gps.pacc @GT 1500 || stateIsAttitudeValid()"/>
</block>
<!-- *********** Set the ground reference height and the home position *********** -->
@@ -276,7 +276,7 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
<block name="Climb-Out">
<!--<exception cond="rc_status==RC_REALLY_LOST" after 3 seconds and no IMU deroute="softlydown">if using RC stil on with a Auto2 mode switch-->
<attitude pitch="nav_takeoff_pitch_setting" roll="0" throttle="1.0" until="GetPosAlt() > ground_alt+40" vmode="throttle"/>
<attitude pitch="nav_takeoff_pitch_setting" roll="0" throttle="1.0" until="GetPosAlt() @GT ground_alt+40" vmode="throttle"/>
<set var="v_ctl_auto_throttle_nominal_cruise_throttle" value="1.0" />
<set var="v_ctl_auto_throttle_nominal_cruise_pitch" value="nav_takeoff_pitch_setting" />
</block>
@@ -508,9 +508,9 @@ extern bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t e
<block name="Land">
<set value="nav_airspeed_nominal_setting" var="v_ctl_auto_airspeed_setpoint"/>
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<set value="nav_airspeed_landing_setting" var="v_ctl_auto_airspeed_setpoint"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 15 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND 15 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
</block>
<block name="Final">
@@ -555,13 +555,13 @@ mode must be activated. A controlled flight termination mode may be
termination mode (Section 5.6).
-->
<block name="CommsHoldLoiter" strip_button="CommsHold" strip_icon="recenter.png" group="Link" > <!-- TODO other ICON -->
<block name="CommsHoldLoiter" strip_button="CommsHold" strip_icon="recenter.png" group="Link" @GT <!-- TODO other ICON -->
<!-- stop streaming thumbnails -->
<!-- CDW set value="0" var="ticket_thumbnails"/ -->
<!-- TODO: maybe set a little lower speed ? max endurence speed? -->
<set value="nav_airspeed_nominal_setting" var="v_ctl_auto_airspeed_setpoint"/>
<!-- Rules 2014 state a loiter of maximum 2 minutes = 120seconds is allowed -->
<exception cond="(stage_time > 120)" deroute="AirfieldHomeLoiter"/>
<exception cond="(stage_time @GT 120)" deroute="AirfieldHomeLoiter"/>
<!-- resume if datalink is regained-->
<!--TODO If the data link is regained prior to the conclusion of 2 minutes of loiter at
Comms Hold the mission maybe continued otherwise any subsequent TODO -->
@@ -574,7 +574,7 @@ termination mode (Section 5.6).
</block>
<block name="AirfieldHomeLoiter" strip_button="AirfieldHome" strip_icon="home.png" group="Link">
<exception cond="stage_time > 120" deroute="ConstantSlopeLandRightAFTD"/>
<exception cond="stage_time @GT 120" deroute="ConstantSlopeLandRightAFTD"/>
<circle wp="AIRFIELDHOME" radius="nav_radius*1.5"/><!-- 1.5 x mean it will be a bit bigger circle -->
</block>
conf/flight_plans/OPENUAS/include_obc2014_safety.xml
+4 -4
View File
@@ -61,7 +61,7 @@
<!-- 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())) || (InsideAirportForbidden(GetPosX(), GetPosY())) || (InsideAirportForbidden2(GetPosX(), GetPosY()))) && gps_has_been_good()" deroute="GeoFence"/>
<exception cond="((!InsideMissionBoundary(GetPosX(), GetPosY())) || (InsideAirportForbidden(GetPosX(), GetPosY())) || (InsideAirportForbidden2(GetPosX(), GetPosY()))) @AND gps_has_been_good()" deroute="GeoFence"/>
<!-- Failure of Autopilot -->
<!-- FTD will failsafe if no commands arrive during 100ms -->
@@ -70,17 +70,17 @@
<!-- FTD will failsafe if no heartbeat arrives from AP (with GPS) during 100ms -->
<!-- Soft Altitude GeoFence 2800ft MSL-->
<!-- RE_ENABLE CDW <exception cond="MoreThan(GetAmsl(), 2800) && gps_has_been_good()" deroute="AlmostHeightViolation" />-->
<!-- RE_ENABLE CDW <exception cond="MoreThan(GetAmsl(), 2800) @AND gps_has_been_good()" deroute="AlmostHeightViolation" />-->
<!-- Hard Altitude GeoFence 3000ft MSL
Removed: because: theoretically impossible to climb 200ft without engine.
- Kill is as safe as Forcecrash, but happens lower.
- Only option to reach hard geofence = sensor failure: then the plane IS NOT TOO high, but the engine will be killed.
<exception cond="MoreThan(GetAmsl(), 3000) && gps_has_been_good()" deroute="HeightViolation" />
<exception cond="MoreThan(GetAmsl(), 3000) @AND gps_has_been_good()" deroute="HeightViolation" />
-->
<!-- Case of loss of GPS AND loss of datalink -->
<exception cond="And(datalink_time > 10, !GpsFixValid())" deroute="DatalinkAndGpsFailure"/>
<exception cond="And(datalink_time @GT 10, !GpsFixValid())" deroute="DatalinkAndGpsFailure"/>
</exceptions>
<blocks>
conf/flight_plans/OPENUAS/openuas_nav_modules_test.xml
+4 -4
View File
@@ -49,7 +49,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="false" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -126,11 +126,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/OPENUAS/openuas_rotorcraft_simple.xml
+14 -14
View File
@@ -33,22 +33,22 @@ Your safe aircraft operation is *your* responsibility
<exceptions>
<!-- Check if battery voltage is low, then try to go to block Land. This is deemed better than to land at that very spot since that can be above something unknown,
is less safe and has less chance of AC recovery. It states battery LOW, not battery critical -->
<!-- <exception cond="electrical.bat_low && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>-->
<exception cond="electrical.bat_critical && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Emergency"/>
<!-- <exception cond="electrical.bat_low @AND !(nav_block == IndexOfBlock('Setting home location')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Land')) @AND !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>-->
<exception cond="electrical.bat_critical @AND !(nav_block == IndexOfBlock('Setting home location')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Land')) @AND !(nav_block == IndexOfBlock('Emergency'))" deroute="Emergency"/>
<!-- No connection for some time in flight, then try to land -->
<exception cond="autopilot_in_flight() && datalink_time > 55 && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>
<exception cond="autopilot_in_flight() @AND datalink_time @GT 55 @AND !(nav_block == IndexOfBlock('Setting home location')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Land')) @AND !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>
<!-- If somehow the AC isVALUEVALUE higher than initial ground position plus 60m, just land -->
<!--tofix only in flight <exception cond="GetPosAlt() > (GetAltRef() + 60)" deroute="Land"/> -->
<!--tofix only in flight <exception cond="GetPosAlt() @GT (GetAltRef() + 60)" deroute="Land"/> -->
<!-- No link for 20 seconds and no GPS valid position fix, but the AC can still hover...however no option to give commands, emergency landing -->
<!-- <exception cond="And(datalink_time > 20, !GpsFixValid()) && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Emergency"/>-->
<!-- <exception cond="And(datalink_time @GT 20, !GpsFixValid()) @AND !(nav_block == IndexOfBlock('Setting home location')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Emergency'))" deroute="Emergency"/>-->
<!-- Check if outside a safety circle from where the drone started the climb, if outside come back to where climb was started, defer from default outside behaviour-->
<exception cond="(((((WaypointX(WP__CLIMB)) - GetPosX())*((WaypointX(WP__CLIMB)) - GetPosX()))) + ((((WaypointY(WP__CLIMB)) - GetPosY())*((WaypointY(WP__CLIMB)) - GetPosY())))) > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME) && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>
<exception cond="(((((WaypointX(WP__CLIMB)) - GetPosX())*((WaypointX(WP__CLIMB)) - GetPosX()))) + ((((WaypointY(WP__CLIMB)) - GetPosY())*((WaypointY(WP__CLIMB)) - GetPosY())))) @GT (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME) @AND !(nav_block == IndexOfBlock('Setting home location')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Land')) @AND !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>
</exceptions>
<blocks>
<block name="Setting home location"><!-- TODO: make sure there is an indicator in "ardrone2_simple.xml" GCS layout config screen displaying "Waiting for GPS position..."-->
<call_once fun="NavKillThrottle()"/>
<!-- if no valid fix or GPS accuracy > 11m or no AHRS , it's' a no-go, just wait TODO: timeout and message?-->
<!-- <while cond="!GpsFixValid() || gps.pacc > 1100 || stateIsAttitudeValid()"/>-->
<!-- if no valid fix or GPS accuracy @GT 11m or no AHRS , it's' a no-go, just wait TODO: timeout and message?-->
<!-- <while cond="!GpsFixValid() || gps.pacc @GT 1100 || stateIsAttitudeValid()"/>-->
<while cond="!GpsFixValid()"/>
<!-- Additional wait to allow for better GPS data in the last secons before takeoff -->
<while cond="LessThan(NavBlockTime(), 8)"/>
@@ -91,16 +91,16 @@ Altough Switching the mode to auto2/NAV from any other mode will reset nav_headi
<set value="0" var="autopilot.flight_time"/>
<!-- If take-off to first point takes to long to reach somehow because of some reason, abort flight -->
<!-- todo test various times -->
<exception cond="block_time > 560" deroute="Land"/>
<exception cond="block_time @GT 560" deroute="Land"/>
<!-- To make sure that takoff is straight up to the height of first waypoint A, so no sideways take-off, since sideways movements starting at low height from takeoff can have nasty effects) -->
<!-- In real life aircraft can be blown sideways with strong wind TODO experiment with results of 2 stages 2 -->
<!-- TODO see if adding this 2nd stage is better -->
<!-- <attitude pitch="0" roll="0" throttle="0.90" until="stage_time > 2" vmode="throttle"/>-->
<!--Alternative <exception cond="WaypointAlt(WP_A) > stateGetPositionEnu_f()->z" deroute="A_to_B_and_back"/>-->
<!-- <attitude pitch="0" roll="0" throttle="0.90" until="stage_time @GT 2" vmode="throttle"/>-->
<!--Alternative <exception cond="WaypointAlt(WP_A) @GT GetPosHeight()" deroute="A_to_B_and_back"/>-->
<!-- See http://lists.gnu.org/archive/html/paparazzi-devel/2014-06/msg00138.html -->
<!--<call_once fun="GuidanceVSetRef(0,0,0)"/>-->
<!--<call_once fun="nav_set_heading_current()"/>-->
<stay vmode="climb" climb="0.7" until="WaypointAlt(WP_A) > stateGetPositionEnu_f()->z" wp="_CLIMB"/>
<stay vmode="climb" climb="0.7" until="WaypointAlt(WP_A) @GT GetPosHeight()" wp="_CLIMB"/>
</block>
<!-- The From A to B and back, The only one visible block so no GCS choices to toy around with, a non expert would get confused -->
<block key="m" name="From A to B and back">
@@ -109,7 +109,7 @@ Altough Switching the mode to auto2/NAV from any other mode will reset nav_headi
<call_once fun="nav_set_heading_towards_waypoint(WP_B)"/>
<go from="A" hmode="route" wp="B"/>
<call_once fun="nav_set_heading_towards_waypoint(WP_A)"/>
<stay wp="B" until="stage_time > 3"/> <!-- Hang around for 3 seconds, any longer and people start get worried if all is OK...-->
<stay wp="B" until="stage_time @GT 3"/> <!-- Hang around for 3 seconds, any longer and people start get worried if all is OK...-->
<go from="B" hmode="route" wp="A"/>
<!-- to make sure the route height to touchdown is high not lower than on the first takeoff-->
<!-- TODO: discuss this behaviour since it will look strange if drone first will climb to climb point and not decend when one presses land, perception difference-->
@@ -129,7 +129,7 @@ Altough Switching the mode to auto2/NAV from any other mode will reset nav_headi
<block name="Emergency" key="e" strip_button="Emergency" strip_icon="home_emergency.png">
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<!-- If landing would takes to long take drastic measures, kill everything -->
<!-- <exception cond="Block_time > 50" value="1" var="autopilot.kill_throttle"/>-->
<!-- <exception cond="Block_time @GT 50" value="1" var="autopilot.kill_throttle"/>-->
<call_once fun="NavSetWaypointHere(WP__TDEMERGENCY)"/>
<!--TODO: better test detect landing different surfaces and agles with sonar baro combi of AR-Drone 2 -->
<call_once fun="NavStartDetectGround()"/>
conf/flight_plans/OPENUAS/openuas_tuning_a_fresh_fixedwing.xml
+4 -4
View File
@@ -154,7 +154,7 @@ perhaps a few global alt exceptions? But where do they deroute to assuming the a
<includes/>
<!-- The exceptions section allows for global error and failsafe handling -->
<exceptions>
<!-- <exception cond="datalink_time > 15" deroute="Standby"/> -->
<!-- <exception cond="datalink_time @GT 15" deroute="Standby"/> -->
</exceptions>
<!-- The blocks section contains each block of the flightplan, which actually make the aircraft do something -->
<blocks>
@@ -173,7 +173,7 @@ perhaps a few global alt exceptions? But where do they deroute to assuming the a
</block>
<!-- need this for simulation, manual takeoff for real testing -->
<block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() > (ground_alt+25)" deroute="Standby"/>
<exception cond="GetPosAlt() @GT (ground_alt+25)" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="1" var="autopilot.launch"/>
<set value="0" var="autopilot.flight_time"/>
@@ -215,7 +215,7 @@ perhaps a few global alt exceptions? But where do they deroute to assuming the a
</while>
</block>
<block name="Descent Auto Throttle Tuning">
<while cond="GetPosAlt() > (ground_alt + 25)">
<while cond="GetPosAlt() @GT (ground_alt + 25)">
<attitude roll="0.0" vmode="climb" climb="-V_CTL_ALTITUDE_MAX_CLIMB"/>
</while>
</block>
@@ -244,7 +244,7 @@ perhaps a few global alt exceptions? But where do they deroute to assuming the a
</while>
</block>
<block name="Descent Auto Pitch Tuning">
<while cond="GetPosAlt() > (ground_alt + 25)">
<while cond="GetPosAlt() @GT (ground_alt + 25)">
<attitude roll="0.0" pitch="auto" throttle="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" vmode="climb" climb="-V_CTL_ALTITUDE_MAX_CLIMB"/>
</while>
</block>
conf/flight_plans/OPENUAS/openuas_versatile_unified.xml
+30 -30
View File
@@ -119,16 +119,16 @@ Should be unified for Hybrid, FW and rotorcraft
<variable var="roll_step" init="15." min="0." max="50." step="1.0"/>
</variables>
<exceptions>
<exception cond="And((datalink_time) > 300, autopilot.launch > 0) " deroute="Standby"/>
<exception cond="LOW_BAT_LEVEL > PowerVoltage()" deroute="Standby"/>
<exception cond="Or(! InsideMissionBoundary(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 600) && !(nav_block == IndexOfBlock('Wait GPS')) && !(nav_block == IndexOfBlock('Geo init')) && !(nav_block == IndexOfBlock('Waiting for RC Transmitter')) && !(nav_block == IndexOfBlock('Takeoff')) && !(nav_block == IndexOfBlock('land')) && !(nav_block == IndexOfBlock('Final')) && !(nav_block == IndexOfBlock('Flare'))" deroute="Standby"/>
<exception cond="And((datalink_time) @GT 300, autopilot.launch @GT 0) " deroute="Standby"/>
<exception cond="LOW_BAT_LEVEL @GT PowerVoltage()" deroute="Standby"/>
<exception cond="Or(! InsideMissionBoundary(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 600) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Waiting for RC Transmitter')) @AND !(nav_block == IndexOfBlock('Takeoff')) @AND !(nav_block == IndexOfBlock('land')) @AND !(nav_block == IndexOfBlock('Final')) @AND !(nav_block == IndexOfBlock('Flare'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS" pre_call="NavKillThrottle()">
<!-- Do Not Kill -->
<!-- <call fun="Fly()"/>-->
<!-- if no valid fix or gps accuracy> 15m or no AHRS , it a no-go wait-->
<!--<while cond="!GpsFixValid() || gps.pacc > 1500 || stateIsAttitudeValid()"/>-->
<!--<while cond="!GpsFixValid() || gps.pacc @GT 1500 || stateIsAttitudeValid()"/>-->
<while cond="!GpsFixValid()"/>
<!-- Wiggle Wiggle when we have GPS , time to fly...-->
<!--<set var="ap_state->commands[COMMAND_YAW]" value="-MAX_PPRZ"/>
@@ -177,7 +177,7 @@ Should be unified for Hybrid, FW and rotorcraft
<!-- <set var="ap_state->commands[COMMAND_LIGHT]" value="PPRZ_MAX"/> -->
<set var="autopilot.kill_throttle" value="0"/>
<!--<var="autopilot.launch" set value="1" />-->
<while cond="1 > autopilot.launch"/> <!-- Setting of launch performed with takeoff_detect module disable -->
<while cond="1 @GT autopilot.launch"/> <!-- Setting of launch performed with takeoff_detect module disable -->
<set var="autopilot.flight_time" value="0"/>
<!--<call_once fun="AirbrakesOff()"/>-->
<set var="v_ctl_auto_airspeed_setpoint" value="nav_airspeed_nominal_setting" /><!--or RACE_AIRSPEED? -->
@@ -187,7 +187,7 @@ Should be unified for Hybrid, FW and rotorcraft
<block name="Climb-Out">
<set var="v_ctl_speed_mode" value="V_CTL_SPEED_AIRSPEED"/>
<!--<exception cond="rc_status==RC_REALLY_LOST" after 3 seconds and no IMU deroute="softlydown">if using RC still on with a Auto2 mode switch-->
<attitude pitch="nav_takeoff_pitch_setting" roll="0" throttle="1.0" until="GetPosAlt() > ground_alt+15" vmode="throttle"/>
<attitude pitch="nav_takeoff_pitch_setting" roll="0" throttle="1.0" until="GetPosAlt() @GT ground_alt+15" vmode="throttle"/>
<set var="v_ctl_auto_throttle_nominal_cruise_throttle" value="1.0" />
<!-- Only for ETECS <set var="v_ctl_auto_throttle_nominal_cruise_pitch" value="nav_takeoff_pitch_setting" />-->
@@ -214,7 +214,7 @@ Should be unified for Hybrid, FW and rotorcraft
<deroute block="Standby"/>
</block>
<block name="SearchForTarget" strip_button="Searchtarget" strip_icon="survey.png" group="Scan">
<exception cond="block_time > 300" deroute="Standby"/> <!-- No patience, five minutes is enough for now... -->
<exception cond="block_time @GT 300" deroute="Standby"/> <!-- No patience, five minutes is enough for now... -->
<call_once fun="nav_survey_polygon_setup(WP_SA1, 5, 45 , POLYSURVEY_DEFAULT_DISTANCE, 25, MIN_CIRCLE_RADIUS, GetAltRef()+40)"/>
<call fun="nav_survey_polygon_run()"/>
</block>
@@ -232,7 +232,7 @@ Should be unified for Hybrid, FW and rotorcraft
<go wp="1"/>
<set var="v_ctl_auto_throttle_cruise_throttle" value="V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE"/>
<set var="v_ctl_auto_airspeed_setpoint" value="RACE_AIRSPEED"/>
<oval p1="1" p2="2" radius="nav_radius*1.2" until="block_time > 180"/>
<oval p1="1" p2="2" radius="nav_radius*1.2" until="block_time @GT 180"/>
<set var="v_ctl_auto_throttle_cruise_throttle" value="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
<set var="v_ctl_auto_airspeed_setpoint" value="NOMINAL_AIRSPEED"/>
<deroute block="Standby"/>
@@ -245,23 +245,23 @@ Should be unified for Hybrid, FW and rotorcraft
<circle pitch="auto" radius="75" throttle="0.7" wp="1"/>
</block>
<block name="Climb 75% throttle">
<circle pitch="10" radius="50+(GetPosAlt()-GetAltRef())/2" throttle="0.75" until="(LOW_BAT_LEVEL > PowerVoltage()) || (GetPosAlt() > GetAltRef()+ 600)" vmode="throttle" wp="1"/>
<circle pitch="10" radius="50+(GetPosAlt()-GetAltRef())/2" throttle="0.75" until="(LOW_BAT_LEVEL @GT PowerVoltage()) || (GetPosAlt() @GT GetAltRef()+ 600)" vmode="throttle" wp="1"/>
</block>
<block name="Climb 0m/s">
<circle climb="0" radius="nav_radius" until="LOW_BAT_LEVEL > PowerVoltage()" vmode="climb" wp="1"/>
<circle climb="0" radius="nav_radius" until="LOW_BAT_LEVEL @GT PowerVoltage()" vmode="climb" wp="1"/>
</block>
<block name="Climb 1m/s">
<circle climb="1" pitch="5" radius="50+(GetPosAlt()-GetAltRef())/2" until="LOW_BAT_LEVEL > PowerVoltage()" vmode="climb" wp="1"/>
<circle climb="1" pitch="5" radius="50+(GetPosAlt()-GetAltRef())/2" until="LOW_BAT_LEVEL @GT PowerVoltage()" vmode="climb" wp="1"/>
</block>
<block name="Climb nav_climb m/s">
<circle climb="nav_climb" radius="nav_radius" until="(LOW_BAT_LEVEL > PowerVoltage()) || (GetPosAlt() > GetAltRef()+ 600)" vmode="climb" wp="1"/>
<circle climb="nav_climb" radius="nav_radius" until="(LOW_BAT_LEVEL @GT PowerVoltage()) || (GetPosAlt() @GT GetAltRef()+ 600)" vmode="climb" wp="1"/>
</block>
<block name="Circle 0% throttle">
<circle pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="GetAltRef()+50 > GetPosAlt()" vmode="throttle" wp="1"/>
<circle pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="GetAltRef()+50 @GT GetPosAlt()" vmode="throttle" wp="1"/>
<deroute block="Standby"/>
</block>
<block name="Oval 0% throttle">
<oval p1="1" p2="2" pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="GetAltRef()+50 > GetPosAlt()" vmode="throttle"/>
<oval p1="1" p2="2" pitch="fp_pitch" radius="nav_radius" throttle="0.0" until="GetAltRef()+50 @GT GetPosAlt()" vmode="throttle"/>
<deroute block="Standby"/>
</block>
<block name="Route 1-2">
@@ -294,10 +294,10 @@ Should be unified for Hybrid, FW and rotorcraft
<!-- ALT
<block key="<control>l" name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="BASELEG"/>
<set var="v_ctl_auto_throttle_cruise_throttle" value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" />
<set var="v_ctl_auto_airspeed_setpoint" value="MINIMUM_AIRSPEED"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
<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>-->
<!-- *********** Land *********** -->
@@ -322,15 +322,15 @@ Should be unified for Hybrid, FW and rotorcraft
<block name="land">
<set value="nav_airspeed_nominal_setting" var="v_ctl_auto_airspeed_setpoint"/>
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<set value="nav_airspeed_landing_setting" var="v_ctl_auto_airspeed_setpoint"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 15 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND 15 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
</block>
<block name="Final">
<!--<call_once fun="AirbrakesOn()"/>-->
<go approaching_time="6" from="AF" hmode="route" wp="TD"/>
<!--TODO: until="1.0 > agl_dist_value_filtered"/>-->
<!--TODO: until="1.0 @GT agl_dist_value_filtered"/>-->
</block>
<block name="Flare">
@@ -344,26 +344,26 @@ Should be unified for Hybrid, FW and rotorcraft
<!-- Extras -->
<block name="Steps roll -10, +10">
<while cond="TRUE">
<attitude alt="GetPosAlt()" roll="10.0" until=" stage_time > 6" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="-10.0" until="stage_time > 6" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="10.0" until=" stage_time @GT 6" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="-10.0" until="stage_time @GT 6" vmode="alt"/>
</while>
</block>
<block name="Steps roll -20, +20">
<while cond="TRUE">
<attitude alt="GetPosAlt()" roll="20.0" until=" stage_time > 3" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="-20.0" until="stage_time > 3" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="20.0" until=" stage_time @GT 3" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="-20.0" until="stage_time @GT 3" vmode="alt"/>
</while>
</block>
<block name="Steps roll from var">
<while cond="TRUE">
<attitude alt="GetPosAlt()" roll="roll_step" until=" stage_time > 3" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="-roll_step" until="stage_time > 3" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="roll_step" until=" stage_time @GT 3" vmode="alt"/>
<attitude alt="GetPosAlt()" roll="-roll_step" until="stage_time @GT 3" vmode="alt"/>
</while>
</block>
<block name="Steps pitch -10, +10">
<while cond="TRUE">
<attitude alt="GetPosAlt()" pitch="10" roll="0.0" until=" stage_time > 2" vmode="alt"/>
<attitude alt="GetPosAlt()" pitch="-10" roll="0.0" until=" stage_time > 2" vmode="alt"/>
<attitude alt="GetPosAlt()" pitch="10" roll="0.0" until=" stage_time @GT 2" vmode="alt"/>
<attitude alt="GetPosAlt()" pitch="-10" roll="0.0" until=" stage_time @GT 2" vmode="alt"/>
</while>
</block>
<block name="Heading 30">
@@ -371,7 +371,7 @@ Should be unified for Hybrid, FW and rotorcraft
</block>
<block name="For loop (circles wp 1)">
<for from="0" to="3" var="i">
<circle radius="DEFAULT_CIRCLE_RADIUS+ $i*10" wp="1" until="NavCircleCount() > 1"/>
<circle radius="DEFAULT_CIRCLE_RADIUS+ $i*10" wp="1" until="NavCircleCount() @GT 1"/>
</for>
<deroute block="Standby"/>
</block>
@@ -380,7 +380,7 @@ Should be unified for Hybrid, FW and rotorcraft
<call fun="nav_flower_run()"/>
</block>
<block name="Test datalink (go to wp 2)">
<exception cond="datalink_time > 22" deroute="Standby"/>
<exception cond="datalink_time @GT 22" deroute="Standby"/>
<go from="STDBY" hmode="route" wp="2"/>
<go from="2" hmode="route" wp="STDBY"/>
</block>
@@ -403,7 +403,7 @@ Should be unified for Hybrid, FW and rotorcraft
<block name="Terrain hugging">
<oval p1="1" p2="2" radius="nav_radius"/>
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
<!--TODO: until="7.0 > agl_dist_value_filtered"/>-->
<!--TODO: until="7.0 @GT agl_dist_value_filtered"/>-->
</block>
</blocks>
</flight_plan>
conf/flight_plans/basic.xml
+4 -4
View File
@@ -18,7 +18,7 @@
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
</waypoints>
<exceptions>
<!--exception cond="datalink_time > 22" deroute="Standby"/-->
<!--exception cond="datalink_time @GT 22" deroute="Standby"/-->
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -35,7 +35,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
@@ -72,11 +72,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND (fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)) @LT 10)" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/basic_sim.xml
+11 -11
View File
@@ -19,7 +19,7 @@
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
</waypoints>
<exceptions>
<!--exception cond="datalink_time > 22" deroute="Standby"/-->
<!--exception cond="datalink_time @GT 22" deroute="Standby"/-->
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -33,10 +33,10 @@
<block name="Holding point">
<set value="1" var="autopilot.kill_throttle"/>
<attitude roll="0" throttle="0" vmode="throttle"/>
<exception cond="NavBlockTime() > 10" deroute="Takeoff"/>
<exception cond="NavBlockTime() @GT 10" deroute="Takeoff"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<set value="1" var="launch"/>
@@ -46,19 +46,19 @@
</block>
<block key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<circle radius="nav_radius" wp="STDBY"/>
<exception cond="NavBlockTime() > 30" deroute="Figure 8 around wp 1"/>
<exception cond="NavBlockTime() @GT 30" deroute="Figure 8 around wp 1"/>
</block>
<block key="F8" name="Figure 8 around wp 1" strip_button="Figure 8 (wp 1-2)" strip_icon="eight.png" group="base_pattern">
<eight center="1" radius="nav_radius" turn_around="2"/>
<exception cond="NavBlockTime() > 120" deroute="Oval 1-2"/>
<exception cond="NavBlockTime() @GT 120" deroute="Oval 1-2"/>
</block>
<block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png" group="base_pattern">
<oval p1="1" p2="2" radius="nav_radius"/>
<exception cond="NavBlockTime() > 90" deroute="Survey S1-S2"/>
<exception cond="NavBlockTime() @GT 90" deroute="Survey S1-S2"/>
</block>
<block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png" group="extra_pattern">
<survey_rectangle grid="150" wp1="S1" wp2="S2"/>
<exception cond="NavBlockTime() > 180" deroute="Path 1,2,S1,S2,STDBY"/>
<exception cond="NavBlockTime() @GT 180" deroute="Path 1,2,S1,S2,STDBY"/>
</block>
<block name="Path 1,2,S1,S2,STDBY" strip_button="Path (1,2,S1,S2,STDBY)" strip_icon="path.png" group="extra_pattern">
<path wpts="1,2 S1"/>
@@ -69,7 +69,7 @@
<call_once fun="NavSetWaypointHere(WP_MOB)"/>
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<circle radius="nav_radius" wp="MOB"/>
<exception cond="NavBlockTime() > 90" deroute="Land Right AF-TD"/>
<exception cond="NavBlockTime() @GT 90" deroute="Land Right AF-TD"/>
</block>
<block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png" group="land">
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
@@ -77,11 +77,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/competitions/EMAV2006.xml
+1 -1
View File
@@ -17,7 +17,7 @@
<while COND="!GPS_FIX_VALID(gps_mode)"/>
</block>
<block NAME="init">
<while COND="10 > block_time"/>
<while COND="10 @GT block_time"/>
<set VALUE="reset_nav_reference()" VAR="unit"/>
<set VALUE="reset_waypoints()" VAR="unit"/>
<deroute BLOCK="circlehome"/>
conf/flight_plans/competitions/EMAV2008.xml
+10 -10
View File
@@ -35,7 +35,7 @@
</waypoints>
<sectors/>
<exceptions>
<exception cond="datalink_time > 60" deroute="Standby"/>
<exception cond="datalink_time @GT 60" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -47,7 +47,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+40" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+40" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<go wp="CLIMB"/>
</block>
@@ -66,7 +66,7 @@
<circle radius="100" wp="MOB"/>
</block>
<block name="Line 1-2" strip_button="Line (wp 1-2)" strip_icon="line.png">
<exception cond="datalink_time > 22" deroute="Standby"/>
<exception cond="datalink_time @GT 22" deroute="Standby"/>
<call fun="nav_line_setup()"/>
<call fun="nav_line_run(WP_1, WP_2, nav_radius)"/>
</block>
@@ -83,14 +83,14 @@
</block>
<block name="land">
<call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 15 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND 15 @GT fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
</block>
<block name="final">
<!-- <exception cond="GetAltRef() + 6 > GetPosAlt()" deroute="flare"/> -->
<!-- <exception cond="GetAltRef() + 6 @GT GetPosAlt()" deroute="flare"/> -->
<go approaching_time="0" from="AF" hmode="route" wp="SF"/>
</block>
<block name="shortfinal">
<!-- <exception cond="GetAltRef() + 6 > GetPosAlt()" deroute="flare"/> -->
<!-- <exception cond="GetAltRef() + 6 @GT GetPosAlt()" deroute="flare"/> -->
<go approaching_time="0" from="SF" hmode="route" wp="TD"/>
</block>
<block name="flare">
@@ -99,7 +99,7 @@
<!-- <attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/> -->
</block>
<block name="Test datalink (go to wp 2)">
<exception cond="datalink_time > 22" deroute="Standby"/>
<exception cond="datalink_time @GT 22" deroute="Standby"/>
<go from="STDBY" hmode="route" wp="2"/>
<go from="2" hmode="route" wp="STDBY"/>
</block>
@@ -112,10 +112,10 @@
<deroute block="Standby"/>
</block>
<block name="Circle 1">
<exception cond="datalink_time > 25" deroute="Standby"/>
<circle alt="WaypointAlt(WP_1)" radius="nav_radius" until="15 > fabs(GetPosAlt() - WaypointAlt(WP_1))" wp="STDBY"/>
<exception cond="datalink_time @GT 25" deroute="Standby"/>
<circle alt="WaypointAlt(WP_1)" radius="nav_radius" until="15 @GT fabs(GetPosAlt() - WaypointAlt(WP_1))" wp="STDBY"/>
<go alt="WaypointAlt(WP_1)" approaching_time="-15" from="STDBY" hmode="route" wp="1"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.7" wp="1"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.7" wp="1"/>
<go alt="WaypointAlt(WP_1)" approaching_time="-15" from="1" hmode="route" wp="STDBY"/>
<deroute block="Standby"/>
</block>
conf/flight_plans/competitions/EMAV2009.xml
+1 -1
View File
@@ -26,7 +26,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot_flight_time"/>
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
conf/flight_plans/competitions/EMAV2009_safety.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<procedure>
<exceptions>
<exception cond="Or(! InsideGreen(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 150)" deroute="Center"/>
<exception cond="Or(! InsideGreen(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 150)" deroute="Center"/>
</exceptions>
<blocks>
conf/flight_plans/competitions/IMAV2014_carto.xml
+6 -6
View File
@@ -42,7 +42,7 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<include name="Data" procedure="IMAV2014_data.xml"/>
</includes>
<exceptions>
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 150)" deroute="Center"/>
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 150)" deroute="Center"/>
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -61,7 +61,7 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<set value="100." var="WaypointAlt(WP_STDBY)"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Carto1"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Carto1"/>
<call fun="{ hackhd_command(HACKHD_POWER_ON); FALSE; }"/>
<while cond="LessThan(NavBlockTime(), 4)"/>
<call fun="{ hackhd_command(HACKHD_SHOOT); FALSE; }"/>
@@ -75,7 +75,7 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<block name="Carto1" strip_button="Carto" strip_icon="survey.png">
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<call fun="nav_compute_baseleg(WP_N3, WP_S3, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 15 > fabs(GetPosAlt() - WaypointAlt(WP_STDBY)))" wp="_BASELEG" alt="WaypointAlt(WP_STDBY)"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 15 @GT fabs(GetPosAlt() - WaypointAlt(WP_STDBY)))" wp="_BASELEG" alt="WaypointAlt(WP_STDBY)"/>
<attitude alt="WaypointAlt(WP_STDBY)" roll="0.0" until="stage_time>=2"/>
<set value="MODULES_START" var="hackhd_hackhd_autoshoot_status"/>
<go approaching_time="0" from="N3" hmode="route" wp="S3" alt="WaypointAlt(WP_STDBY)"/>
@@ -119,11 +119,11 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
</block>
<block name="land">
<call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 3 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 3 @GT GetPosAlt()" deroute="flare"/>
<call fun="{ hackhd_command(HACKHD_POWER_OFF); FALSE; }"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
conf/flight_plans/competitions/IMAV2014_digit.xml
+8 -8
View File
@@ -25,8 +25,8 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<include name="Data" procedure="IMAV2014_data.xml"/>
</includes>
<exceptions>
<exception cond="(autopilot_flight_time > (60*8)) && (IndexOfBlock('Back Home') >= nav_block) && !(nav_block == IndexOfBlock('Holding point'))" deroute="Back Home"/>
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 150)" deroute="Center"/>
<exception cond="(autopilot_flight_time @GT (60*8)) @AND (IndexOfBlock('Back Home') >= nav_block) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Back Home"/>
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 150)" deroute="Center"/>
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -45,10 +45,10 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<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"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<call fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>3 && autopilot_motors_on" vmode="throttle"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>3 @AND autopilot_motors_on" vmode="throttle"/>
<stay vmode="climb" climb="0.5" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
@@ -82,9 +82,9 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
</block>
<block name="Roof" strip_button="Roof" group="roof">
<go from="STDBY" hmode="route" wp="ROOF"/>
<stay climb="-0.8" vmode="climb" wp="ROOF" until="0.2 > agl_dist_value_filtered"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time > 15"/>
<stay climb="0.8" vmode="climb" wp="ROOF" until="agl_dist_value_filtered > 1.0"/>
<stay climb="-0.8" vmode="climb" wp="ROOF" until="0.2 @GT agl_dist_value_filtered"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time @GT 15"/>
<stay climb="0.8" vmode="climb" wp="ROOF" until="agl_dist_value_filtered @GT 1.0"/>
</block>
<block name="Back from Roof" strip_button="BackRoof" group="roof">
<set var="nav_flight_altitude" value="WPAlt(WP_TD)"/>
@@ -105,7 +105,7 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
</block>
<block name="flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="0.1 > agl_dist_value_filtered" deroute="Holding point"/>
<exception cond="0.1 @GT agl_dist_value_filtered" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call fun="NavStartDetectGround()"/>
<stay climb="-0.8" vmode="climb" wp="TD"/>
conf/flight_plans/competitions/IMAV2014_vision.xml
+20 -20
View File
@@ -29,8 +29,8 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<include name="Data" procedure="IMAV2014_data.xml"/>
</includes>
<exceptions>
<exception cond="(autopilot_flight_time > (60*8)) && (IndexOfBlock('Back Home') >= nav_block) && !(nav_block == IndexOfBlock('Holding point'))" deroute="Back Home"/>
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 150)" deroute="Center"/>
<exception cond="(autopilot_flight_time @GT (60*8)) @AND (IndexOfBlock('Back Home') >= nav_block) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Back Home"/>
<exception cond="Or(InsideRed(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 150)" deroute="Center"/>
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -49,10 +49,10 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<call fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>3 && autopilot_motors_on" vmode="throttle"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>3 @AND autopilot_motors_on" vmode="throttle"/>
<stay climb="0.5" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
@@ -64,10 +64,10 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<call fun="nav_set_heading_towards_waypoint(WP_H1)"/>
<go from="HERE" hmode="route" wp="H1"/>
<call fun="nav_set_heading_deg(310)"/>
<stay wp="H1" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
<stay wp="H1" climb="-1.0" vmode="climb" until="2.0 @GT agl_dist_value_filtered"/>
<call fun="viewvideo_save_shot()"/>
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
<stay wp="H1" until="stage_time > 2"/-->
<stay wp="H1" until="stage_time @GT 2"/-->
<deroute block="House 2"/>
</block>
<block name="House 2" strip_button="H2" group="house1">
@@ -76,10 +76,10 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<call fun="nav_set_heading_towards_waypoint(WP_H2)"/>
<go from="HERE" hmode="route" wp="H2"/>
<call fun="nav_set_heading_deg(90)"/>
<stay wp="H2" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
<stay wp="H2" climb="-1.0" vmode="climb" until="2.0 @GT agl_dist_value_filtered"/>
<call fun="viewvideo_save_shot()"/>
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
<stay wp="H2" until="stage_time > 2"/-->
<stay wp="H2" until="stage_time @GT 2"/-->
<deroute block="House 3"/>
</block>
<block name="House 3" strip_button="H3" group="house1">
@@ -88,10 +88,10 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<call fun="nav_set_heading_towards_waypoint(WP_H3)"/>
<go from="HERE" hmode="route" wp="H3"/>
<call fun="nav_set_heading_deg(130)"/>
<stay wp="H3" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
<stay wp="H3" climb="-1.0" vmode="climb" until="2.0 @GT agl_dist_value_filtered"/>
<call fun="viewvideo_save_shot()"/>
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
<stay wp="H3" until="stage_time > 2"/-->
<stay wp="H3" until="stage_time @GT 2"/-->
<deroute block="House 4"/>
</block>
<block name="House 4" strip_button="H4" group="house2">
@@ -100,10 +100,10 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<call fun="nav_set_heading_towards_waypoint(WP_H4)"/>
<go from="HERE" hmode="route" wp="H4"/>
<call fun="nav_set_heading_deg(80)"/>
<stay wp="H4" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
<stay wp="H4" climb="-1.0" vmode="climb" until="2.0 @GT agl_dist_value_filtered"/>
<call fun="viewvideo_save_shot()"/>
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
<stay wp="H4" until="stage_time > 20"/-->
<stay wp="H4" until="stage_time @GT 20"/-->
<deroute block="Back Home"/>
</block>
<block name="House 5" strip_button="H5" group="house2">
@@ -112,10 +112,10 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<call fun="nav_set_heading_towards_waypoint(WP_H5)"/>
<go from="HERE" hmode="route" wp="H5"/>
<call fun="nav_set_heading_deg(140)"/>
<stay wp="H5" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
<stay wp="H5" climb="-1.0" vmode="climb" until="2.0 @GT agl_dist_value_filtered"/>
<call fun="viewvideo_save_shot()"/>
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
<stay wp="H5" until="stage_time > 20"/-->
<stay wp="H5" until="stage_time @GT 20"/-->
<deroute block="House 6"/>
</block>
<block name="House 6" strip_button="H6" group="house2">
@@ -124,17 +124,17 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
<call fun="nav_set_heading_towards_waypoint(WP_H6)"/>
<go from="HERE" hmode="route" wp="H6"/>
<call fun="nav_set_heading_deg(180)"/>
<stay wp="H6" climb="-1.0" vmode="climb" until="2.0 > agl_dist_value_filtered"/>
<stay wp="H6" climb="-1.0" vmode="climb" until="2.0 @GT agl_dist_value_filtered"/>
<call fun="viewvideo_save_shot()"/>
<!--set var="nav_flight_altitude" value="stateGetPositionEnu_i()->z"/>
<stay wp="H5" until="stage_time > 20"/-->
<stay wp="H5" until="stage_time @GT 20"/-->
<deroute block="Back Home"/>
</block>
<block group="roof" name="Roof" strip_button="Roof">
<go from="STDBY" hmode="route" wp="ROOF"/>
<stay climb="-0.8" until="0.2 > agl_dist_value_filtered" vmode="climb" wp="ROOF"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time > 15" vmode="throttle"/>
<stay climb="0.8" until="agl_dist_value_filtered > 1.0" vmode="climb" wp="ROOF"/>
<stay climb="-0.8" until="0.2 @GT agl_dist_value_filtered" vmode="climb" wp="ROOF"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time @GT 15" vmode="throttle"/>
<stay climb="0.8" until="agl_dist_value_filtered @GT 1.0" vmode="climb" wp="ROOF"/>
</block>
<block name="Back Home" strip_button="Back Home" group="roof">
<set var="nav_flight_altitude" value="WPAlt(WP_TD)"/>
@@ -148,7 +148,7 @@ static inline float wp_dist(float wp1_x, float wp1_y, float wp2_x, float wp2_y)
</block>
<block name="flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="0.1 > agl_dist_value_filtered" deroute="Holding point"/>
<exception cond="0.1 @GT agl_dist_value_filtered" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call fun="NavStartDetectGround()"/>
<stay climb="-0.8" vmode="climb" wp="TD"/>
conf/flight_plans/competitions/IMAV2018_carto.xml
+13 -13
View File
@@ -97,21 +97,21 @@ static inline void set_expo(float e) {
</modules>
<exceptions>
<!-- RC-Loss -->
<exception cond="(delay_test_rc(RCLost(),20) &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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"/>
<!-- Datalink loss -->
<exception cond="(datalink_time > 10 &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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"/>
<!-- Geofence -->
<exception cond="(delay_test_gf(!InsideFlyZone(GetPosX(), GetPosY()),10) &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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>
@@ -128,7 +128,7 @@ static inline void set_expo(float e) {
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" pitch="30" throttle="0.8" vmode="throttle" wp="CLIMB"/>
@@ -151,11 +151,11 @@ static inline void set_expo(float e) {
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 3 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 3 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/competitions/IMAV2019_carto.xml
+13 -13
View File
@@ -95,17 +95,17 @@ static inline void set_expo(float e) {
<module name="nav" type="survey_zamboni"/>
</modules>
<!--exceptions>
<exception cond="(delay_test_rc(RCLost(),20) &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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 > 10 &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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) &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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>
@@ -121,7 +121,7 @@ static inline void set_expo(float e) {
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+20" deroute="Align Zamboni"/>
<exception cond="GetPosAlt() @GT GetAltRef()+20" deroute="Align Zamboni"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" pitch="30" throttle="0.8" vmode="throttle" wp="CLIMB"/>
@@ -149,11 +149,11 @@ static inline void set_expo(float e) {
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 3 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 3 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/competitions/IMAV2019_drop.xml
+23 -23
View File
@@ -84,17 +84,17 @@ static inline bool delay_test_gf(bool test, int delay) {
<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')) &&
<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 > 10 &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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) &&
!(IndexOfBlock('Takeoff') > nav_block) &&
!(nav_block >= IndexOfBlock('Land Right AF-TD')) &&
<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>
@@ -114,7 +114,7 @@ static inline bool delay_test_gf(bool test, int delay) {
<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"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
</block>
@@ -127,8 +127,8 @@ static inline bool delay_test_gf(bool test, int delay) {
<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"/>
<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)"/>
@@ -143,11 +143,11 @@ static inline bool delay_test_gf(bool test, int delay) {
<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"/>
<stay wp="RED" until="stage_time @GT 3"/>
<go wp="BLUE" from="RED" hmode="route"/>
<stay wp="BLUE" until="stage_time > 3"/>
<stay wp="BLUE" until="stage_time @GT 3"/>
<go wp="YELLOW" from="BLUE"/>
<stay wp="YELLOW" until="stage_time > 3"/>
<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"/>
@@ -157,8 +157,8 @@ static inline bool delay_test_gf(bool test, int delay) {
<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"/>
<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))"/>
@@ -189,8 +189,8 @@ static inline bool delay_test_gf(bool test, int delay) {
<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"/>
<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))"/>
@@ -221,8 +221,8 @@ static inline bool delay_test_gf(bool test, int delay) {
<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"/>
<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))"/>
@@ -253,8 +253,8 @@ static inline bool delay_test_gf(bool test, int delay) {
<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"/>
<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))"/>
conf/flight_plans/competitions/mav06.xml
+7 -7
View File
@@ -26,13 +26,13 @@
<!-- <block name="land">
<set value="compute_baseleg()" var="unit"/>
<circle radius="BOMB_RADIUS" until="circle_count > 0.5" wp="BASELEG"/>
<circle radius="BOMB_RADIUS" until="circle_count @GT 0.5" wp="BASELEG"/>
<set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
<circle radius="BOMB_RADIUS" until="Qdr(DegOfRad(bomb_start_qdr)-10) && baseleg_alt_tolerance > fabs(GetPosAlt() - baseleg_alt)" wp="BASELEG"/>
<circle radius="BOMB_RADIUS" until="Qdr(DegOfRad(bomb_start_qdr)-10) @AND baseleg_alt_tolerance @GT fabs(GetPosAlt() - baseleg_alt)" wp="BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go alt="baseleg_alt" approaching_time="16" from="AF" hmode="route" wp="TD"/>
<go approaching_time="2" from="AF" hmode="route" throttle="0.1" vmode="throttle" wp="TD"/>
</block>
@@ -43,12 +43,12 @@
</block>
<block name="circle 1">
<exception cond="block_time > 300" deroute="wait"/>
<exception cond="block_time @GT 300" deroute="wait"/>
<circle wp="C1" radius="80"/>
</block>
<block name="climb">
<exception cond=" GetPosAlt() > GetAltRef() +30" deroute="wait"/>
<exception cond=" GetPosAlt() @GT GetAltRef() +30" deroute="wait"/>
<attitude pitch="15" roll="0" throttle="0.8" until="10> stage_time" vmode="throttle"/>
</block>-->
<block name="wait" strip_button="Wait">
@@ -61,7 +61,7 @@
<block name="target1" strip_button="T1">
<exception cond="block_time > 300" deroute="wait"/>
<exception cond="block_time @GT 300" deroute="wait"/>
<eight center="TARGET1" radius="80" turn_around="C1"/>
</block>
@@ -87,7 +87,7 @@
<block name="roll" strip_button="Roll">
<set value="TRUE" var="h_ctl_disabled"/>
<set value="(-0.75*MAX_PPRZ)" var="h_ctl_aileron_setpoint"/>
<while cond="3 > stage_time && (stateGetNedToBodyEulers_f()->phi > 0 || -M_PI_2 > stateGetNedToBodyEulers_f()->phi)"/>
<while cond="3 @GT stage_time @AND (stateGetNedToBodyEulers_f()->phi @GT 0 || -M_PI_2 @GT stateGetNedToBodyEulers_f()->phi)"/>
<set value="FALSE" var="h_ctl_disabled"/>
<return/>
</block>-->
conf/flight_plans/competitions/mav07.xml
+5 -5
View File
@@ -57,7 +57,7 @@
</sectors>
<exceptions>
<exception cond="!InsideFonsorbes(GetPosX(), GetPosY())" deroute="kill"/>
<exception cond="datalink_time > 35" deroute="standby"/>
<exception cond="datalink_time @GT 35" deroute="standby"/>
</exceptions>
<blocks>
<block name="init">
@@ -72,7 +72,7 @@
</while>
</block>
<block name="takeoff">
<exception cond="GetPosAlt() > GetAltRef() + 25" deroute="standby"/>
<exception cond="GetPosAlt() @GT GetAltRef() + 25" deroute="standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<go wp="TAKEOFF"/>
</block>
@@ -128,11 +128,11 @@
</block>
<block name="land">
<call fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="BASELEG"/>
<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">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<call fun="compute_TOD(WP_AF, WP_TD, WP_TOD, GLIDE_AIRSPEED, GLIDE_VSPEED)"/>
<go approaching_time="0" from="AF" hmode="route" wp="TOD"/>
<go from="TOD" hmode="route" pitch="stage_time*(GLIDE_PITCH/3)" throttle="0" vmode="throttle" wp="TD"/>
conf/flight_plans/competitions/mav08.xml
+6 -6
View File
@@ -32,8 +32,8 @@
<kml file="MAV08_legs.kml"/>
</sectors>
<exceptions>
<exception cond="datalink_time > 30" deroute="Standby"/>
<exception cond="9.7 > PowerVoltage()" deroute="Standby"/>
<exception cond="datalink_time @GT 30" deroute="Standby"/>
<exception cond="9.7 @GT PowerVoltage()" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Holding point">
@@ -41,7 +41,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot_flight_time"/>
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -96,11 +96,11 @@
</block>
<block name="land">
<call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<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">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/demo_gvf.xml
+4 -4
View File
@@ -51,7 +51,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -102,11 +102,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/dynamic_sectors.xml
+4 -4
View File
@@ -41,7 +41,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block name="Takeoff" strip_icon="takeoff.png" strip_button="Takeoff (wp CLIMB)">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<go wp="CLIMB"/>
</block>
@@ -67,12 +67,12 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP_BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
<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) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
<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">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/formation/form_follow.xml
+1 -1
View File
@@ -40,7 +40,7 @@
<block name="land"> <!-- basic landing function out of formation -->
<call_once fun="stop_formation()"/>
<exception cond="(GROUND_ALT+10> GetPosAlt())" deroute="stop"/>
<circle radius="50" until="225 > GetPosAlt()" wp="IAF"/>
<circle radius="50" until="225 @GT GetPosAlt()" wp="IAF"/>
<go alt="215" wp="AF"/>
<go from="AF" hmode="route" vmode="glide" wp="RWAY"/>
</block>
conf/flight_plans/formation/form_leader.xml
+1 -1
View File
@@ -50,7 +50,7 @@
</block>
<block name="land"> <!-- basic landing function out of formation -->
<exception cond="(GROUND_ALT+10> GetPosAlt())" deroute="stop"/>
<circle radius="50" until="WaypointAlt(WP_IAF) > GetPosAlt()" wp="IAF"/>
<circle radius="50" until="WaypointAlt(WP_IAF) @GT GetPosAlt()" wp="IAF"/>
<go alt="WaypointAlt(WP_AF)" wp="AF"/>
<go from="AF" hmode="route" vmode="glide" wp="RWAY"/>
</block>
conf/flight_plans/joystick.xml
+5 -5
View File
@@ -51,7 +51,7 @@ static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), i
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
@@ -71,7 +71,7 @@ static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), i
<circle radius="nav_radius" wp="MOB"/>
</block>
<block name="Line 1-2" strip_button="Line (wp 1-2)" strip_icon="line.png">
<exception cond="datalink_time > 22" deroute="Standby"/>
<exception cond="datalink_time @GT 22" deroute="Standby"/>
<call_once fun="nav_line_setup()"/>
<call fun="nav_line_run(WP_1, WP_2, nav_radius)"/>
</block>
@@ -96,11 +96,11 @@ static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), i
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<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">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/lagonisi_beach_survey.xml
+20 -20
View File
@@ -26,8 +26,8 @@
<sectors/>
<variables/>
<exceptions>
<exception cond="(imcu_get_radio(RADIO_YAW) > MAX_PPRZ/2)&&(autopilot.mode == AP_MODE_AUTO2) && (GetPosAlt() > GetAltRef()+20)" deroute="Target"/>
<exception cond="(MIN_PPRZ/2 > imcu_get_radio(RADIO_YAW))&&(autopilot.mode == AP_MODE_AUTO2) && (GetPosAlt() > GetAltRef()+20)" deroute="Return Home"/>
<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">
@@ -42,25 +42,25 @@
<call_once fun="NavSetWaypointHere(WP_HOME)"/>
</block>
<block name="Holding Point">
<exception cond="((imcu_get_radio(RADIO_THROTTLE) > (MAX_PPRZ-(MAX_PPRZ/10)))&&(autopilot.mode == AP_MODE_AUTO2))" deroute="Takeoff"/>
<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) > 10)&& (700 > gps.pdop))" vmode="throttle"/>
<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() > GetAltRef()+25" deroute="Standby"/>
<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() > GetAltRef()+20" vmode="throttle"/>
<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&&(autopilot.mode == AP_MODE_AUTO2)&&(GetPosAlt() > GetAltRef()+20)" deroute="Land"/>
<exception cond="radio_control.status == RC_REALLY_LOST&&(autopilot.mode == AP_MODE_AUTO2)&&(GetPosAlt() > GetAltRef()+20)" deroute="Land"/>
<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"/>
@@ -68,7 +68,7 @@
<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&&(autopilot.mode == AP_MODE_AUTO2)&&(GetPosAlt() > GetAltRef()+20)" deroute="Return Home NOW"/>
<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"/>
@@ -77,7 +77,7 @@
<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() > 2" 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">
@@ -103,19 +103,19 @@
<deroute block="Land1"/>
</block>
<block name="Land1">
<exception cond="airborne_wind_dir > 270. || 90. > airborne_wind_dir" deroute="Land From South"/>
<exception cond="airborne_wind_dir > 90. && 270. > airborne_wind_dir" deroute="Land From North"/>
<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() > 0.5" wp="BASELEG"/>
<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) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
<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 > GetPosAlt()" deroute="Flare North"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="Flare North"/>
<go from="AF_NORTH" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="Flare North">
@@ -125,13 +125,13 @@
<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() > 0.5" wp="BASELEG"/>
<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) && 10>fabs(GetPosAlt()-WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
<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 > GetPosAlt()" deroute="Flare South"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="Flare South"/>
<go from="AF_SOUTH" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="Flare South">
@@ -142,11 +142,11 @@
<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) && 10>fabs(GetPosAlt()-(ground_alt+30))" wp="BASELEG"/>
<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 > stateGetHorizontalSpeedNorm_f()" vmode="throttle"/>
<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"/>
conf/flight_plans/landing.xml
+3 -3
View File
@@ -17,11 +17,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD" alt="GetAltRef()"/>
</block>
<block name="flare">
conf/flight_plans/mission_fw.xml
+4 -4
View File
@@ -29,7 +29,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
@@ -51,11 +51,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/nav_modules.xml
+4 -4
View File
@@ -62,7 +62,7 @@
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
@@ -147,11 +147,11 @@
</block>
<block name="land">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
conf/flight_plans/quadshot_delft.xml
+10 -10
View File
@@ -35,35 +35,35 @@
<call_once fun="NavResurrect()"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 5.0" deroute="Climb"/>
<exception cond="GetPosHeight() @GT 5.0" deroute="Climb"/>
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<set value="FALSE" var="force_forward_flight"/>
<attitude pitch="0" roll="0" throttle="0.5" until="FALSE" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="stateGetPositionEnu_f()->z > 20.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<set value="FALSE" var="force_forward_flight"/>
<stay climb="2" vmode="climb" wp="STDBY"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<exception cond="radio_control.status @GT RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<exception cond="radio_control.status @GT RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<stay wp="p1"/>
</block>
<block name="go_p2_p1">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<exception cond="radio_control.status @GT RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="pylon_racing">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<exception cond="radio_control.status @GT RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<go wp="p2"/>
<go wp="p3"/>
@@ -72,7 +72,7 @@
<deroute block="stay_p1"/>
</block>
<block name="pylon_racing_forward">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<exception cond="radio_control.status @GT RC_OK" deroute="land"/>
<set value="TRUE" var="force_forward_flight"/>
<go wp="p2"/>
<go wp="p3"/>
@@ -90,15 +90,15 @@
<go wp="STDBY"/>
</block>
<block name="descent">
<exception cond="8.0 > stateGetPositionEnu_f()->z" deroute="descent_slow"/>
<exception cond="8.0 @GT GetPosHeight()" deroute="descent_slow"/>
<stay climb="-1" vmode="climb" wp="STDBY"/>
</block>
<block name="descent_slow">
<exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="flare"/>
<exception cond="2.0 @GT GetPosHeight()" deroute="flare"/>
<stay climb="-1" vmode="climb" wp="STDBY"/>
</block>
<block name="flare">
<exception cond="0.3 > sonar_adc.distance" deroute="landed"/>
<exception cond="0.3 @GT sonar_adc.distance" deroute="landed"/>
<attitude pitch="0" roll="0" throttle="0.13" vmode="throttle"/>
</block>
<block name="landed">
conf/flight_plans/rotorcraft_basic.xml
+1 -1
View File
@@ -67,7 +67,7 @@
<block name="test yaw">
<go wp="p1"/>
<for var="i" from="1" to="16">
<heading alt="WaypointAlt(WP_p1)" course="90 * $i" until="stage_time > 3"/>
<heading alt="WaypointAlt(WP_p1)" course="90 * $i" until="stage_time @GT 3"/>
</for>
<deroute block="Standby"/>
</block>
conf/flight_plans/rotorcraft_basic_geofence.xml
+1 -1
View File
@@ -58,7 +58,7 @@
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="0.5" wp="CLIMB"/>
</block>
conf/flight_plans/rotorcraft_basic_safety.xml
+2 -2
View File
@@ -39,7 +39,7 @@
</sectors>
<exceptions>
<!-- Check inside Flight Area, then goto Standby -->
<exception cond="Or(!InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 50) && !(nav_block == IndexOfBlock('Wait GPS')) && !(nav_block == IndexOfBlock('Geo init')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('landed')) && !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
<exception cond="Or(!InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 50) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('landed')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -60,7 +60,7 @@
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="0.5" wp="CLIMB"/>
</block>
conf/flight_plans/rotorcraft_basic_superbitrf.xml
+5 -5
View File
@@ -39,16 +39,16 @@
</sectors>
<exceptions>
<!-- Check inside small flight area, then goto Center(Standby) -->
<exception cond="Or(! InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 50) && !(nav_block == BLOCK_Init) && !(nav_block == BLOCK_Geo_init) && !(nav_block == BLOCK_Landed)" deroute="Standby"/>
<exception cond="Or(! InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 50) @AND !(nav_block == BLOCK_Init) @AND !(nav_block == BLOCK_Geo_init) @AND !(nav_block == BLOCK_Landed)" deroute="Standby"/>
<!-- Check if battery is empty, then Land Here -->
<exception cond="electrical.bat_low && !(nav_block == BLOCK_Land) && !(nav_block == BLOCK_Flare) && !(nav_block == BLOCK_Landed)" deroute="Land_Here"/>
<exception cond="electrical.bat_low @AND !(nav_block == BLOCK_Land) @AND !(nav_block == BLOCK_Flare) @AND !(nav_block == BLOCK_Landed)" deroute="Land_Here"/>
<!-- Check if time is up(10 seconds range), then Land Here -->
<exception cond="10 > time_until_end && !(nav_block == BLOCK_Land) && !(nav_block == BLOCK_Flare) && !(nav_block == BLOCK_Landed)" deroute="Land_Here"/>
<exception cond="10 @GT time_until_end @AND !(nav_block == BLOCK_Land) @AND !(nav_block == BLOCK_Flare) @AND !(nav_block == BLOCK_Landed)" deroute="Land_Here"/>
</exceptions>
<blocks>
<block name="Init">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || gps.pacc > 1500 || !(ahrs.status == AHRS_RUNNING)"/>
<while cond="!GpsFixValid() || gps.pacc @GT 1500 || !(ahrs.status == AHRS_RUNNING)"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
@@ -65,7 +65,7 @@
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<exception cond="stateGetPositionEnu_f()->z > 5.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 5.0" deroute="Standby"/>
<attitude pitch="0" roll="0" throttle="0.5" vmode="throttle"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml
+13 -13
View File
@@ -46,17 +46,17 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
</sectors>
<exceptions>
<!-- Check inside small flight area, then goto Center(Standby) -->
<exception cond="Or(! InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() > GetAltRef() + 50) && !(nav_block == IndexOfBlock('Init')) && !(nav_block == IndexOfBlock('Geo init')) && !(nav_block == IndexOfBlock('landed'))" deroute="Standby"/>
<exception cond="Or(! InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 50) @AND !(nav_block == IndexOfBlock('Init')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('landed'))" deroute="Standby"/>
<!-- Check if battery is empty, then Land Here -->
<exception cond="electrical.bat_low && !(nav_block == IndexOfBlock('land')) && !(nav_block == IndexOfBlock('flare')) && !(nav_block == IndexOfBlock('landed'))" deroute="land"/>
<exception cond="electrical.bat_critical && !(nav_block == IndexOfBlock('land')) && !(nav_block == IndexOfBlock('flare')) && !(nav_block == IndexOfBlock('landed'))" deroute="land here"/>
<exception cond="electrical.bat_low @AND !(nav_block == IndexOfBlock('land')) @AND !(nav_block == IndexOfBlock('flare')) @AND !(nav_block == IndexOfBlock('landed'))" deroute="land"/>
<exception cond="electrical.bat_critical @AND !(nav_block == IndexOfBlock('land')) @AND !(nav_block == IndexOfBlock('flare')) @AND !(nav_block == IndexOfBlock('landed'))" deroute="land here"/>
<!-- Check if time is up(10 seconds range), then Land Here
<exception cond="10 > time_until_land && !(nav_block == IndexOfBlock('land')) && !(nav_block == IndexOfBlock('flare')) && !(nav_block == IndexOfBlock('landed'))" deroute="land here"/>-->
<exception cond="10 @GT time_until_land @AND !(nav_block == IndexOfBlock('land')) @AND !(nav_block == IndexOfBlock('flare')) @AND !(nav_block == IndexOfBlock('landed'))" deroute="land here"/>-->
</exceptions>
<blocks>
<block name="Init">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || gps.pacc > 400 || !(stateIsAttitudeValid())"/>
<while cond="!GpsFixValid() || gps.pacc @GT 400 || !(stateIsAttitudeValid())"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 8)"/>
@@ -69,23 +69,23 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
</block>
<block name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time > 1"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time @GT 1"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="NavSetAltitudeReferenceHere()"/>
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<exception cond="stateGetPositionEnu_f()->z > 5.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 5.0" deroute="Standby"/>
<set value="0" var="autopilot.flight_time"/>
<!-- If take-off to first point takes to long to reach somehow because of some reason, abort flight -->
<exception cond="block_time > 30" deroute="land"/>
<exception cond="block_time @GT 30" deroute="land"/>
<!-- To make sure that takoff is straight up -->
<attitude pitch="0" roll="0" throttle="0.90" until="stage_time > 1" vmode="throttle"/>
<!--Alternative <exception cond="WaypointAlt(WP_A) > stateGetPositionEnu_f()->z" deroute="A_to_B_and_back"/>-->
<stay vmode="climb" climb="nav_climb_vspeed" until="WaypointAlt(WP_STDBY) > stateGetPositionEnu_f()->z" wp="CLIMB"/>
<attitude pitch="0" roll="0" throttle="0.90" until="stage_time @GT 1" vmode="throttle"/>
<!--Alternative <exception cond="WaypointAlt(WP_A) @GT GetPosHeight()" deroute="A_to_B_and_back"/>-->
<stay vmode="climb" climb="nav_climb_vspeed" until="WaypointAlt(WP_STDBY) @GT GetPosHeight()" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="if(!InsideKill(GetPosX(), GetPosY())) NavKillThrottle()">
<exception cond="block_time > 60" deroute="land"/>
<exception cond="block_time @GT 60" deroute="land"/>
<go wp="STDBY"/>
<stay wp="STDBY"/>
</block>
@@ -127,7 +127,7 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
<stay climb="-0.8" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time > 0.5"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time @GT 0.5"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
conf/flight_plans/rotorcraft_cam.xml
+2 -2
View File
@@ -33,7 +33,7 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
@@ -67,7 +67,7 @@
<block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
<set value="25" var="nav_radius"/>
<call_once fun="dc_Circle(30)"/>
<circle radius="nav_radius" until="NavCircleCount() > 1.0" wp="CAM"/>
<circle radius="nav_radius" until="NavCircleCount() @GT 1.0" wp="CAM"/>
<call_once fun="dc_stop()"/>
<deroute block="Standby"/>
</block>
conf/flight_plans/rotorcraft_guided_flightplan.xml
+2 -2
View File
@@ -68,7 +68,7 @@ Remember to choose AP_MODE_GUIDED or else it won't work!!!
<block name="Take off">
<set var="desired_heading" value="stateGetNedToBodyEulers_f()->psi"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_Z_VEL,0.,0.,-climb_descent_vel,desired_heading)"/>
<while cond="LessThan(stateGetPositionEnu_f()->z,nominal_alt-0.2)"/>
<while cond="LessThan(GetPosHeight(),nominal_alt-0.2)"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET,0.,0.,-nominal_alt,desired_heading)"/>
<while cond="LessThan(stage_time,1)"/>
</block>
@@ -115,7 +115,7 @@ NOTE: A velocity forcefield in the horizontal plane applied here, so if an obsta
<block name="Land">
<set var="desired_heading" value="stateGetNedToBodyEulers_f()->psi"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_Z_VEL,0.,0.,climb_descent_vel,desired_heading)"/>
<while cond="MoreThan(stateGetPositionEnu_f()->z,0.1)"/>
<while cond="MoreThan(GetPosHeight(),0.1)"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_YAW_OFFSET,0.,0.,0.,0.)"/>
<while cond="LessThan(block_time,5)"/>
<deroute block="Kill Engines"/>
conf/flight_plans/rotorcraft_guido_optitrack.xml
+1 -1
View File
@@ -42,7 +42,7 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 1.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 1.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
conf/flight_plans/rotorcraft_joystick_enac.xml
+1 -1
View File
@@ -65,7 +65,7 @@ static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), i
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 1.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 1.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="STDBY"/>
</block>
conf/flight_plans/rotorcraft_optitrack.xml
+1 -1
View File
@@ -41,7 +41,7 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 1.0" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 1.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml
+4 -4
View File
@@ -45,10 +45,10 @@
<block name="Start Engine" strip_button="Start" strip_icon="takeoff.png">
<call_once fun="NavResurrect()"/>
<set value="stateGetNedToBodyEulers_i()->psi" var="nav_heading"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time > 1" vmode="throttle"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time @GT 1" vmode="throttle"/>
</block>
<block name="Takeoff">
<exception cond="stateGetPositionEnu_f()->z > 0.5" deroute="Standby"/>
<exception cond="GetPosHeight() @GT 0.5" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
@@ -63,7 +63,7 @@
<exception cond="electrical.bat_low" deroute="land here"/>
<call_once fun="nav_set_heading_towards_waypoint(WP_GOAL)"/>
<call_once fun="NavSetWaypointHere(WP_route_START)"/>
<stay until="stage_time > 3" wp="route_START"/>
<stay until="stage_time @GT 3" wp="route_START"/>
</block>
<block name="route_to_GOAL">
<exception cond="GpsIsLost()" deroute="attitude_land"/>
@@ -107,7 +107,7 @@
<exception cond="NavDetectGround()" deroute="landed"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav_descend_vspeed" until="stage_time > 4" vmode="climb" wp="TD"/>
<stay climb="nav_descend_vspeed" until="stage_time @GT 4" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<call_once fun="NavKillThrottle()"/>

Some files were not shown because too many files have changed in this diff Show More