AP_Scripting: add forward flight motor shutdown applet

libraries/AP_Scripting/applets/forward_flight_motor_shutdown.lua

@@ -0,0 +1,176 @@
+-- This is a script that stops motors in forward flight, for use on tiltrotors and tailsitters
+-- Thanks to PteroDynamics for supporting the development of this script
+-- Set the motor numbers to stop and configure a RCx_OPTION to 300 to enable and disable
+-- Throttle thresholds also allow automatic enable and disable of stop motors
+-- slew up and down times allow to configure how fast the motors are disabled and re-enabled
+
+-- Config
+
+-- Motors numbers to stop
+local stop_motors = {2,4}
+
+-- motors should be shutdown if throttle goes lower than this value (%), set to > 100 to disable auto enable/disable
+local throttle_off_threshold = 50
+
+-- motors should be re-enabled if throttle higher than this value (%)
+local throttle_on_threshold = 75
+
+local slew_down_time = 5 -- seconds
+local slew_up_time = 1 -- seconds
+
+-- end of config
+
+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 = assert(param:get("Q_M_PWM_MIN"),"Lua: Could not read Q_M_PWM_MIN")
+local pwm_max = assert(param:get("Q_M_PWM_MAX"),"Lua: Could not read Q_M_PWM_MAX")
+
+-- calculate the slew up and down steps to achieve the given slew time
+local pwm_range = pwm_max - pwm_min
+local slew_down = (-pwm_range / slew_down_time) * (10/1000)
+local slew_up = (pwm_range / slew_up_time) * (10/1000)
+
+if throttle_off_threshold < 100 then
+  assert((throttle_off_threshold < throttle_on_threshold) and (throttle_on_threshold < 100), "throttle on and off thresholds not configured correctly")
+end
+
+-- clear vars we don't need anymore
+slew_down_time = nil
+slew_up_time = nil
+pwm_range = nil
+
+
+for i = 1, #stop_motors do
+  -- Check for a valid motor number
+  assert(stop_motors[i] >= 1 and stop_motors[i] <= 12, string.format("Lua: motor %i not valid",stop_motors[i]))
+end
+
+-- find any motors enabled, populate channels to stop and channels to run
+-- run channels provide a average motor PWM to slew down from
+local stop_motor_chan = {}
+local run_motor_fun = {}
+for i = 1, 12 do
+
+  local output_function
+  if i <= 8 then
+    output_function = i+32
+  else
+    output_function = i+81-8
+  end
+
+  local temp_chan = SRV_Channels:find_channel(output_function)
+
+  local should_stop = false
+  for j = 1, #stop_motors do
+    if i == stop_motors[j] then
+      should_stop = true
+      break
+    end
+  end
+
+  if should_stop then
+    assert(temp_chan, string.format("Lua: Could not find motor %i",i))
+    table.insert(stop_motor_chan, temp_chan)
+  elseif temp_chan then
+    table.insert(run_motor_fun, output_function)
+  end
+end
+assert(#stop_motor_chan == #stop_motors, "Lua: could not find all motors to stop")
+assert(#run_motor_fun > 0, "Lua: cannot stop all motors")
+
+-- keep track of last time in a VTOL mode, this allows to delay switching after a transition/assist
+local last_vtol_active = 0
+
+-- current action
+local script_enabled = false
+local motors_disabled = false
+local slew
+local slew_pwm
+function update()
+
+  local switch_pos = switch:get_aux_switch_pos()
+  if switch:get_aux_switch_pos() == 2 then
+    if not script_enabled then
+      gcs:send_text(0, "Lua: Forward flight motor shutdown enabled")
+    end
+    script_enabled = true
+  else
+    if script_enabled then
+      gcs:send_text(0, "Lua: Forward flight motor shutdown disabled")
+    end
+    script_enabled = false
+  end
+
+  if quadplane:in_vtol_mode() or quadplane:in_assisted_flight() or not arming:is_armed() then
+    -- in a VTOL mode, nothing to do
+    last_vtol_active = millis()
+    motors_disabled = false
+    return update, 1000 -- reschedule at 1hz
+  end
+
+  -- script not enabled
+  if not script_enabled then
+    motors_disabled = false
+    return update, 1000 -- reschedule at 1hz
+  end
+
+  -- in forward flight and enabled with switch, if armed then check that we are at least 10s past transition
+  if (millis() - last_vtol_active) < 10000 then
+    -- armed and have not been in a VTOL mode for longer than 10s
+    motors_disabled = false
+    return update, 1000 -- reschedule at 10hz
+  end
+
+  -- check throttle level to see if motors should be disabled or enabled
+  local throttle = SRV_Channels:get_output_scaled(70)
+
+  if motors_disabled and (throttle > throttle_on_threshold) then
+    gcs:send_text(0, "Lua: Throttle high motors enabled")
+    slew = slew_up
+
+  elseif (not motors_disabled) and (throttle < throttle_off_threshold) then
+    slew_pwm = pwm_max
+    motors_disabled = true
+    gcs:send_text(0, "Lua: Throttle low motors disabled")
+    slew = slew_down
+  end
+
+  if not motors_disabled then
+    return update, 10
+  end
+
+  local average_pwm = 0
+  for i = 1, #run_motor_fun do
+    average_pwm = average_pwm + SRV_Channels:get_output_pwm(run_motor_fun[i])
+  end
+  average_pwm = average_pwm / #run_motor_fun
+
+  slew_pwm = slew_pwm + slew
+  if (slew > 0) and (slew_pwm > average_pwm) then
+    -- slewed back up past the output of other motors, stop overriding
+    motors_disabled = false
+    slew_pwm = pwm_max
+    return update, 1000
+  end
+
+  -- never slew lower than min
+  slew_pwm = math.max(slew_pwm, pwm_min)
+
+  -- never output higher than other motors
+  local output_pwm = math.min(slew_pwm, average_pwm)
+
+  -- make sure a integer
+  output_pwm = math.floor(output_pwm + 0.5)
+
+  for i = 1, #stop_motor_chan do
+    -- override for 15ms, should be called every 10ms when active
+    -- 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],output_pwm,15)
+  end
+
+  return update, 10 -- reschedule at 100hz
+end
+
+return update() -- run immediately before starting to reschedule