diff --git a/scripts/droptest.lua b/scripts/droptest.lua index a83778e41cfef1..9941f03d9329eb 100644 --- a/scripts/droptest.lua +++ b/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