From 1395f9ce79fa8eecd4a28dd9f240dc881ad3c460 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 26 Jan 2021 13:03:20 +0000 Subject: [PATCH] AP_Scripting: add 6DoF motors matrix bindings and example --- .../AP_Scripting/examples/Motors_6DoF_LynchPin.lua | 10 ++++++++++ .../AP_Scripting/generator/description/bindings.desc | 6 ++++++ 2 files changed, 16 insertions(+) create mode 100644 libraries/AP_Scripting/examples/Motors_6DoF_LynchPin.lua diff --git a/libraries/AP_Scripting/examples/Motors_6DoF_LynchPin.lua b/libraries/AP_Scripting/examples/Motors_6DoF_LynchPin.lua new file mode 100644 index 0000000000..c3a45dfe75 --- /dev/null +++ b/libraries/AP_Scripting/examples/Motors_6DoF_LynchPin.lua @@ -0,0 +1,10 @@ +-- This script loads the 6DoF mixer matrix for the LynchPin frame + +Motors_6DoF:add_motor(0, 0.012558, 0.391064, 0.069463, 0.867789, 0.507225, -0.015613, true, 1) +Motors_6DoF:add_motor(1, -0.385797, -0.187969, 0.106479, 0.782773, -0.243803, 0.479664, true, 2) +Motors_6DoF:add_motor(2, 0.373240, -0.203095, -0.105312, 0.865451, -0.263422, -0.464051, true, 3) +Motors_6DoF:add_motor(3, -0.153922, -0.166480, -0.345955, 0.003411, 0.908383, -0.456939, true, 4) +Motors_6DoF:add_motor(4, -0.210017, -0.010839, 0.379773, -0.056142, -0.014059, -1.035508, true, 5) +Motors_6DoF:add_motor(5, 0.142076, -0.202427, 0.347056, -0.030879, 0.861758, 0.471667, true, 6) + +assert(Motors_6DoF:init(6),'unable to setup 6 motors') diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index d8416b3c0e..57502427dc 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -322,3 +322,9 @@ singleton AC_AttitudeControl_Multi_6DoF method set_lateral_enable void boolean singleton AC_AttitudeControl_Multi_6DoF method set_forward_enable void boolean singleton AC_AttitudeControl_Multi_6DoF method set_offset_roll_pitch void float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX + +include AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h depends APM_BUILD_TYPE(APM_BUILD_ArduCopter) +singleton AP_MotorsMatrix_6DoF_Scripting depends APM_BUILD_TYPE(APM_BUILD_ArduCopter) +singleton AP_MotorsMatrix_6DoF_Scripting alias Motors_6DoF +singleton AP_MotorsMatrix_6DoF_Scripting method init boolean uint8_t 0 (AP_MOTORS_MAX_NUM_MOTORS-1) +singleton AP_MotorsMatrix_6DoF_Scripting method add_motor void int8_t 0 (AP_MOTORS_MAX_NUM_MOTORS-1) float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX boolean uint8_t 0 (AP_MOTORS_MAX_NUM_MOTORS-1)