-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
course_orangeavoid_cyberzoo.xml
133 lines (133 loc) · 5.58 KB
/
course_orangeavoid_cyberzoo.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
<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
<header>
#include "modules/datalink/datalink.h"
#include "modules/energy/electrical.h"
#include "modules/radio_control/radio_control.h"
#include "modules/ahrs/ahrs.h"
#define NPS_GAZEBO_WORLD "cyberzoo_orange_poles.world"
</header>
<waypoints>
<waypoint lat="51.990631" lon="4.376796" name="HOME"/>
<waypoint name="CLIMB" x="1.9" y="1.0"/>
<waypoint name="STDBY" x="1.9" y="1.0"/>
<waypoint name="TD" x="0.8" y="-1.7"/>
<waypoint name="GOAL" x="1.9" y="1.0"/>
<waypoint name="TRAJECTORY" x="1.9" y="1.0"/>
<waypoint lat="51.9905834" lon="4.3767710" name="_CZ1"/>
<waypoint lat="51.9906440" lon="4.3767060" name="_CZ2"/>
<waypoint lat="51.9906860" lon="4.3768080" name="_CZ3"/>
<waypoint lat="51.9906238" lon="4.3768729" name="_CZ4"/>
<waypoint lat="51.9906006" lon="4.3767764" name="_OZ1"/>
<waypoint lat="51.9906405" lon="4.3767316" name="_OZ2"/>
<waypoint lat="51.9906687" lon="4.3768025" name="_OZ3"/>
<waypoint lat="51.9906273" lon="4.3768438" 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>
<!-- RC lost -->
<!--exception cond="((radio_control.status == RC_REALLY_LOST) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Standby"/-->
<!-- Datalink lost (constant RPM descent) -->
<exception cond="((datalink_time @GT 5) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing XY -->
<exception cond="(!InsideCyberZoo(GetPosX(), GetPosY()) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing Z 3.5 -->
<exception cond="((GetPosAlt() @GT 3.5) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing Z 4.5 (constant RPM descent)-->
<exception cond="((GetPosAlt() @GT 4.5) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- Bat low -->
<exception cond="(electrical.bat_low @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Bat critical (constant RPM no stabilization)-->
<exception cond="(electrical.bat_critical @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 2)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block key="r" name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<call_once fun="NavResurrect()"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosAlt() @GT 0.8" deroute="Standby"/>
<call_once 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_once fun="NavSetWaypointHere(WP_STDBY)"/>
<stay wp="STDBY"/>
</block>
<block key="g" name="START" strip_button="Go" strip_icon="lookfore.png">
<call_once fun="NavSetWaypointHere(WP_GOAL)"/>
<stay wp="GOAL"/>
</block>
<block name="STOP">
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<stay wp="STDBY"/>
</block>
<block key="l" name="land here" strip_button="land here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Land">
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
<exception cond="0.10 @GT GetPosAlt()" deroute="Landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Landed">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>