AP_Scripting: add Plane autoland applet

ArduPlane/mode_cruise.cpp

@@ -85,7 +85,7 @@ void ModeCruise::navigate()
     if (locked_heading) {
         plane.next_WP_loc = plane.prev_WP_loc;
         // always look 1km ahead
-        plane.next_WP_loc.offset_bearing(locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
+ //       plane.next_WP_loc.offset_bearing(locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
         plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
     }
 }

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
+

libraries/AP_Scripting/applets/UniversalAutoLand.md

@@ -0,0 +1,9 @@
+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.
+
+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 untilt 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.