Skip to content

Commit 244ea59

Browse files
HwurzburgHwurzburg
committed
AP_Scripting: add Plane autoland applet
1 parent 1e99226 commit 244ea59

File tree

2 files changed

+183
-0
lines changed

2 files changed

+183
-0
lines changed

‎libraries/AP_Scripting/applets/UniversalAutoLand.lua

Lines changed: 174 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,174 @@
1+
--[[ Upon Arming , creates a four item mission consisting of: NAV_TAKEOFF, DO_LAND_START,Final Approach WP opposite bearing from HOME of heading used during takeoff, at TKOFF_ALT or SCR_USER3 above home, SCR_USER2 or 2X TKOFF_DIST, and a LAND waypoint at HOME and stops until next disarm/boot. SCR_USER1 is used to enable or disable it.
2+
--]]
3+
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
4+
5+
local PARAM_TABLE_KEY = 72
6+
local PARAM_TABLE_PREFIX = "AUTOLAND_"
7+
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add param table')
8+
9+
-- add a parameter and bind it to a variable
10+
function bind_add_param(name, idx, default_value)
11+
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
12+
return Parameter(PARAM_TABLE_PREFIX .. name)
13+
end
14+
15+
--[[
16+
// @Param: ENABLE
17+
// @DisplayName: AUTOLAND ENABLE
18+
// @Description: enable AUTOLAND script action
19+
// @Values: 0:Disabled,1:Enabled
20+
// @User: Standard
21+
--]]
22+
local AULND_ENABLE = bind_add_param('ENABLE', 1, 1)
23+
local enable = AULND_ENABLE:get()
24+
25+
--[[
26+
// @Param: WP_ALT
27+
// @DisplayName: Final approach waypoint alt
28+
// @Description: Altitude of final approach waypoint created by script
29+
// @Range: 1 100
30+
// @Units: m
31+
// @User: Standard
32+
--]]
33+
local AULND_ALT = bind_add_param('ALT', 2, 0)
34+
local final_wp_alt = AULND_ALT:get()
35+
--[[
36+
// @Param: WP_DIST
37+
// @DisplayName: Final approach waypoint distance
38+
// @Description: Distance from landng point (HOME) to final approach waypoint created by script in the opposite direction of initial takeoff
39+
// @Range: 0 1000
40+
// @Units: m
41+
// @User: Standard
42+
--]]
43+
local AULND_DIST = bind_add_param('DIST', 3, 0)
44+
local final_wp_dist = AULND_DIST:get()
45+
46+
FRAME_GLOBAL = 3
47+
NAV_WAYPOINT = 16
48+
NAV_TAKEOFF = 22
49+
NAV_LAND = 21
50+
DO_LAND_START = 189
51+
52+
TAKEOFF_PITCH = 15
53+
54+
local function wrap_360(angle)
55+
local res = math.fmod(angle, 360.0)
56+
if res < 0 then
57+
res = res + 360.0
58+
end
59+
return res
60+
end
61+
62+
local function wrap_180(angle)
63+
local res = wrap_360(angle)
64+
if res > 180 then
65+
res = res - 360
66+
end
67+
return res
68+
end
69+
70+
function create_final_approach_WP(i,bearing,dist,alt) --index,degs,m,m
71+
local item = mavlink_mission_item_int_t()
72+
local loc = ahrs:get_home()
73+
loc:offset_bearing(bearing,dist) ---degs and meters
74+
75+
item:seq(i)
76+
item:frame(FRAME_GLOBAL)
77+
item:command(NAV_WAYPOINT)
78+
item:param1(0)
79+
item:param2(0)
80+
item:param3(0)
81+
item:param4(0)
82+
item:x(loc:lat())
83+
item:y(loc:lng())
84+
item:z(alt)
85+
return item
86+
end
87+
88+
function create_takeoff_WP(alt)
89+
local item = mavlink_mission_item_int_t()
90+
local loc = ahrs:get_home()
91+
92+
item:seq(1)
93+
item:frame(FRAME_GLOBAL)
94+
item:command(NAV_TAKEOFF)
95+
item:param1(TAKEOFF_PITCH)
96+
item:param2(0)
97+
item:param3(0)
98+
item:param4(0)
99+
item:x(loc:lat())
100+
item:y(loc:lng())
101+
item:z(alt)
102+
return item
103+
end
104+
105+
function create_land_WP()
106+
local item = mavlink_mission_item_int_t()
107+
local loc = ahrs:get_home()
108+
109+
item:seq(4)
110+
item:frame(FRAME_GLOBAL)
111+
item:command(NAV_LAND)
112+
item:param1(15)
113+
item:param2(0)
114+
item:param3(0)
115+
item:param4(0)
116+
item:x(loc:lat())
117+
item:y(loc:lng())
118+
item:z(0)
119+
return item
120+
end
121+
122+
function create_do_land_start_WP()
123+
local item = mavlink_mission_item_int_t()
124+
125+
item:seq(2)
126+
item:frame(FRAME_GLOBAL)
127+
item:command(DO_LAND_START)
128+
item:param1(0)
129+
item:param2(0)
130+
item:param3(0)
131+
item:param4(0)
132+
item:x(0)
133+
item:y(0)
134+
item:z(0)
135+
return item
136+
end
137+
138+
function update()
139+
if not arming:is_armed() then --if disarmed, wait until armed
140+
mission_loaded = false
141+
return update,1000
142+
end
143+
144+
if not mission_loaded then --if first time after arm and enabled is valid then create mission
145+
local home = ahrs:get_home()
146+
local location = ahrs:get_location()
147+
if location and home and location:alt() > (home:alt() + param:get("TKOFF_LVL_ALT")*100) then
148+
local yaw = ahrs:get_yaw()
149+
mission:set_item(3,create_final_approach_WP(3,wrap_180(math.deg(yaw)+180),final_wp_dist,final_wp_alt))
150+
mission:set_item(4,create_land_WP())
151+
mission_loaded = true
152+
end
153+
end
154+
return update, 1000
155+
end
156+
157+
gcs:send_text(MAV_SEVERITY.INFO,"Loaded UniversalAutoLand.lua")
158+
if enable == 1 then
159+
if final_wp_dist == 0 or final_wp_alt ==0 then
160+
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Must set Final Waypoint alt and dist values!"))
161+
return
162+
end
163+
mission:clear()
164+
local item = mavlink_mission_item_int_t()
165+
item:command(NAV_WAYPOINT)
166+
mission:set_item(0,item)
167+
mission:set_item(1,create_takeoff_WP(param:get("TKOFF_ALT")))
168+
mission:set_item(2,create_do_land_start_WP())
169+
return update, 1000
170+
else
171+
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Script disabled by AUTOLAND_ENABLE"))
172+
return
173+
end
174+

