diff --git a/libraries/AP_Scripting/examples/copter-nav-script-time.lua b/libraries/AP_Scripting/examples/copter-nav-script-time.lua new file mode 100644 index 0000000000..5cd9b6f32a --- /dev/null +++ b/libraries/AP_Scripting/examples/copter-nav-script-time.lua @@ -0,0 +1,90 @@ +-- Copter perform a simple maneuver in response to a NAV_SCRIPT_TIME mission command while the vehicle is in Auto mode +-- +-- Create a simple mission like below +-- TAKEOFF, Alt:10m +-- NAV_SCRIPT_TIME, command=0 (not used in this script), timeout=30 (seconds), arg1=5 (square width in meters), arg2=10 (square height in meters) +-- RETURN-TO-LAUNCH +-- +-- Arm the vehicle (in Loiter mode), switch to Auto and raise the throttle +-- The vehicle should climb to 10m +-- This lua script will be fly the vehicle in square pattern in clockwise direction at the current altitude +-- "arg1" specifies the width (e.g. East-West) in meters +-- "arg2" specifies the height (e.g. North-South) in meters +-- Once the vehicle completes the square or the timeout expires the mission will continue and the vehicle should RTL home + +local running = false +local last_id = -1 -- unique id used to detect if a new NAV_SCRIPT_TIME command has started +local start_loc -- vehicle's location when command starts (South-West corner of square) +local target_loc -- vehicle's target location +local stage = 0 -- stage0: fly North arg2 meters + -- stage1: fly East arg1 meters + -- stage2: fly South arg2 meters + -- stage3: fly West arg2 meters + -- stage4: done +local prev_stage = -1 -- previous stage, used to initate call to move to next corner + +function update() + id, cmd, arg1, arg2 = vehicle:nav_script_time() + if id then + -- handle start of new command + if id ~= last_id then + start_loc = ahrs:get_location() -- initialise south-west corner of square + if start_loc then + running = true + last_id = id + stage = 0 -- start heading North + prev_stage = -1 + else + gcs:send_text(0, "nav-script-time: failed to get location") + running = false + end + end + + -- set waypoint target according to stage + if (running and stage ~= prev_stage) then + prev_stage = stage + local corner_loc = start_loc:copy() -- initialise target location to starting location + if (stage == 0) then + corner_loc:offset(arg2, 0) -- North West corner + end + if (stage == 1) then + corner_loc:offset(arg2, arg1) -- North East corner + end + if (stage == 2) then + corner_loc:offset(0, arg1) -- South East corner + end + -- stage 3 is back to start_loc + if vehicle:set_target_location(corner_loc) then + target_loc = corner_loc + else + gcs:send_text(0, "nav-script-time: failed to set target") + vehicle:nav_script_time_done(last_id) + running = false + end + end + + -- advance stage if we have reached within 1m of target + if running then + local curr_loc = ahrs:get_location() + if curr_loc then + local dist_m = curr_loc:get_distance(target_loc) + if (dist_m < 1.0) then + stage = stage + 1 + if stage == 4 then + vehicle:nav_script_time_done(last_id) + running = false + end + end + else + gcs:send_text(0, "nav-script-time: failed to get location") + end + end + else + -- no active command + running = false + end + + return update, 100 -- update at 10hz +end + +return update()