From 6d650e74f2fdcdbbb63afdfec651d87779cda6f8 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 8 Feb 2021 19:10:44 +0000 Subject: [PATCH] AP_Scripting: add fault tolerant hex MotorsMatrix example --- .../MotorMatrix_fault_tolerant_hex.lua | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 libraries/AP_Scripting/examples/MotorMatrix_fault_tolerant_hex.lua diff --git a/libraries/AP_Scripting/examples/MotorMatrix_fault_tolerant_hex.lua b/libraries/AP_Scripting/examples/MotorMatrix_fault_tolerant_hex.lua new file mode 100644 index 0000000000..c0f5ef2a76 --- /dev/null +++ b/libraries/AP_Scripting/examples/MotorMatrix_fault_tolerant_hex.lua @@ -0,0 +1,35 @@ +-- This script is an example setting up a custom motor matrix mix + +-- this is the config for hexacopter with the motor rotations configured for improved fault tolerance +-- see: https://arxiv.org/pdf/1403.5986.pdf + +-- 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 + +-- helper function duplication 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 + +-- this duplicates the add motor format used in AP_Motors for ease of modification of existing mixes +add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2) +add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5) +add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6) +add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3) +add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1) +add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4) + +assert(MotorsMatrix:init(6), "Failed to init MotorsMatrix")