diff --git a/libraries/AP_Scripting/examples/copter_deploy.lua b/libraries/AP_Scripting/examples/copter_deploy.lua new file mode 100644 index 0000000000000..19efeec619651 --- /dev/null +++ b/libraries/AP_Scripting/examples/copter_deploy.lua @@ -0,0 +1,84 @@ +--[[ + script to auto-deploy a vehicle on descent after reaching a specified altitude + uses raw pressure to not depend on either GPS or on home alt +--]] +local PARAM_TABLE_KEY = 72 +local PARAM_TABLE_PREFIX = "DEPL_" +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +---@diagnostic disable: missing-parameter +---@diagnostic disable: param-type-mismatch + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +-- 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 bind_param(PARAM_TABLE_PREFIX .. name) +end + +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table') + +local end_pos = bind_add_param('OPEN_POS', 1, 1200) +local offset = bind_add_param('OPEN_OFFSET', 2, 400) +local deployment_alt = bind_add_param('ALT', 3, 1) +local DEPL_CLIMB_ALT = bind_add_param('CLIMB_ALT', 4, 2) +local base_pressure = nil +local SERVO_FUNCTION = 94 +local reached_climb_alt = false +local deployed = false + +local MODE_LAND = 9 + +function update_state() + local pressure = baro:get_pressure() + if not pressure then + return + end + if not base_pressure then + base_pressure = pressure + end + local altitude = baro:get_altitude_difference(base_pressure, pressure) + if not altitude then + return + end + gcs:send_named_float('DALT',altitude) + logger.write("DEPL",'BP,P,Alt','fff', + base_pressure, pressure, altitude) + if not reached_climb_alt then + local start_pos = end_pos:get() + offset:get() + SRV_Channels:set_output_pwm(SERVO_FUNCTION, start_pos) + if altitude > DEPL_CLIMB_ALT:get() then + reached_climb_alt = true + gcs:send_text(MAV_SEVERITY.ERROR, "DEPL: Reached climb alt") + end + return + end + if deployed then + return + end + if altitude > deployment_alt:get() then + return + end + deployed = true + gcs:send_text(MAV_SEVERITY.INFO, "DEPL: deploying") + vehicle:set_mode(MODE_LAND) + arming:arm_force() + if not vehicle:get_mode() == MODE_LAND or not arming:is_armed() then + gcs:send_text(MAV_SEVERITY.INFO, "DEPL: Arming failed") + return + end + --SRV_Channels:set_output_pwm(SERVO_FUNCTION, end_pos:get()) + gcs:send_text(MAV_SEVERITY.INFO, "DEPL: Deployed successfully") +end +function update() -- this is the loop which periodically runs + update_state() + return update, 20 -- Reschedules the loop at 50Hz +end +gcs:send_text(MAV_SEVERITY.INFO, "DEPL: loaded") +return update() -- Run immediately before starting to reschedule