From 0846746932f81e75f14dff3aa34e392e4658ea4d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Mar 2020 14:08:53 +0900 Subject: [PATCH] AP_Scripting: add copter-fly-vertical-circle example script --- .../examples/copter-fly-vertical-circle.lua | 78 +++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 libraries/AP_Scripting/examples/copter-fly-vertical-circle.lua diff --git a/libraries/AP_Scripting/examples/copter-fly-vertical-circle.lua b/libraries/AP_Scripting/examples/copter-fly-vertical-circle.lua new file mode 100644 index 0000000000..5d26ef7e4e --- /dev/null +++ b/libraries/AP_Scripting/examples/copter-fly-vertical-circle.lua @@ -0,0 +1,78 @@ +-- command a Copter to takeoff to 5m and fly a vertical circle in the clockwise direction +-- +-- CAUTION: This script only works for Copter +-- this script waits for the vehicle to be armed and RC6 input > 1800 and then: +-- a) switches to Guided mode +-- b) takeoff to 5m +-- c) flies a vertical circle using the velocity controller +-- d) switches to RTL mode + +local takeoff_alt_above_home = 5 +local copter_guided_mode_num = 4 +local copter_rtl_mode_num = 6 +local stage = 0 +local circle_angle = 0 +local circle_angle_increment = 1 -- increment the target angle by 1 deg every 0.1 sec (i.e. 10deg/sec) +local circle_speed = 1 -- velocity is always 1m/s +local yaw_cos = 0 -- cosine of yaw at takeoff +local yaw_sin = 0 -- sine of yaw at takeoff + +-- the main update function that uses the takeoff and velocity controllers to fly a rough square pattern +function update() + if not arming:is_armed() then -- reset state when disarmed + stage = 0 + circle_angle = 0 + else + pwm6 = rc:get_pwm(6) + if pwm6 and pwm6 > 1800 then -- check if RC6 input has moved high + if (stage == 0) then -- change to guided mode + if (vehicle:set_mode(copter_guided_mode_num)) then -- change to Guided mode + local yaw_rad = ahrs:get_yaw() + yaw_cos = math.cos(yaw_rad) + yaw_sin = math.sin(yaw_rad) + stage = stage + 1 + end + elseif (stage == 1) then -- Stage1: takeoff + if (vehicle:start_takeoff(takeoff_alt_above_home)) then + stage = stage + 1 + end + elseif (stage == 2) then -- Stage2: check if vehicle has reached target altitude + local home = ahrs:get_home() + local curr_loc = ahrs:get_position() + if home and curr_loc then + local vec_from_home = home:get_distance_NED(curr_loc) + gcs:send_text(0, "alt above home: " .. tostring(math.floor(-vec_from_home:z()))) + if (math.abs(takeoff_alt_above_home + vec_from_home:z()) < 1) then + stage = stage + 1 + bottom_left_loc = curr_loc -- record location when starting square + end + end + elseif (stage == 3 ) then -- Stage3: fly a vertical circle + + -- calculate velocity vector + circle_angle = circle_angle + circle_angle_increment + if (circle_angle >= 360) then + stage = stage + 1 + end + local target_vel = Vector3f() + local vel_xy = math.cos(math.rad(circle_angle)) * circle_speed + target_vel:x(yaw_sin * vel_xy) + target_vel:y(yaw_cos * -vel_xy) + target_vel:z(-math.sin(math.rad(circle_angle)) * circle_speed) + + -- send velocity request + if not (vehicle:set_target_velocity_NED(target_vel)) then + gcs:send_text(0, "failed to execute velocity command") + end + elseif (stage == 4) then -- Stage4: change to RTL mode + vehicle:set_mode(copter_rtl_mode_num) + stage = stage + 1 + gcs:send_text(0, "finished square, switching to RTL") + end + end + end + + return update, 100 +end + +return update()