‎libraries/AP_Scripting/applets/UniversalAutoLand.md

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
This script is intended to allow easy, unpre-planned operation at any location with the protection of a do-land-start autoland sequence for failsafes that accounts for takeoff direction (ie wind direction). Final approach objects must be considered before you launch.
2+
3+
If enabled by AUTOLAND_ENABLE =1, setups up an autotakeoff waypoint as first waypoint and upon Arming , adds mission items consisting of: DO_LAND_START,Final Approach WP opposite bearing from HOME of heading used during takeoff, to AUTOLAND_WP_ALT above home, and at AUTOLAND_WP_DIST distancee, and a LAND waypoint at HOME and stops until next disarm/boot. Warnings are given if the SCR_USER values are not set.
4+
5+
In use you just arm and switch to AUTO, and then switch to other flight modes after takeoff is completed to fly around.....relatively assured that a failsafe (assuming defaults for FS) will result in an autolanding in the correct direction. You can also just switch back to AUTO or RTL to do an autoland.
6+
7+
Caveats: be sure you have tested and setup autoland and SCR_USER1/2 parameters. Setting AUTLAOND_WP_ALT and _DIST for a good glide path on a final approach is required (be aware of possible obstructions when using). Values of 400 meters distance and 55 meters altitude work well for typcial 1m wingspan/1 kg foam planes. RTL_AUTOLAND =2 is recommended also.
8+
9+
On compass-less planes the takeoff heading is not locked until TKOFF_LVL_ALT. Hand launching into a crosswind usually results in a 10-15 degree yaw by the crosswind as the vehicle comes up to speed and until flying surfaces become effective in the forward airflow. Be aware of obstacles that might come into play if the final approach is shifted from initial launch heading by that initial yaw shift.

0 commit comments

Comments
 (0)