diff --git a/libraries/AP_Scripting/applets/UniversalAutoLand.lua b/libraries/AP_Scripting/applets/UniversalAutoLand.lua new file mode 100644 index 00000000000000..135c4643bf68b4 --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.lua @@ -0,0 +1,139 @@ +--[[ 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 + + +FRAME_GLOBAL = 3 +NAV_WAYPOINT = 16 +NAV_TAKEOFF = 22 +NAV_LAND = 21 +DO_LAND_START = 189 + +TAKEOFF_PITCH = 15 + +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +local function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +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(alt) + 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(TAKEOFF_PITCH) + 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_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 and home and location:alt() > (home:alt() + param:get("TKOFF_LVL_ALT")*100) then + local yaw = ahrs:get_yaw() + mission:set_item(3,create_final_approach_WP(3,wrap_180(math.deg(yaw)+180),FIN_WP_DIST,FIN_WP_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 FIN_WP_DIST == 0 or FIN_WP_ALT ==0 then + gcs:send_text(0, string.format("Must set SCR_USER1 and 2 values!")) + return + end + mission:clear() + local item = mavlink_mission_item_int_t() + item:command(NAV_WAYPOINT) + mission:set_item(0,item) + mission:set_item(1,create_takeoff_WP(param:get("TKOFF_ALT"))) + mission:set_item(2,create_do_land_start_WP()) + return update, 1000 +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..1017468945300f --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.md @@ -0,0 +1,7 @@ +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. + +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, to SCR_USER3 above home, and at SCR_USER2 distancee, and a LAND waypoint at HOME and stops until next disarm/boot. Warnings are given if the SCR_USER values are not set. + +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. + +Caveats: be sure you have tested and setup autoland and SCR_USER1/2 parameters. Setting SCR_USER2 and 3 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.