diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index b211384e1c..9f0255059a 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -647,6 +647,13 @@ void AP_MotorsUGV::setup_omni() add_omni_motor(2, 0.0f, -1.0f, 1.0f); add_omni_motor(3, 1.0f, 0.0f, 0.0f); break; + + case FRAME_TYPE_OMNI3MECANUM: + _motors_num = 3; + add_omni_motor(0, -1.0f, 1.0f, -0.26795f); + add_omni_motor(1, 0.73205f, 1.0f, -0.73205f); + add_omni_motor(2, 0.26795f, 1.0f, 1.0f); + break; } } diff --git a/libraries/AR_Motors/AP_MotorsUGV.h b/libraries/AR_Motors/AP_MotorsUGV.h index 42c2df5377..4df000f74e 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.h +++ b/libraries/AR_Motors/AP_MotorsUGV.h @@ -27,6 +27,7 @@ public: FRAME_TYPE_OMNI3 = 1, FRAME_TYPE_OMNIX = 2, FRAME_TYPE_OMNIPLUS = 3, + FRAME_TYPE_OMNI3MECANUM = 4, }; // initialise motors