diff --git a/libraries/AP_Scripting/applets/UniversalAutoLand.lua b/libraries/AP_Scripting/applets/UniversalAutoLand.lua new file mode 100644 index 00000000000000..2b6842478a5b5c --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.lua @@ -0,0 +1,121 @@ +--[[ 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. +--]] + +--local variablees + +SCR_USER_EN = Parameter("SCR_USER1") -- enable this script +local FIN_WP_DIST = param:get("SCR_USER2") -- distance of final wp from HOME +local FIN_WP_ALT = param:get("SCR_USER3") --- altitude of final wp from HOME +local TKOFF_ALT = param:get("TKOFF_ALT") +local TKOFF_DIST = param:get("TKOFF_DIST") + +-- make sure you have a reasonable final angle if someone does not change defualts other than SCR1_USER to enable +if FIN_WP_ALT < TKOFF_ALT then FIN_WP_ALT = TKOFF_ALT end +if FIN_WP_DIST < TKOFF_DIST then FIN_WP_DIST = 2 * TKOFF_DIST end + +FRAME_GLOBAL = 3 +NAV_WAYPOINT = 16 +NAV_TAKEOFF = 22 +NAV_LAND = 21 +DO_LAND_START = 189 + +function create_final_approach_WP(i,bearing,dist,alt) --index,degs,m,m + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + loc:offset_bearing(bearing,dist) ---degs and meters + + item:seq(i) + item:frame(FRAME_GLOBAL) + item:command(NAV_WAYPOINT) + item:param1(0) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(alt) + return item +end + +function create_takeoff_WP() + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + + item:seq(1) + item:frame(FRAME_GLOBAL) + item:command(NAV_TAKEOFF) + item:param1(15) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(TKOFF_ALT) + return item +end + +function create_land_WP() + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + + item:seq(4) + item:frame(FRAME_GLOBAL) + item:command(NAV_LAND) + item:param1(15) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(0) + return item +end + +function create_do_land_start_WP() + local item = mavlink_mission_item_int_t() + + item:seq(2) + item:frame(FRAME_GLOBAL) + item:command(DO_LAND_START) + item:param1(0) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(0) + item:y(0) + item:z(0) + return item +end + +function update() + if not arming:is_armed() then --if disarmed, wait until armed + mission_loaded = false + return update,1000 + end + + if not mission_loaded then --if first time after arm and enabled is valid then create mission + local home = ahrs:get_home() + local location = ahrs:get_location() + if location:alt() > (home:alt() + param:get("TKOFF_LVL_ALT")*100) then + local yaw = ahrs:get_yaw() + mission:set_item(2,create_do_land_start_WP()) + mission:set_item(3,create_final_approach_WP(3,math.deg(yaw)+180,FIN_WP_DIST,param:get("TKOFF_ALT"))) + mission:set_item(4,create_land_WP()) + mission_loaded = true + end + end + return update, 1000 +end + +gcs:send_text(5,"Loaded TKOFF/RTL/Land Mission'.lua") +if SCR_USER_EN:get() == 1 then + if not mission:set_item(0,create_final_approach_WP(0,0,0,0)) then + gcs:send_text(0, string.format("Failed to create HOME WP, investigate!")) --something's really wrong! + end +mission:set_item(1,create_takeoff_WP()) +return update, 5000 +else +gcs:send_text(0, string.format("Script disabled by SCR_USER1")) +return +end + diff --git a/libraries/AP_Scripting/applets/UniversalAutoLand.md b/libraries/AP_Scripting/applets/UniversalAutoLand.md new file mode 100644 index 00000000000000..6bab26a703c150 --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.md @@ -0,0 +1,7 @@ +This script is intended to allow eady, 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. + +If enabled by SCR_USER1 =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, at TKOFF_ALT or SCR_USER3 above home, and at SCR_USER2 or 2X TKOFF_DIST, and a LAND waypoint at HOME and stops until next disarm/boot. + +In use you just arm and switch to AUTO, and switch to other flight modes after takeoff is completed.....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. + +Caveats: be sure you have tested and setup autoland and TKOFF parameters. Setting SCR_USER2 and 3 for a good glide on a final approach is recommended. RTL_AUTOLAND =2 is recommended also.