diff --git a/libraries/AP_Scripting/applets/plane_package_place.lua b/libraries/AP_Scripting/applets/plane_package_place.lua new file mode 100644 index 0000000000..b5d7da1f5a --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_package_place.lua @@ -0,0 +1,186 @@ +--[[ + support package place for quadplanes +--]] + +local PARAM_TABLE_KEY = 9 +local PARAM_TABLE_PREFIX = "PKG_" + +local MODE_AUTO = 10 + +local NAV_TAKEOFF = 22 +local NAV_VTOL_PAYLOAD_PLACE = 94 + +-- 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 Parameter(PARAM_TABLE_PREFIX .. name) +end + +-- setup package place specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 5), 'could not add param table') +local PKG_ENABLE = bind_add_param('ENABLE', 1, 0) +local PKG_RELEASE_FUNC = bind_add_param('RELEASE_FUNC', 2, 94) +local PKG_RELEASE_HGT = bind_add_param('RELEASE_HGT', 3, 10) +local PKG_RELEASE_HOLD = bind_add_param('RELEASE_HOLD', 4, 1) + +local Q_LAND_SPEED = Parameter("Q_LAND_SPEED") +local Q_LAND_FINAL_ALT = Parameter("Q_LAND_FINAL_ALT") + +local MAV_SEVERITY_INFO = 6 +local MAV_SEVERITY_NOTICE = 5 +local MAV_SEVERITY_EMERGENCY = 0 + +local RNG_ORIENT_DOWN = 25 + +-- motors state +local MOTORS_SHUT_DOWN = 0 + +-- release state +local RELEASE_NONE = 0 +local RELEASE_DESCENT = 1 +local RELEASE_HOLD1 = 2 +local RELEASE_HOLD2 = 3 +local RELEASE_DONE = 4 +local release_state = RELEASE_NONE + +local release_start_t = 0 + +if PKG_ENABLE:get() ~= 1 then + -- not enabled + return +end + +SRV_Channels:set_range(PKG_RELEASE_FUNC:get(), 1000) +SRV_Channels:set_output_scaled(PKG_RELEASE_FUNC:get(), 0) + +-- get time in seconds +function get_time() + return millis():tofloat() * 0.001 +end + +-- reset state +function reset() + release_state = RELEASE_NONE + SRV_Channels:set_output_scaled(PKG_RELEASE_FUNC:get(), 0) +end + +--[[ + main update function, called at 20Hz +--]] +function update() + if PKG_ENABLE:get() ~= 1 then + -- not enabled + return + end + + -- only do something if in AUTO and in a PACKAGE_PLACE waypoint + if vehicle:get_mode() ~= MODE_AUTO or mission:get_current_nav_id() ~= NAV_VTOL_PAYLOAD_PLACE then + reset() + return + end + + if not arming:is_armed() then + -- nothing to do when disarmed + reset() + return + end + + -- check spool state for if we are landed + local landed = false + if motors:get_desired_spool_state() == MOTORS_SHUT_DOWN and release_state >= RELEASE_DESCENT then + landed = true + end + + -- wait till we are in the descent + if not quadplane:in_vtol_land_descent() and release_state == RELEASE_NONE then + reset() + return + end + + if release_state == RELEASE_NONE then + -- we have started the descent + release_state = RELEASE_DESCENT + end + + -- see if we have valid rangefinder data + if not landed and not rangefinder:has_data_orient(RNG_ORIENT_DOWN) then + return + end + + -- check the distance, if less than RNG_ORIENT_DOWN then release + local dist_m + if not landed then + dist_m = rangefinder:distance_cm_orient(RNG_ORIENT_DOWN) * 0.01 + else + dist_m = 0.0 + end + + -- slow down when within Q_LAND_FINAL_ALT of target + local remaining_m = dist_m - PKG_RELEASE_HGT:get() + if remaining_m > 0 and remaining_m < Q_LAND_FINAL_ALT:get() then + vehicle:set_land_descent_rate(Q_LAND_SPEED:get()*0.01) + end + + if remaining_m <= 0 then + if PKG_RELEASE_HGT:get() <= 0 and not landed then + -- wait for landing + return + end + + local now = get_time() + -- we are at the target altitude + if release_state == RELEASE_DESCENT then + -- start timer + release_start_t = now + release_state = RELEASE_HOLD1 + vehicle:set_land_descent_rate(0) + elseif release_state == RELEASE_HOLD1 then + -- start waiting for the hold time for vehicle to settle + vehicle:set_land_descent_rate(0) + if now - release_start_t > PKG_RELEASE_HOLD:get() then + gcs:send_text(MAV_SEVERITY_INFO, string.format("Package released at %.1fm", dist_m)) + SRV_Channels:set_output_scaled(PKG_RELEASE_FUNC:get(), 1000) + release_state = RELEASE_HOLD2 + end + elseif release_state == RELEASE_HOLD2 then + -- do the release and wait for hold time again to ensure clean release + vehicle:set_land_descent_rate(0) + if now - release_start_t > PKG_RELEASE_HOLD:get()*2 then + release_state = RELEASE_DONE + -- aborting the landing causes us to climb back up and continue the mission + if quadplane:abort_landing() then + gcs:send_text(MAV_SEVERITY_INFO, string.format("Climbing")) + else + gcs:send_text(MAV_SEVERITY_NOTICE, string.format("land abort failed")) + end + end + end + end + +end + +function loop() + update() + -- run at 20Hz + return loop, 50 +end + +-- wrapper around update(). This calls update() at 20Hz, +-- and if update faults then an error is displayed, but the script is not +-- stopped +function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(0, "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, 50 +end + +gcs:send_text(MAV_SEVERITY_INFO, "Loaded package place script") + +-- start running update loop +return protected_wrapper() + diff --git a/libraries/AP_Scripting/applets/plane_package_place.md b/libraries/AP_Scripting/applets/plane_package_place.md new file mode 100644 index 0000000000..77fb0d1892 --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_package_place.md @@ -0,0 +1,65 @@ +# Quadplane Package Place Support + +This script implements support for package place in quadplanes. + +To use this script you need to install it in the APM/scripts folder on +your microSD card (or build it into the firmware in ROMFS). Then +enable scripting with SCR_ENABLE=1 and reboot. + +# Parameters + +The script adds the following parameters: + +## PKG_ENABLE + +You need to set PKG_ENABLE=1 to enable this script + +## PKG_RELEASE_FUNC + +This needs to be set to the SERVOn_FUNCTION of the release servo. It +is recommended that you leave it at the default of 94 and set +SERVOn_FUNCTION to 94 for the servo you want to use for package +release. + +## PKG_RELEASE_HGT + +The parameter PKG_RELEASE_HGT controls the rangefinder height at which +the package will be released. This can be zero if you want to release +the package after you land. + +## PKG_RELEASE_HOLD + +This controls the time that the vehicle will stop the descent before +it releases the package. This defaults to 1 second and is used to let +the vehicle get into a steady hover. + +After package release the vehicle will hold for another +PKG_RELEASE_HOLD seconds to let the package cleanly release before the +vehicle climbs. + +# Operation + +To setup a mission for package place you should setup your vehicle for +rangefinder landings. Setup a good lidar or radar and test that it +works. Then set RNGFND_LANDING=1 to enable use of rangefinder for VTOL +landing. + +In the mission you should add a PAYLOAD_PLACE waypoint at the desired +location of the payload placement. The altitude of the PAYLOAD_PLACE +command should be set to zero as a relative height. + +The PAYLOAD_PLACE command has a parametere "max descent" (parameter 1 +of the command). If this is non-zero then this is the maximum amount +of height that the aircraft will descend from the start of the +descent. If the aircraft tries to descend more than this height then +the payload place will abort and the aircraft will climb back up to +the initial descent height then continue the mission. + +# Landing Then Release + +You can also do payload place where you wait till the vehicle fully +lands before doing the release. To do that set PGK_RELEASE_HGT to 0 +and the aircraft will land fully and shutdown the motors for +PKG_RELEASE_HOLD seconds before releasing the servo. It will then hold +for another PKG_RELEASE_HOLD seconds, then will climb back up to the +original descent start height before continuing with the mission.