mirror of https://github.com/ArduPilot/ardupilot
109 lines
3.7 KiB
Lua
109 lines
3.7 KiB
Lua
-- winch-control.lua: allows the winch to be deployed or retracted at a fixed speed using an auxiliary switch
|
|
--
|
|
-- How To Use
|
|
-- 1. set RCx_OPTION to 300 to enable controlling the winch rate from an auxiliary switch
|
|
-- 2. set WINCH_RATE_UP to the fixed retract speed (in m/s)
|
|
-- 3. set WINCH_RATE_DN to the fixed deploy speed (in m/s)
|
|
-- 4. raise the RC auxiliary switch to retract the winch's line
|
|
-- 5. lower the RC auxiliary switch to deploy the winch's line
|
|
-- 6. center the RC auxiliary switch to stop the winch
|
|
-- Alternatively Mission Planner's Aux Function screen can be used in place of an actual RC switch
|
|
--
|
|
|
|
-- global definitions
|
|
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
|
local PARAM_TABLE_KEY = 80
|
|
local PARAM_TABLE_PREFIX = "WINCH_"
|
|
|
|
-- bind a parameter to a variable
|
|
function bind_param(name)
|
|
local p = Parameter()
|
|
assert(p:init(name), string.format("WinchControl: 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("WinchControl: could not add param %s", name))
|
|
return bind_param(PARAM_TABLE_PREFIX .. name)
|
|
end
|
|
|
|
-- setup quicktune specific parameters
|
|
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), "WinchControl: could not add param table")
|
|
|
|
--[[
|
|
// @Param: WINCH_RATE_UP
|
|
// @DisplayName: WinchControl Rate Up
|
|
// @Description: Maximum rate when retracting line
|
|
// @Range: 0.1 5.0
|
|
// @User: Standard
|
|
--]]
|
|
local WINCH_RATE_UP = bind_add_param('RATE_UP', 1, 0.5)
|
|
|
|
--[[
|
|
// @Param: WINCH_RATE_DN
|
|
// @DisplayName: WinchControl Rate Down
|
|
// @Description: Maximum rate when releasing line
|
|
// @Range: 0.1 5.0
|
|
// @User: Standard
|
|
--]]
|
|
local WINCH_RATE_DN = bind_add_param('RATE_DN', 2, 2.0)
|
|
|
|
--[[
|
|
// @Param: WINCH_RC_FUNC
|
|
// @DisplayName: Winch Rate Control RC function
|
|
// @Description: RCn_OPTION number to use to control winch rate
|
|
// @Values: 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8
|
|
// @User: Standard
|
|
--]]
|
|
local WINCH_RC_FUNC = bind_add_param('RC_FUNC', 3, 300)
|
|
|
|
-- local variables and definitions
|
|
local UPDATE_INTERVAL_MS = 100
|
|
local last_rc_switch_pos = -1 -- last known rc switch position. Used to detect change in RC switch position
|
|
|
|
-- initialisation
|
|
gcs:send_text(MAV_SEVERITY.INFO, "WinchControl: started")
|
|
|
|
-- the main update function
|
|
function update()
|
|
|
|
-- get RC switch position
|
|
local rc_switch_pos = rc:get_aux_cached(WINCH_RC_FUNC:get())
|
|
if not rc_switch_pos then
|
|
-- if rc switch has never been set the return immediately
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
|
|
-- initialise RC switch at startup
|
|
if last_rc_switch_pos == -1 then
|
|
last_rc_switch_pos = rc_switch_pos
|
|
end
|
|
|
|
-- check if user has moved RC switch
|
|
if rc_switch_pos == last_rc_switch_pos then
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
last_rc_switch_pos = rc_switch_pos
|
|
|
|
-- set winch rate based on switch position
|
|
if rc_switch_pos == 0 then -- LOW, deploy winch line
|
|
local rate_dn = math.abs(WINCH_RATE_DN:get())
|
|
winch:set_desired_rate(rate_dn)
|
|
gcs:send_text(6, string.format("Winch: lowering at %.1f m/s", rate_dn))
|
|
end
|
|
if rc_switch_pos == 1 then -- MIDDLE, stop winch
|
|
winch:set_desired_rate(0)
|
|
gcs:send_text(6, "Winch: stopped")
|
|
end
|
|
if rc_switch_pos == 2 then -- HIGH, retract winch line
|
|
local rate_up = math.abs(WINCH_RATE_UP:get())
|
|
winch:set_desired_rate(-rate_up)
|
|
gcs:send_text(6, string.format("Winch: raising at %.1f m/s", rate_up))
|
|
end
|
|
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
|
|
return update()
|