So with the help of @iampete from this thread, I have made a start at wrighting the proposed script. Sadly I keep failing at the point where I run MotorsMatrix:init(8)
. I’m a little stuck here, since I haven’t been able to run any of the MotorMatrix
example scripts from github.
My assumption is that some of my configuration parameters are incompatable with what I’m trying to do, but figureing this out with trail and error seems like it will he quite the pita. I’m hoping that someone with some more lua/ardupilot experience might glance over what I have and point me toward a probable culprit. The script so far is as follows:
-- duplicate the #defines from AP_Motors
local AP_MOTORS_MATRIX_YAW_FACTOR_CW = -1
local AP_MOTORS_MATRIX_YAW_FACTOR_CCW = 1
local AP_MOTORS_MOT_1 = 0
local AP_MOTORS_MOT_2 = 1
local AP_MOTORS_MOT_3 = 2
local AP_MOTORS_MOT_4 = 3
local AP_MOTORS_MOT_5 = 4
local AP_MOTORS_MOT_6 = 5
local AP_MOTORS_MOT_7 = 6
local AP_MOTORS_MOT_8 = 7
local COAX_CURRENT_TOP_THRUST_RATIO = 1.0
local COAX_CURRENT_TOP_YAW_RATIO = 1.0
local PARAM_TABLE_KEY = 72
assert(param:add_table(PARAM_TABLE_KEY, "COAX_", 30), 'could not add COAX table')
assert(param:add_param(PARAM_TABLE_KEY, 1, 'TOP_THRUST_RATIO', 0.8), 'could not add TOP_THRUST_RATIO')
assert(param:add_param(PARAM_TABLE_KEY, 2, 'TOP_YAW_RATIO', 0.5), 'could not add TOP_YAW_RATIO')
-- helper function duplication of the one found in AP_MotorsMatrix
local function add_motor(motor_num, angle_degrees, yaw_factor, testing_order)
MotorsMatrix:add_motor_raw(motor_num,math.cos(math.rad(angle_degrees + 90)),
math.cos(math.rad(angle_degrees)),
yaw_factor,
testing_order)
end
local function updateThrustFactor()
-- this duplicates the add motor format used in AP_Motors for ease of modification of existing mixes
-- standard octa quad X mix
local COAX_NEW_TOP_THRUST_RATIO = param:get('COAX_TOP_THRUST_RATIO')
local COAX_NEW_TOP_YAW_RATIO = param:get('COAX_TOP_YAW_RATIO')
-- if unchanged, return
if COAX_NEW_TOP_THRUST_RATIO==COAX_CURRENT_TOP_THRUST_RATIO and COAX_NEW_TOP_YAW_RATIO==COAX_CURRENT_TOP_YAW_RATIO then
--no action required check back in, in 5 seconds
return updateThrustFactor, 5000
end
-- if out of range, return
if (COAX_NEW_TOP_THRUST_RATIO>1) or (COAX_NEW_TOP_THRUST_RATIO<0) or (COAX_NEW_TOP_YAW_RATIO>1) or (COAX_NEW_TOP_YAW_RATIO<0) then
gcs:send_text(6, "WARNING: Tried to set COAX value out of range [0,1], skipping...")
return updateThrustFactor, 5000
end
-- TODO if armed return...
COAX_CURRENT_TOP_THRUST_RATIO = COAX_NEW_TOP_THRUST_RATIO
COAX_CURRENT_TOP_YAW_RATIO = COAX_NEW_TOP_YAW_RATIO
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW*COAX_CURRENT_TOP_YAW_RATIO, 1) -- top front right
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW*COAX_CURRENT_TOP_YAW_RATIO, 7) -- top front left
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW*COAX_CURRENT_TOP_YAW_RATIO, 5) -- top back left
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW*COAX_CURRENT_TOP_YAW_RATIO, 3) -- top back right
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8) -- bottom front left
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2) -- bottom front right
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4) -- bottom back right
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6) -- bottom back left
-- -- setup throttle factors
-- -- top motors reduce throttle
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_1, COAX_CURRENT_TOP_THRUST_RATIO)
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_2, COAX_CURRENT_TOP_THRUST_RATIO)
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_3, COAX_CURRENT_TOP_THRUST_RATIO)
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_4, COAX_CURRENT_TOP_THRUST_RATIO)
-- bottom motors get full (the default is one so these lines don't actually do anything.)
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_5, 1.0)
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_6, 1.0)
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_7, 1.0)
MotorsMatrix:set_throttle_factor(AP_MOTORS_MOT_8, 1.0)
assert(MotorsMatrix:init(8), "Failed to init MotorsMatrix")
motors:set_frame_string("Octo Quad with different top and bottom throttle")
gcs:send_text(6, "Updated thrust factors TOP_TRUST_RATIO=" .. COAX_CURRENT_TOP_THRUST_RATIO .. " TOP_YAW_RATIO=" .. COAX_CURRENT_TOP_YAW_RATIO)
return updateThrustFactor, 5000
end
return updateThrustFactor, 1000
As I mentioned here and above, failure happens when MotorMatrix.init(n)
gets called. Parameters are attached in the text file BC2_30_Nov.txt
The linked thread contains a more minimal example which also fails in the same way.
Any help would be greatly appreciated.
BC2_30_Nov.txt (19.8 KB)