From 3dca7f08b78c79916f43882d30691d5ce080a062 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Jan 2022 17:32:03 +1100 Subject: [PATCH] AP_Scripting: plane ship landing script --- .../examples/plane_ship_landing.lua | 439 ++++++++++++++++++ 1 file changed, 439 insertions(+) create mode 100644 libraries/AP_Scripting/examples/plane_ship_landing.lua diff --git a/libraries/AP_Scripting/examples/plane_ship_landing.lua b/libraries/AP_Scripting/examples/plane_ship_landing.lua new file mode 100644 index 0000000000..8820977277 --- /dev/null +++ b/libraries/AP_Scripting/examples/plane_ship_landing.lua @@ -0,0 +1,439 @@ +-- support takeoff and landing on moving platforms for VTOL planes + +local PARAM_TABLE_KEY = 7 +local PARAM_TABLE_PREFIX = "SHIP_" + +local MODE_MANUAL = 0 +local MODE_RTL = 11 +local MODE_QRTL = 21 +local MODE_AUTO = 10 +local MODE_QLOITER = 19 + +local NAV_TAKEOFF = 22 +local NAV_VTOL_TAKEOFF = 84 + +local ALT_FRAME_ABSOLUTE = 0 + +-- 3 throttle position +local THROTTLE_LOW = 0 +local THROTTLE_MID = 1 +local THROTTLE_HIGH = 2 + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- setup SHIP specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table') +SHIP_ENABLE = bind_add_param('ENABLE', 1, 0) +SHIP_LAND_ANGLE = bind_add_param('LAND_ANGLE', 2, 0) +SHIP_AUTO_OFS = bind_add_param('AUTO_OFS', 3, 0) + +-- other parameters +RCMAP_THROTTLE = bind_param("RCMAP_THROTTLE") +ALT_HOLD_RTL = bind_param("ALT_HOLD_RTL") +Q_RTL_ALT = bind_param("Q_RTL_ALT") +TRIM_ARSPD_CM = bind_param("TRIM_ARSPD_CM") +TECS_LAND_ARSPD = bind_param("TECS_LAND_ARSPD") +Q_TRANS_DECEL = bind_param("Q_TRANS_DECEL") +WP_LOITER_RAD = bind_param("WP_LOITER_RAD") +FOLL_OFS_X = bind_param("FOLL_OFS_X") +FOLL_OFS_Y = bind_param("FOLL_OFS_Y") +FOLL_OFS_Z = bind_param("FOLL_OFS_Z") + +-- an auth ID to disallow arming when we don't have the beacon +local auth_id = arming:get_aux_auth_id() +arming:set_aux_auth_failed(auth_id, "Ship: no beacon") + +-- current target +local target_pos = Location() +local current_pos = Location() +local target_velocity = Vector3f() +local target_heading = 0.0 + +-- landing stages +local STAGE_HOLDOFF = 0 +local STAGE_DESCEND = 1 +local STAGE_APPROACH = 2 +local STAGE_IDLE = 2 +local landing_stage = STAGE_HOLDOFF + +-- other state +local vehicle_mode = MODE_MANUAL +local reached_alt = false +local throttle_pos = THROTTLE_HIGH +local have_target = false + +-- square a variable +function sq(v) + return v*v +end + +-- return time since boot in seconds +function time_seconds() + return millis():tofloat() * 0.001 +end + +--[[ + parameter values which are auto-set on startup +--]] +local key_params = { + FOLL_ENABLE = 1, + FOLL_OFS_TYPE = 1, + FOLL_ALT_TYPE = 0, +} + +-- check key parameters +function check_parameters() + for p, v in pairs(key_params) do + local current = param:get(p) + if not current then + gcs:send_text(0,string.format("Parameter %s not found", p)) + end + if math.abs(v-current) > 0.001 then + param:set_and_save(p, v) + gcs:send_text(0,string.format("Parameter %s set to %.2f was %.2f", p, v, current)) + end + end +end + +-- update the pilots throttle position +function update_throttle_pos() + local tpos + if not rc:has_valid_input() then + tpos = THROTTLE_LOW + else + local tchan = rc:get_channel(RCMAP_THROTTLE:get()) + local tval = (tchan:norm_input_ignore_trim()+1.0)*0.5 + if tval >= 0.40 then + tpos = THROTTLE_HIGH + elseif tval >= 0.1 then + tpos = THROTTLE_MID + else + tpos = THROTTLE_LOW + end + end + if tpos ~= throttle_pos then + reached_alt = false + if landing_stage == STAGE_HOLDOFF and tpos <= THROTTLE_MID then + landing_stage = STAGE_DESCEND + gcs:send_text(0, string.format("Descending for approach (hd=%.1fm h=%.1f th=%.1f)", + get_holdoff_distance(), current_pos:alt()*0.01, get_target_alt())) + end + if landing_stage == STAGE_DESCEND and tpos == THROTTLE_HIGH then + gcs:send_text(0,"Climbing for holdoff") + landing_stage = STAGE_HOLDOFF + end + end + throttle_pos = tpos +end + +-- get landing airspeed +function get_land_airspeed() + if TECS_LAND_ARSPD:get() < 0 then + return TRIM_ARSPD_CM:get() * 0.01 + end + return TECS_LAND_ARSPD:get() +end + +--[[ + calculate stopping distance assuming we are flying at + TECS_LAND_ARSPD and are approaching the landing target from + behind. Take account of the wind estimate to get approach + groundspeed +--]] +function stopping_distance() + -- get the target true airspeed for approach + local tas = get_land_airspeed() * ahrs:get_EAS2TAS() + + -- add in wind in direction of flight + local wind = ahrs:wind_estimate():xy() + + -- rotate wind to be in approach frame + wind:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get())) + + -- ship velocity rotated to the approach frame + local ship2d = target_velocity:xy() + ship2d:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get())) + + -- calculate closing speed + -- use pythagoras theorem to solve for the wind triangle + local tas_sq = sq(tas) + local y_sq = sq(wind:y()) + local closing_speed + if tas_sq >= y_sq then + closing_speed = math.sqrt(tas_sq - y_sq) + else + -- min 1 m/s + closing_speed = 1.0 + end + + -- include the wind in the direction of the ship + closing_speed = closing_speed + wind:x() + + -- account for the ship velocity + closing_speed = closing_speed - ship2d:x() + + -- calculate stopping distance + return sq(closing_speed) / (2.0 * Q_TRANS_DECEL:get()) +end + +-- get holdoff distance +function get_holdoff_distance() + local radius = WP_LOITER_RAD:get() + local holdoff_dist = math.abs(radius*1.5) + local stop_distance = stopping_distance() + + -- increase holdoff distance by up to 50% to ensure we can stop + holdoff_dist = math.max(holdoff_dist, math.min(holdoff_dist*2.5, stop_distance*2)) + return holdoff_dist +end + +-- get the holdoff position +function get_holdoff_position() + local radius = WP_LOITER_RAD:get() + local heading_deg = target_heading + SHIP_LAND_ANGLE:get() + local holdoff_dist = get_holdoff_distance() + + local ofs = Vector2f() + ofs:x(-holdoff_dist) + ofs:y(radius) + ofs:rotate(math.rad(heading_deg)) + local target = target_pos:copy() + target:offset(ofs:x(), ofs:y()) + return target +end + +function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +--[[ + check if we have reached the tangent to the landing location +--]] +function check_approach_tangent() + local distance = current_pos:get_distance(target_pos) + local holdoff_dist = get_holdoff_distance() + if landing_stage == STAGE_HOLDOFF and throttle_pos <= THROTTLE_MID and distance < 4*holdoff_dist then + gcs:send_text(0, string.format("Descending for approach (hd=%.1fm)", holdoff_dist)) + landing_stage = STAGE_DESCEND + end + if reached_alt and landing_stage == STAGE_DESCEND then + -- go to approach stage when throttle is low, we are + -- pointing at the ship and have reached target alt. + -- Also require we are within 2.5 radius of the ship, and our heading is within 20 + -- degrees of the target heading + local target_bearing_deg = wrap_180(math.deg(current_pos:get_bearing(target_pos))) + local ground_bearing_deg = wrap_180(math.deg(ahrs:groundspeed_vector():angle())) + local margin = 10 + local distance = current_pos:get_distance(target_pos) + local holdoff_dist = get_holdoff_distance() + local error1 = math.abs(wrap_180(target_bearing_deg - ground_bearing_deg)) + local error2 = math.abs(wrap_180(ground_bearing_deg - (target_heading + SHIP_LAND_ANGLE:get()))) + logger.write('SLND','TBrg,GBrg,Dist,HDist,Err1,Err2','ffffff',target_bearing_deg, ground_bearing_deg, distance, holdoff_dist, error1, error2) + if (error1 < margin and + distance < 2.5*holdoff_dist and + distance > 0.7*holdoff_dist and + error2 < 2*margin) then + -- we are on the tangent, switch to QRTL + gcs:send_text(0, "Starting approach") + landing_stage = STAGE_APPROACH + vehicle:set_mode(MODE_QRTL) + end + end +end + +--[[ + check if we should abort a QRTL landing +--]] +function check_approach_abort() + local alt = current_pos:alt() * 0.01 + local target_alt = get_target_alt() + if alt > target_alt then + gcs:send_text(0, "Aborting landing") + landing_stage = STAGE_HOLDOFF + vehicle:set_mode(MODE_RTL) + end +end + +-- update state based on vehicle mode +function update_mode() + local mode = vehicle:get_mode() + if mode == vehicle_mode then + return + end + vehicle_mode = mode + if mode == MODE_RTL then + landing_stage = STAGE_HOLDOFF + reached_alt = false + elseif mode ~= MODE_QRTL then + landing_stage = STAGE_IDLE + reached_alt = false + end +end + +-- update target state +function update_target() + if not follow:have_target() then + if have_target then + gcs:send_text(0,"Lost beacon") + arming:set_aux_auth_failed(auth_id, "Ship: no beacon") + end + have_target = false + end + if not have_target then + gcs:send_text(0,"Have beacon") + arming:set_aux_auth_passed(auth_id) + end + have_target = true + + target_pos, target_velocity = follow:get_target_location_and_velocity_ofs() + target_heading = follow:get_target_heading_deg() + -- zero vertical velocity to reduce impact of ship movement + target_velocity:z(0) +end + +-- get the alt target for holdoff, AMSL +function get_target_alt() + local base_alt = target_pos:alt() * 0.01 + if landing_stage == STAGE_HOLDOFF then + return base_alt + ALT_HOLD_RTL:get() * 0.01 + end + return base_alt + Q_RTL_ALT:get() +end + +function update_alt() + local alt = current_pos:alt() * 0.01 + local target_alt = get_target_alt() + if landing_stage == STAGE_HOLDOFF or landing_stage == STAGE_DESCEND then + if math.abs(alt - target_alt) < 3 then + if not reached_alt then + gcs:send_text(0,"Reached target altitude") + end + reached_alt = true + end + end +end + +--[[ + update automatic beacon offsets +--]] + +function update_auto_offset() + if arming:is_armed() or math.floor(SHIP_AUTO_OFS:get()) ~= 1 then + return + end + + -- get target without offsets applied + target_no_ofs, vel = follow:get_target_location_and_velocity() + + -- setup offsets so target location will be current location + local new = target_no_ofs:get_distance_NED(current_pos) + new:rotate_xy(-math.rad(target_heading)) + + gcs:send_text(0,string.format("Set follow offset (%.2f,%.2f,%.2f)", new:x(), new:y(), new:z())) + FOLL_OFS_X:set_and_save(new:x()) + FOLL_OFS_Y:set_and_save(new:y()) + FOLL_OFS_Z:set_and_save(new:z()) + + SHIP_AUTO_OFS:set_and_save(0) +end + +-- main update function +function update() + if SHIP_ENABLE:get() < 1 then + return + end + if not follow:have_target() then + return + end + + update_target() + if not have_target then + return + end + + current_pos = ahrs:get_position() + if not current_pos then + return + end + current_pos:change_alt_frame(ALT_FRAME_ABSOLUTE) + + update_throttle_pos() + update_mode() + update_alt() + update_auto_offset() + + ahrs:set_home(target_pos) + + local next_WP = vehicle:get_target_location() + if not next_WP then + -- not in a flight mode with a target location + return + end + + if vehicle_mode == MODE_RTL then + local holdoff_pos = get_holdoff_position() + holdoff_pos:change_alt_frame(ALT_FRAME_ABSOLUTE) + holdoff_pos:alt(math.floor(get_target_alt()*100)) + vehicle:update_target_location(next_WP, holdoff_pos) + + if throttle_pos == THROTTLE_LOW then + check_approach_tangent() + end + + elseif vehicle_mode == MODE_QRTL then + vehicle:set_velocity_match(target_velocity:xy()) + target_pos:alt(next_WP:alt()) + vehicle:update_target_location(next_WP, target_pos) + + if throttle_pos == THROTTLE_HIGH then + check_approach_abort() + end + + elseif vehicle_mode == MODE_AUTO then + local id = mission:get_current_nav_id() + if id == NAV_VTOL_TAKEOFF or id == NAV_TAKEOFF then + vehicle:set_velocity_match(target_velocity:xy()) + local tpos = current_pos:copy() + tpos:alt(next_WP:alt()) + vehicle:update_target_location(next_WP, tpos) + end + + elseif vehicle_mode == MODE_QLOITER then + vehicle:set_velocity_match(target_velocity:xy()) + end + +end + +function loop() + update() + -- run at 20Hz + return loop, 50 +end + +check_parameters() + +-- start running update loop +return loop()