-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
form_leader.xml
57 lines (56 loc) · 2.67 KB
/
form_leader.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
48
49
50
51
52
53
54
55
56
57
<!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="formation flight" qfu="270" security_height="25">
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="mid" x="72.9" y="133.0"/>
<waypoint name="wp0" x="161.9" y="35.9"/>
<waypoint name="wp1" x="225.5" y="159.2"/>
<waypoint name="wp2" x="57.4" y="290.9"/>
<waypoint name="wp3" x="-79.8" y="220.4"/>
<waypoint name="wp4" x="20.4" y="148.4"/>
<waypoint name="wp5" x="22.9" y="69.5"/>
<waypoint alt="215.0" name="IAF" x="239.7" y="92.9"/>
<waypoint alt="210.0" name="AF" x="187.0" y="-2.8"/>
<waypoint alt="185.0" name="RWAY" x="42.3" y="47.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"/>
</block>
<block name="cirle"> <!-- waiting circle out of formation -->
<circle radius="nav_radius" until="FALSE" wp="mid"/>
</block>
<block name="start formation">
<call fun="start_formation()"/> <!-- inform other AC that formation is started -->
</block>
<block name="traj" pre_call="formation_pre_call()" post_call="formation_flight()"> <!-- formation flight is call after all other navigation tasks -->
<go from="wp5" hmode="route" wp="wp0"/>
<go from="wp0" hmode="route" wp="wp1"/>
<go from="wp1" hmode="route" wp="wp2"/>
<go from="wp2" hmode="route" wp="wp3"/>
<go from="wp3" hmode="route" wp="wp4"/>
<go from="wp4" hmode="route" wp="wp5"/>
<deroute block="traj"/>
</block>
<block name="stop formation"> <!-- when leader stop the formation, the other AC will do the same -->
<call fun="stop_formation()"/>
</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"/>
<go alt="WaypointAlt(WP_AF)" 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>