From 380f3aece2fc82b56e7b72166600542405386c2f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 10 Aug 2021 17:21:27 +0900 Subject: [PATCH] AP_Scripting: copter-fast-descent.lua example script --- .../examples/copter-fast-descent.lua | 159 ++++++++++++++++++ 1 file changed, 159 insertions(+) create mode 100644 libraries/AP_Scripting/examples/copter-fast-descent.lua diff --git a/libraries/AP_Scripting/examples/copter-fast-descent.lua b/libraries/AP_Scripting/examples/copter-fast-descent.lua new file mode 100644 index 0000000000..fa51dd29f9 --- /dev/null +++ b/libraries/AP_Scripting/examples/copter-fast-descent.lua @@ -0,0 +1,159 @@ +-- Copter descends very rapidly in a spiral pattern to a preset altitude above home +-- +-- CAUTION: This script only works for Copter +-- this script waits for the vehicle to be changed to Guided mode and then: +-- a) flies a spiral pattern using the velocity and acceleration control +-- b) slows the spiral and stops at the preset altitude +-- c) switches to RTL + +-- constants +local copter_guided_mode_num = 4 -- Guided mode is 4 on copter +local copter_rtl_mode_num = 6 -- RTL is 6 on copter +local alt_above_home_min = 50 -- copter will stop at this altitude above home +local circle_radius_rate_max_ms = 1 -- radius expands at max of this many m/s +local circle_radius_accel_mss = 1 -- radius expansion speed accelerates at this many m/s/s +local circle_radius_max = 30 -- target circle's maximum radius +local speed_xy_max = 5 -- max target horizontal speed +local accel_xy = 1 -- horizontal acceleration in m/s^2 +local speed_z_max = 10 -- target descent rate is 15m/s +local accel_z = 1 -- target vertical acceleration is 1m/s/s + +-- timing and state machine variables +local stage = 0 -- stage of descent +local last_update_ms -- system time of last update +local dt = 0.01 -- update rate of script (0.01 = 100hz) +local interval_ms = 1 -- update interval in ms +local last_print_ms = 0 -- pilot update timer + +-- control related variables +local circle_center_pos = Vector3f()-- center of circle position as an offset from EKF origin +local circle_radius = 0 -- target circle's current radius (this is slowly expanded to circle_radius_max) +local circle_radius_rate_ms = 0 -- target circle's radius is increasing at this rate in m/s +local circle_angle_rad = 0 -- current target angle on circle (in radians) +local target_alt_D = 0 -- target altitude in m from EKF origin (Note: down is positive) +local speed_xy = 0 -- target horizontal speed (i.e. tangential velocity or horizontal speed around the circle) +local speed_z = 0 -- target descent rate currently +local target_yaw_deg = 0 -- target yaw in degrees (degrees is more convenient based on interface) + +-- the main update function +function update() + + -- update dt + local now_ms = millis() + if (last_update_ms) then + dt = (now_ms - last_update_ms):tofloat() / 1000.0 + end + if (dt > 1) then + dt = 0 + end + last_update_ms = now_ms + + -- determine if progress update should be sent to user + local update_user = false + if (now_ms - last_print_ms > 5000) then + last_print_ms = now_ms + update_user = true + end + + -- reset stage when disarmed or not in Guided mode + if not arming:is_armed() or (vehicle:get_mode() ~= copter_guided_mode_num) then + stage = 0 + if (update_user) then + gcs:send_text(0, "Fast Descent: waiting for Guided" .. string.format(" dt:%6.4f", dt)) + end + return update, interval_ms + end + + if (stage == 0) then -- Stage0: initialise + local home = ahrs:get_home() + local curr_loc = ahrs:get_position() + if home and curr_loc then + circle_center_pos = ahrs:get_relative_position_NED_origin() + circle_radius_rate_ms = 0 -- reset circle radius expandion rate to zero + circle_radius = 0 -- reset circle radius to zero + circle_angle_rad = ahrs:get_yaw() -- reset starting angle to current heading + target_yaw_deg = math.deg(circle_angle_rad) -- target heading will be kept at original heading + target_alt_D = circle_center_pos:z() -- initialise target alt using current position (Note: down is positive) + speed_xy = 0 + speed_z = 0 + stage = stage + 1 -- advance to next stage + gcs:send_text(0, "Fast Descent: starting") + end + elseif (stage == 1) then -- Stage1: descend + + -- increase circle radius + circle_radius_rate_ms = math.min(circle_radius_rate_ms + (circle_radius_accel_mss * dt), circle_radius_rate_max_ms) -- accelerate radius expansion + circle_radius = math.min(circle_radius + (circle_radius_rate_ms * dt), circle_radius_max) -- increase radius + + -- calculate horizontal and vertical speed + if (circle_radius < circle_radius_max) then + speed_xy = math.max(speed_xy - (accel_xy * dt), 0) -- decelerate horizontal speed to zero + speed_z = math.max(speed_z - (accel_z * dt), 0) -- decelerate vertical speed to zero + else + speed_xy = math.min(speed_xy + (accel_xy * dt), speed_xy_max) -- accelerate horizontal speed to max + speed_z = math.min(speed_z + (accel_z * dt), speed_z_max) -- accelerate to max descent rate + end + + -- calculate angular velocity + local ang_vel_rads = 0 + if (circle_radius >= circle_radius_max) then + ang_vel_rads = speed_xy / circle_radius; + end + + -- increment angular position + circle_angle_rad = circle_angle_rad + (ang_vel_rads * dt) + if (circle_angle_rad >= (math.pi * 2)) then + circle_angle_rad = circle_angle_rad - (math.pi * 2) + end + + -- calculate target position + local cos_ang = math.cos(circle_angle_rad) + local sin_ang = math.sin(circle_angle_rad) + local target_pos = Vector3f() + target_pos:x(circle_center_pos:x() + (circle_radius * cos_ang)) + target_pos:y(circle_center_pos:y() + (circle_radius * sin_ang)) + target_alt_D = target_alt_D + (speed_z * dt) + target_pos:z(target_alt_D) + + -- calculate target velocity + target_vel = Vector3f() + target_vel:x(speed_xy * -sin_ang) + target_vel:y(speed_xy * cos_ang) + target_vel:z(speed_z) + + -- calculate target acceleration + local centrip_accel = 0 + if (circle_radius > 0) then + centrip_accel = speed_xy * speed_xy / circle_radius + end + target_accel = Vector3f() + target_accel:x(centrip_accel * -cos_ang) + target_accel:y(centrip_accel * -sin_ang) + + -- send targets to vehicle with original yaw target + vehicle:set_target_posvelaccel_NED(target_pos, target_vel, target_accel, true, target_yaw_deg, false, 0, false) + + -- advance to stage 2 when below target altitude + local rel_pos_home_NED = ahrs:get_relative_position_NED_home() + if (rel_pos_home_NED) then + if (-rel_pos_home_NED:z() <= alt_above_home_min) then + stage = stage + 1 + end + if (update_user) then + gcs:send_text(0, string.format("Fast Descent: alt:%d target:%d", math.floor(-rel_pos_home_NED:z()), math.floor(alt_above_home_min))) + end + else + gcs:send_text(0, "Fast Descent: lost position estimate, aborting") + stage = stage + 1 + end + + elseif (stage == 2) then -- Stage2: change to RTL mode + vehicle:set_mode(copter_rtl_mode_num) + stage = stage + 1 + gcs:send_text(0, "Fast Descent: done!") + end + + return update, interval_ms +end + +return update()