-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
rotorcraft_basic_superbitrf_from_hand.xml
137 lines (135 loc) · 6 KB
/
rotorcraft_basic_superbitrf_from_hand.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
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<!--
The goal of this flightplan is to have a safe, simple no-brainer flightplan for a ladylisa.
-->
<flight_plan alt="5" ground_alt="0" lat0="51.991231" lon0="4.378035" max_dist_from_home="60" name="Fromhand" security_height="2">
<header>
#include "autopilot.h"
#include "subsystems/ahrs/ahrs_int_cmpl_quat.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="0.0" y="0.0"/>
<waypoint name="STDBY" x="0.0" y="0.0"/>
<waypoint name="p1" x="0.0" y="2.0"/>
<waypoint name="p2" x="0.0" y="-2.0"/>
<waypoint name="p3" x="2.0" y="2.0"/>
<waypoint name="p4" x="2.0" y="0.0"/>
<waypoint name="FLY1" x="10" y="10"/>
<waypoint name="FLY2" x="10" y="-10"/>
<waypoint name="FLY3" x="-10" y="-10"/>
<waypoint name="FLY4" x="-10" y="10"/>
<waypoint name="KILL1" x="15" y="15"/>
<waypoint name="KILL2" x="15" y="-15"/>
<waypoint name="KILL3" x="-15" y="-15"/>
<waypoint name="KILL4" x="-15" y="15"/>
<waypoint name="CAM" x="-5" y="-5" height="2.0"/>
<waypoint name="TD" x="0.0" y="0.0"/>
</waypoints>
<sectors>
<sector color="red" name="Flight_Area">
<corner name="FLY1"/>
<corner name="FLY2"/>
<corner name="FLY3"/>
<corner name="FLY4"/>
</sector>
<sector color="red" name="Kill">
<corner name="KILL1"/>
<corner name="KILL2"/>
<corner name="KILL3"/>
<corner name="KILL4"/>
</sector>
</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"/>
<!-- 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"/>
<!-- 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"/>-->
</exceptions>
<blocks>
<block name="Init">
<call fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || gps.pacc > 400 || !(ahrs_icq.status == AHRS_ICQ_RUNNING)"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 8)"/>
<call fun="NavSetGroundReferenceHere()"/>
<call fun="NavSetAltitudeReferenceHere()"/>
<call fun="NavSetWaypointHere(WP_HOME)"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<call fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="Start Engine">
<call fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time > 1"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call fun="NavSetAltitudeReferenceHere()"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<exception cond="stateGetPositionEnu_f()->z > 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"/>
<!-- 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"/>
</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"/>
<go wp="STDBY"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<stay wp="p1"/>
</block>
<block name="go_p2">
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="line_p1_p2">
<go from="p1" hmode="route" wp="p2"/>
<stay wp="p2" until="stage_time>10"/>
<go from="p2" hmode="route" wp="p1"/>
<deroute block="stay_p1"/>
</block>
<block name="route">
<go from="p1" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="stay_p1"/>
</block>
<block name="circle">
<circle radius="nav_radius" wp="p1"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<go wp="TD"/>
<exception cond="NavDetectGround()" deroute="landed"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<exception cond="NavDetectGround()" deroute="landed"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<call fun="NavStartDetectGround()"/>
<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"/>
</block>
<block name="Holding point">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
</blocks>
</flight_plan>