diff --git a/libraries/AP_Scripting/examples/copter-fast-descent.lua b/libraries/AP_Scripting/examples/copter-fast-descent.lua index fafaa0cf14..fa7a42ff22 100644 --- a/libraries/AP_Scripting/examples/copter-fast-descent.lua +++ b/libraries/AP_Scripting/examples/copter-fast-descent.lua @@ -1,6 +1,6 @@ -- Copter descends very rapidly in a spiral pattern to a preset altitude above home -- --- CAUTION: This script only works for Copter +-- CAUTION: This script only works for Copter 4.2 (and higher) -- 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 @@ -9,13 +9,9 @@ -- 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 @@ -24,6 +20,7 @@ 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 +local auto_last_id = -1 -- unique id used to detect if a new NAV_SCRIPT_TIME command has started -- control related variables local circle_center_pos = Vector3f()-- center of circle position as an offset from EKF origin @@ -35,6 +32,29 @@ local speed_xy = 0 -- target horizontal speed (i.e. tangential local speed_z = 0 -- target descent rate currently local target_yaw_deg = 0 -- target yaw in degrees (degrees is more convenient based on interface) +-- create and initialise parameters +local PARAM_TABLE_KEY = 75 -- parameter table key must be used by only one script on a particular flight controller +assert(param:add_table(PARAM_TABLE_KEY, "FDST_", 6), 'could not add param table') +assert(param:add_param(PARAM_TABLE_KEY, 1, 'ACTIVATE', 0), 'could not add FDST_ACTIVATE param') -- 0:active in Guided, 1:active in Auto's NAV_SCRIPT_TIME command +assert(param:add_param(PARAM_TABLE_KEY, 2, 'ALT_MIN', 50), 'could not add FDST_ALT_MIN param') -- copter will stop at this altitude above home +assert(param:add_param(PARAM_TABLE_KEY, 3, 'RADIUS', 30), 'could not add FDST_RADIUS parameter') -- target circle's maximum radius +assert(param:add_param(PARAM_TABLE_KEY, 4, 'SPEED_XY', 5), 'could not add FDST_SPEED_XY param') -- max target horizontal speed +assert(param:add_param(PARAM_TABLE_KEY, 5, 'SPEED_DN', 10), 'could not add FDST_SPEED_DN param') -- target descent rate +assert(param:add_param(PARAM_TABLE_KEY, 6, 'YAW_BEHAVE', 0), 'could not add FDST_YAW_BEHAVE param') -- 0:yaw does not change 1:yaw points toward center + +-- bind parameters to variables +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end +local activate_type = bind_param("FDST_ACTIVATE") -- activate type 0:Guided, 1:Auto's NAV_SCRIPT_TIME +local alt_above_home_min = bind_param("FDST_ALT_MIN") -- copter will stop at this altitude above home +local circle_radius_max = bind_param("FDST_RADIUS") -- target circle's maximum radius +local speed_xy_max = bind_param("FDST_SPEED_XY") -- max target horizontal speed +local speed_z_max = bind_param("FDST_SPEED_DN") -- target descent rate +local yaw_behave = bind_param("FDST_YAW_BEHAVE") -- 0:yaw is static, 1:yaw points towards center of circle + -- the main update function function update() @@ -55,13 +75,26 @@ function update() 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)) + -- reset stage until activated in Guided or Auto mode + if (activate_type:get() == 0) then + -- activate_type 0: 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") + end + return update, interval_ms + end + else + -- activate_type 1: reset stage when disarmed or not in Auto executing NAV_SCRIPT_TIME command + auto_last_id, cmd, arg1, arg2 = vehicle:nav_script_time() + if not arming:is_armed() or not auto_last_id then + stage = 0 + if (update_user) then + gcs:send_text(0, "Fast Descent: waiting for NAV_SCRIPT_TIME") + end + return update, interval_ms end - return update, interval_ms end if (stage == 0) then -- Stage0: initialise @@ -71,8 +104,17 @@ function update() 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 + if yaw_behave:get() == 0 then + -- yaw does not move so reset starting angle to current heading + circle_angle_rad = ahrs:get_yaw() + else + -- yaw points towards center so start 180deg behind vehicle + circle_angle_rad = ahrs:get_yaw() + math.pi + if (circle_angle_rad >= (math.pi * 2)) then + circle_angle_rad = circle_angle_rad - (math.pi * 2) + end + end + target_yaw_deg = math.deg(ahrs:get_yaw()) -- 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 @@ -83,20 +125,20 @@ function update() -- 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 + circle_radius = math.min(circle_radius + (circle_radius_rate_ms * dt), circle_radius_max:get()) -- increase radius -- calculate horizontal and vertical speed - if (circle_radius < circle_radius_max) then + if (circle_radius < circle_radius_max:get()) 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 + speed_xy = math.min(speed_xy + (accel_xy * dt), speed_xy_max:get()) -- accelerate horizontal speed to max + speed_z = math.min(speed_z + (accel_z * dt), speed_z_max:get()) -- accelerate to max descent rate end -- calculate angular velocity local ang_vel_rads = 0 - if (circle_radius >= circle_radius_max) then + if (circle_radius >= circle_radius_max:get()) then ang_vel_rads = speed_xy / circle_radius; end @@ -130,27 +172,41 @@ function update() target_accel:x(centrip_accel * -cos_ang) target_accel:y(centrip_accel * -sin_ang) - -- send targets to vehicle with original yaw target + -- calculate target yaw + if yaw_behave:get() == 1 then + target_yaw_deg = math.deg(circle_angle_rad + math.pi) + if target_yaw_deg > 360 then + target_yaw_deg = target_yaw_deg - 360 + end + end + + -- send targets to vehicle with 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 + if (-rel_pos_home_NED:z() <= alt_above_home_min:get()) 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))) + 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:get()))) 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) + elseif (stage == 2) then -- Stage2: done! stage = stage + 1 gcs:send_text(0, "Fast Descent: done!") + if (activate_type:get() == 0) then + -- if activated from Guided change to RTL mode + vehicle:set_mode(copter_rtl_mode_num) + else + -- if activated from Auto NAV_SCRIPT_TIME then mark command as done + vehicle:nav_script_time_done(auto_last_id) + end end return update, interval_ms