From f3d979165949def504f0ad2437292cc49548ac0c Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Mon, 8 Feb 2016 09:02:54 -0800 Subject: [PATCH] Sub: Added coupling factor as parameter --- libraries/AP_Motors/AP_Motors6DOF.cpp | 7 +++++++ libraries/AP_Motors/AP_Motors6DOF.h | 1 + libraries/AP_Motors/AP_MotorsVectoredROV.cpp | 6 ++++-- 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp index ffb58b3a33..8000bf8e37 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.cpp +++ b/libraries/AP_Motors/AP_Motors6DOF.cpp @@ -83,6 +83,13 @@ const AP_Param::GroupInfo AP_Motors6DOF::var_info[] = { // @User: Standard AP_GROUPINFO("MOT8_REVERSE", 7, AP_Motors6DOF, _motor_reverse[7], 1), + // @Param: FWDVERT_CPL_K + // @DisplayName: Forward/vertical to pitch decoupling factor + // @Description: Used to decouple pitch from forward/vertical motion + // @Values: 0: min (no effect), 1: normal, 1.5: maximum + // @User: Standard + AP_GROUPINFO("FWDVERT_CPL_K", 8, AP_Motors6DOF, _forwardVerticalCouplingFactor, 1.0), + AP_GROUPEND }; diff --git a/libraries/AP_Motors/AP_Motors6DOF.h b/libraries/AP_Motors/AP_Motors6DOF.h index 87e5748e7a..c601df0ff2 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.h +++ b/libraries/AP_Motors/AP_Motors6DOF.h @@ -39,6 +39,7 @@ protected: // Parameters AP_Int8 _motor_reverse[8]; + AP_Float _forwardVerticalCouplingFactor; float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent) float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward diff --git a/libraries/AP_Motors/AP_MotorsVectoredROV.cpp b/libraries/AP_Motors/AP_MotorsVectoredROV.cpp index b0b7640d4b..ffe596fa69 100644 --- a/libraries/AP_Motors/AP_MotorsVectoredROV.cpp +++ b/libraries/AP_Motors/AP_MotorsVectoredROV.cpp @@ -153,8 +153,10 @@ void AP_MotorsVectoredROV::output_armed_stabilizing() } } - float forwardVerticalCouplingFactor = 1.0; // 0.0-1.5 - int16_t forward_coupling_limit = max(forwardVerticalCouplingFactor*(400-fabs(throttle_radio_output)),0); + int16_t forward_coupling_limit = _forwardVerticalCouplingFactor*(400-fabs(throttle_radio_output)); + if ( forward_coupling_limit < 0 ) { + forward_coupling_limit = 0; + } int8_t forward_coupling_direction[] = {-1,0,1,1,0,-1}; // calculate linear command for each motor