-
Notifications
You must be signed in to change notification settings - Fork 0
/
variance_flightplan_group10.xml
152 lines (128 loc) · 6.18 KB
/
variance_flightplan_group10.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
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
<!---Edited by Group 10 AFMAV Course March 2016--->
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="1.5" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="8" name="Bebop variance_avoider TU Delft Cyberzoo" security_height="0.4">
<header>
#include "autopilot.h"
#include "subsystems/ahrs.h"
#include "subsystems/electrical.h"
#include "subsystems/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="1.2" y="-0.6"/>
<waypoint name="GOAL" x="2.0" y="2.0"/>
<waypoint height="1.342321" name="STDBY" x="-0.7" y="-0.8"/>
<waypoint name="TD" x="0.8" y="-1.7"/>
<waypoint lat="51.9906213" lon="4.3768628" name="_CZ1"/>
<waypoint lat="51.9905874" lon="4.3767766" name="_CZ2"/>
<waypoint lat="51.9906409" lon="4.3767226" name="_CZ3"/>
<waypoint lat="51.990667" lon="4.376806" name="_CZ4"/>
<waypoint lat="51.990624" lon="4.376845" name="_OZ1"/>
<waypoint lat="51.990601" lon="4.376782" name="_OZ2"/>
<waypoint lat="51.990638" lon="4.376748" name="_OZ3"/>
<waypoint lat="51.990657" lon="4.376804" name="_OZ4"/>
</waypoints>
<sectors>
<sector color="red" name="CyberZoo">
<corner name="_CZ1"/>
<corner name="_CZ2"/>
<corner name="_CZ3"/>
<corner name="_CZ4"/>
</sector>
<sector color="#FF9922" name="ObstacleZone">
<corner name="_OZ1"/>
<corner name="_OZ2"/>
<corner name="_OZ3"/>
<corner name="_OZ4"/>
</sector>
</sectors>
<exceptions>
<exception cond="!InsideCyberZoo(GetPosX(), GetPosY())" deroute="Standby"/>
<!--<exception cond="datalink_time > 2" deroute="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"/>
-->
</exceptions>
<blocks>
<block name="Wait GPS">
<call fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 2)"/>
<call fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block key="r" name="Start Engine">
<call fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 1.25" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png">
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<stay wp="STDBY"/>
</block>
<block key="g" name="START" strip_button="Go" strip_icon="lookfore.png">
<call fun="NavSetWaypointHere(WP_GOAL)"/>
</block>
<!-- dit is het main block -->
<block name="StayGoal">
<go wp="GOAL"/>
<exception cond="block_time > 1" deroute="MoveGoalForward"/>
<exception cond="!safeToGoForwards" deroute="ChangeHeading"/> <!-- NOT safeToGoForwards -> ChangeHeading -->
<exception cond="!InsideObstacleZone(GetPosX(),GetPosY())" deroute="PrepareObstacleRun"/> <!-- NOT InsideObstacleZone -> Block PrepareObstacleRun -->
</block>
<!-- Aangeroepen door StayGoal, als het safe is to go forward -->
<block name="MoveGoalForward">
<call fun="moveWaypointForwards(WP_GOAL,2.0)"/>
<deroute block="StayGoal"/> <!-- Back to StayGoal -->
<exception cond="!safeToGoForwards" deroute="ChangeHeading"/> <!-- NOT safeToGoForwards -> ChangeHeading -->
<exception cond="!InsideObstacleZone(WaypointX(WP_GOAL),WaypointY(WP_GOAL))" deroute="PrepareObstacleRun"/>
</block>
<!-- Aangeroepen door StayGoal en MoveGoalForward als het niet veilig is -->
<block name="ChangeHeading">
<call fun="NavSetWaypointHere(WP_STDBY)"/> <!-- Set Waypoint to Waypoint STDBY -->
<while cond="1">
<go wp="STDBY"/> <!-- To waypoint STDBY -->
<call fun="increase_nav_heading(&nav_heading, incrementForAvoidance)"/>
<call fun="moveWaypointForwards(WP_GOAL,2.0)"/>
</while>
<exception cond="safeToGoForwards" deroute="StayGoal"/> <!-- Check if safe to go forward, then to block StayGoal -->
</block>
<block name="PrepareObstacleRun">
<call fun="NavSetWaypointHere(WP_STDBY)"/> <!-- set waypoint to WP_STDBY -->
<call fun="nav_set_heading_towards_waypoint(WP_HOME)"/>
<stay wp="STDBY"/> <!-- Hover at Wp STDBY -->
<exception cond="block_time > 1" deroute="GetOutOfObstacleZone"/>
</block>
<block name="GetOutOfObstacleZone">
<call fun="moveWaypointForwards(WP_GOAL,0.5)"/>
<call fun="chooseRandomIncrementAvoidance()"/>
<stay wp="GOAL"/>
<exception cond="block_time > 1" deroute="START"/>
</block>
<block key="l" name="Land here" strip_button="Land Here" strip_icon="land-right.png">
<call fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="Land">
<go wp="TD"/>
</block>
<block name="Flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
<call fun="NavStartDetectGround()"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Landed">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>