From 8ff19357cdb1507659aa6ad5b0acec3d3cc5323f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 7 Mar 2022 23:39:59 +0000 Subject: [PATCH 1/2] AP_Scripting: applets: add motor failure testing sctipt --- .../applets/motor_failure_test.lua | 67 +++++++++++++++++++ .../applets/motor_failure_test.md | 7 ++ 2 files changed, 74 insertions(+) create mode 100644 libraries/AP_Scripting/applets/motor_failure_test.lua create mode 100644 libraries/AP_Scripting/applets/motor_failure_test.md diff --git a/libraries/AP_Scripting/applets/motor_failure_test.lua b/libraries/AP_Scripting/applets/motor_failure_test.lua new file mode 100644 index 0000000000000..64f64440e6075 --- /dev/null +++ b/libraries/AP_Scripting/applets/motor_failure_test.lua @@ -0,0 +1,67 @@ +-- 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 diff --git a/libraries/AP_Scripting/applets/motor_failure_test.md b/libraries/AP_Scripting/applets/motor_failure_test.md new file mode 100644 index 0000000000000..8a749b7506a07 --- /dev/null +++ b/libraries/AP_Scripting/applets/motor_failure_test.md @@ -0,0 +1,7 @@ +# Motor Failure testing Lua script + +This script allows testing failure of motors on copter and quadplane (VTOL only). Vehicles with eight or more motors should be able to fly easily with a single failed motor. Hexacopters can also cope with motor failure if they have sufficient thrust. + +Motor failure is triggered by a RC switch configured to option 300 (Scripting1). Switch low all motors will run, switch high will stop motors. + +Configure which motors stop with the param MOT_STOP_BITMASK, this is added by the script so will only show up once the script is loaded on the SD card. The parameters is a bitmask of motors to stop. A value of 1 will stop motor 1, value of 2 stop motor 2, a value of 3 stops both motors 1 and 2. From 4cf07e6745d5a778a53c3b44ded01e45196d60fb Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 7 Mar 2022 23:40:48 +0000 Subject: [PATCH 2/2] AP_Scripting: applets: add forward flight motor shutdown readme --- .../applets/forward_flight_motor_shutdown.md | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md diff --git a/libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md b/libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md new file mode 100644 index 0000000000000..4055be44bd513 --- /dev/null +++ b/libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md @@ -0,0 +1,9 @@ +# Forward flight motor shutdown script for tailsitters and tiltrotors + +This allows to shutdown selected motors to be stopped once in forward flight for efficiency. + +Set the motors to shutdown in with the `stop_motors` variable. Enable and disable the functionality with a RC switch with options 300 (Scripting1). + +Motors will automatically be shutdown if forward throttle is lower than the value set in `throttle_off_threshold` (50% by default) the motors will then be re-enabled if the throttle goes above the value set in `throttle_on_threshold` (75% by default). + +Time for stopped motors to go from throttle value to 0 and 0 back to throttle can be set with `slew_down_time` and `slew_up_time`.