LUA: fix script so it writes the mission

scripts/droptest.lua

@@ -33,17 +33,32 @@ local last_mission_setup_t = 0.0
 local last_wp_change_t = 0.0
 local release_t = 0
 
-logfile = io.open("log.txt", "w")
+local ENABLE_LOGGING = false
+local logging_init = false
+
 
 function logit(txt)
-   --logfile:write(txt .. "\n")
-   --logfile:flush()
+   if not ENABLE_LOGGING then
+      return
+   end
+
+   if (logging_init == false) then
+      logfile = io.open("log.txt", "w")
+      logging_init = true
+   end
+
+   logfile:write(txt .. "\n")
+   logfile:flush()
 end
 
 DO_JUMP = 177
 NAV_WAYPOINT = 16
 NAV_LAND = 21
+DO_LAND_START = 189
 
+local APPROACH_DIST_MAX = 6400
+local BASE_DIST = 1390
+local STEP_RATIO = 0.5
 -- see if we are running on SITL
 local is_SITL = param:get('SIM_SPEEDUP')
 
@@ -491,6 +506,210 @@ function fix_WP_heights()
       end
    end
 end
+-- see if mission is setup for auto-creation
+function create_mission_check()
+   if mission:num_commands() ~= 4 then
+      return false
+   end
+   if mission:get_item(1):command() ~= NAV_LAND or mission:get_item(2):command() ~= NAV_LAND then
+      return false
+   end
+   if mission:get_item(3):command() ~= DO_LAND_START then
+      return false
+   end
+   return true
+end
+
+function loc_copy(loc)
+   local loc2 = Location()
+   loc2:lat(loc:lat())
+   loc2:lng(loc:lng())
+   loc2:alt(loc:alt())
+   return loc2
+end
+
+-- set wp location
+function wp_setloc(wp, loc)
+   wp:x(loc:lat())
+   wp:y(loc:lng())
+   wp:z(loc:alt()*0.01)
+end
+
+-- get wp location
+function wp_getloc(wp)
+   local loc = Location()
+   loc:lat(wp:x())
+   loc:lng(wp:y())
+   loc:alt(math.floor(wp:z()*100))
+   return loc
+end
+
+-- shift a wp in polar coordinates
+function wp_offset(wp, ang1, dist1)
+   local loc = wp_getloc(wp)
+   loc:offset_bearing(ang1,dist1)
+   wp:x(loc:lat())
+   wp:y(loc:lng())
+end
+
+-- copy and shift a location
+function loc_shift(loc,angle,dist)
+   local loc2 = Location()
+   loc2:lat(loc:lat())
+   loc2:lng(loc:lng())
+   loc2:alt(loc:alt())
+   loc2:offset_bearing(angle,dist)
+   return loc2
+end
+
+-- add a waypoint to the end
+function wp_add(loc,ctype,param1,param2)
+   local wp = mavlink_mission_item_int_t()
+   wp_setloc(wp,loc)
+   wp:command(ctype)
+   local seq = mission:num_commands()
+   wp:seq(seq)
+   wp:param1(param1)
+   wp:param2(param2)
+   mission:set_item(seq,wp)
+end
+
+-- adjust approach distance based on glide slope and location of LAND1
+-- and DO_LAND_START
+function adjust_approach_dist(land1_loc, dls_loc, angle)
+   local alt_change = (dls_loc:alt() - land1_loc:alt()) * 0.01
+   local target_dist = 1.3 * GLIDE_SLOPE * alt_change
+   gcs:send_text(0, string.format("alt_change=%.2f target_dist=%.2f", alt_change, target_dist))
+
+   -- converge on correct approach distance
+   local dist
+   for i = 1, 10 do
+      local loc2 = loc_shift(land1_loc, angle, APPROACH_DIST_MAX)
+      dist = land1_loc:get_distance(loc2) + loc2:get_distance(dls_loc)
+      APPROACH_DIST_MAX = APPROACH_DIST_MAX * target_dist / dist
+   end
+   gcs:send_text(0, string.format("app_dist=%.2f dist=%.2f target_dist=%.2f", APPROACH_DIST_MAX, dist, target_dist))
+end
+
+function create_pattern(wp, basepos, angle, base_angle, runway_length)
+
+   local jump_target = mission:num_commands() + 3
+
+   wp:command(NAV_WAYPOINT)
+   local loc = loc_copy(basepos)
+   loc:offset_bearing(angle,APPROACH_DIST_MAX)
+   loc:offset_bearing(base_angle,BASE_DIST)
+   wp_add(loc,NAV_WAYPOINT,0,0)
+
+   loc = loc_copy(basepos)
+   loc:offset_bearing(angle,APPROACH_DIST_MAX)
+   wp_add(loc,NAV_WAYPOINT,0,0)
+
+   loc = loc_copy(basepos)
+   loc:offset_bearing(angle,runway_length*1.2)
+   wp_add(loc,NAV_WAYPOINT,0,0)
+
+   loc = loc_copy(basepos)
+   wp_add(loc,NAV_LAND,0,0)
+
+   local step = runway_length*STEP_RATIO
+   NUM_ALTERNATES = math.min(70,math.floor((APPROACH_DIST_MAX - runway_length*1.5) / step))
+
+   gcs:send_text(0, string.format("NUM_ALTERNATES=%u", NUM_ALTERNATES))
+
+   for i = 1, NUM_ALTERNATES do
+      loc = Location()
+      wp_add(loc,DO_JUMP,jump_target,-1)
+
+      loc = loc_copy(basepos)
+      loc:offset_bearing(angle,APPROACH_DIST_MAX - step*i)
+      loc:offset_bearing(base_angle,BASE_DIST)
+      wp_add(loc,NAV_WAYPOINT,0,0)
+
+      loc = loc_copy(basepos)
+      loc:offset_bearing(angle,APPROACH_DIST_MAX - step*i)
+      wp_add(loc,NAV_WAYPOINT,0,0)
+   end
+
+   loc = Location()
+   wp_add(loc,DO_JUMP,jump_target,-1)
+end
+
+-- auto-create mission
+function create_mission()
+   gcs:send_text(0, string.format("creating mission"))
+   local land1_loc = get_location(1)
+   local land2_loc = get_location(2)
+   local dls = mission:get_item(3)
+   local dls_loc = get_location(3)
+   local runway_length = land1_loc:get_distance(land2_loc)
+
+   local alt_agl = (dls_loc:alt() - land1_loc:alt()) * 0.01
+   if alt_agl <= 0 then
+      gcs:send_text(0, string.format("invalid altitudes"))
+      return
+   end
+
+   -- refresh glide slope
+   get_glide_slope()
+
+   local flight_dist1 = dls_loc:get_distance(land1_loc)
+   local flight_dist2 = dls_loc:get_distance(land2_loc)
+   local slope1 = flight_dist1 / alt_agl
+   local slope2 = flight_dist2 / alt_agl
+
+   if slope1 > GLIDE_SLOPE then
+      gcs:send_text(0, string.format("bad glide slope1 %.2f", slope1))
+      return
+   end
+   if slope2 > GLIDE_SLOPE then
+      gcs:send_text(0, string.format("bad glide slope2 %.2f", slope2))
+      return
+   end
+
+
+   local bearing12 = math.deg(land1_loc:get_bearing(land2_loc))
+   local bearing21 = math.deg(land2_loc:get_bearing(land1_loc))
+   local base_angle1 = bearing12 + 90
+   local base_angle2 = bearing12 - 90
+
+   -- work out which base angle brings us closer to the DLS
+   local base_dist1 = loc_shift(land1_loc,base_angle1,10):get_distance(dls_loc)
+   local base_dist2 = loc_shift(land1_loc,base_angle2,10):get_distance(dls_loc)
+   if base_dist1 < base_dist2 then
+      base_angle = base_angle1
+   else
+      base_angle = base_angle2
+   end
+
+   adjust_approach_dist(land1_loc, dls_loc, bearing12)
+   APPROACH_DIST_MAX = math.max(APPROACH_DIST_MAX, 3*runway_length)
+
+   local wp = mission:get_item(1)
+   wp:command(NAV_WAYPOINT)
+
+   mission:clear()
+
+   -- setup home
+   wp_add(dls_loc,NAV_WAYPOINT,0,0)
+
+   -- setup DLS1
+   wp_add(dls_loc,DO_LAND_START,0,0)
+
+   -- first pattern
+   create_pattern(wp, land1_loc, bearing12, base_angle, runway_length)
+
+   adjust_approach_dist(land2_loc, dls_loc, bearing21)
+   APPROACH_DIST_MAX = math.max(APPROACH_DIST_MAX, 3*runway_length)
+
+   -- setup DLS2
+   wp_add(dls_loc,DO_LAND_START,0,0)
+
+   -- second pattern
+   create_pattern(wp, land2_loc, bearing21, base_angle, runway_length)
+
+   fix_WP_heights()
+end
 
 -- init system
 function init()
@@ -537,6 +756,9 @@ function update()
       notify:handle_rgb(255,255,255,10)
       return
    end
+   if not arming:is_armed() and create_mission_check() then
+      create_mission()
+   end
    local t = 0.001 * millis():tofloat()
 
    local state