mirror of https://github.com/ArduPilot/ardupilot
68 lines
2.1 KiB
Lua
68 lines
2.1 KiB
Lua
|
-- This is a script that stops motors in flight, for use testing motor failure handling
|
||
|
|
|
||
|
|
-- add new param MOT_STOP_BITMASK
|
||
|
|
local PARAM_TABLE_KEY = 75
|
||
|
|
assert(param:add_table(PARAM_TABLE_KEY, "MOT_", 1), "could not add param table")
|
||
|
|
assert(param:add_param(PARAM_TABLE_KEY, 1, "STOP_BITMASK", 0), "could not add param")
|
||
|
|
|
||
|
|
local stop_motor_bitmask = Parameter()
|
||
|
|
assert(stop_motor_bitmask:init("MOT_STOP_BITMASK"), "could not find param")
|
||
|
|
|
||
|
|
-- find rc switch with option 300
|
||
|
|
local switch = assert(rc:find_channel_for_option(300),"Lua: Could not find switch")
|
||
|
|
|
||
|
|
-- read spin min param, we set motors to this PWM to stop them
|
||
|
|
local pwm_min
|
||
|
|
if quadplane then
|
||
|
|
pwm_min = assert(param:get("Q_M_PWM_MIN"),"Lua: Could not read Q_M_PWM_MIN")
|
||
|
|
else
|
||
|
|
pwm_min = assert(param:get("MOT_PWM_MIN"),"Lua: Could not read MOT_PWM_MIN")
|
||
|
|
end
|
||
|
|
|
||
|
|
local stop_motor_chan
|
||
|
|
local last_motor_bitmask
|
||
|
|
|
||
|
|
-- find any motors enabled, populate channels numbers to stop
|
||
|
|
local function update_stop_motors(new_bitmask)
|
||
|
|
if last_motor_bitmask == new_bitmask then
|
||
|
|
return
|
||
|
|
end
|
||
|
|
stop_motor_chan = {}
|
||
|
|
for i = 1, 12 do
|
||
|
|
if ((1 << (i-1)) & new_bitmask) ~= 0 then
|
||
|
|
-- convert motor number to output function number
|
||
|
|
local output_function
|
||
|
|
if i <= 8 then
|
||
|
|
output_function = i+32
|
||
|
|
else
|
||
|
|
output_function = i+81-8
|
||
|
|
end
|
||
|
|
|
||
|
|
-- get channel number for output function
|
||
|
|
local temp_chan = SRV_Channels:find_channel(output_function)
|
||
|
|
if temp_chan then
|
||
|
|
table.insert(stop_motor_chan, temp_chan)
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
last_motor_bitmask = new_bitmask
|
||
|
|
end
|
||
|
|
|
||
|
|
function update()
|
||
|
|
|
||
|
|
update_stop_motors(stop_motor_bitmask:get())
|
||
|
|
|
||
|
|
if switch:get_aux_switch_pos() == 2 then
|
||
|
|
for i = 1, #stop_motor_chan do
|
||
|
|
-- override for 15ms, called every 10ms
|
||
|
|
-- using timeout means if the script dies the timeout will expire and all motors will come back
|
||
|
|
-- we cant leave the vehicle in a un-flyable state
|
||
|
|
SRV_Channels:set_output_pwm_chan_timeout(stop_motor_chan[i],pwm_min,15)
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
return update, 10 -- reschedule at 100hz
|
||
|
|
end
|
||
|
|
|
||
|
|
return update() -- run immediately before starting to reschedule
|