-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
form_follow.xml
47 lines (46 loc) · 2.29 KB
/
form_follow.xml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="250" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="400" name="test laas" qfu="270" security_height="25">
<waypoints>
<waypoint alt="250" name="HOME" x="0.0" y="0.0"/>
<waypoint alt="215.0" name="IAF" x="239.7" y="92.9"/>
<waypoint alt="215.0" name="AF" x="187.0" y="-2.8"/>
<waypoint alt="185.0" name="RWAY" x="42.3" y="47.5"/>
<waypoint alt="260.0" name="WAIT1" x="118.0" y="84.9"/>
<waypoint alt="240.0" name="WAIT2" x="-34.4" y="143.5"/>
</waypoints>
<blocks>
<block name="init formation">
<!-- formation_init() is already called -->
<!-- init formation by adding knew slots in the formation -->
<!-- three UAVs are expected but it can work with 1 or 2 -->
<call fun="add_slot(21, 10, 0, 0)"/>
<call fun="add_slot(22, -20, 20, 10)"/>
<call fun="add_slot(23, -20, -20, -10)"/>
<set var="form_mode" value="0"/> <!-- form mode global add_slot(id, east, north, up) -->
<set var="leader_id" value="21"/> <!-- leader id -->
<set var="nav_mode" value="NAV_MODE_ROLL"/> <!-- alternate navigation mode should be set in formation flight -->
</block>
<block name="cirle 1"> <!-- waiting circle out of formation -->
<call fun="stop_formation()"/>
<circle radius="nav_radius" until="FALSE" wp="WAIT1"/>
</block>
<block name="cirle 2"> <!-- waiting circle out of formation -->
<call fun="stop_formation()"/>
<circle radius="nav_radius" until="FALSE" wp="WAIT2"/>
</block>
<block name="formation"> <!-- start formation for a follower, leader has to be ready and in formation -->
<call fun="start_formation()"/> <!-- inform other AC that formation is started -->
<call fun="formation_flight()"/> <!-- formation control loop -->
</block>
<block name="land"> <!-- basic landing function out of formation -->
<call fun="stop_formation()"/>
<exception cond="(GROUND_ALT+10> GetPosAlt())" deroute="stop"/>
<circle radius="50" until="225 > GetPosAlt()" wp="IAF"/>
<go alt="215" wp="AF"/>
<go from="AF" hmode="route" vmode="glide" wp="RWAY"/>
</block>
<block name="stop">
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>