You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
67 lines
2.1 KiB
67 lines
2.1 KiB
-- 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
|
|
|