From 560b2545c894cb5cd36f181c12d4f996c6360055 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sat, 3 Sep 2022 22:24:11 +0530 Subject: [PATCH] AR_Motors: fix have_skid_steering to return true for omni too --- libraries/AR_Motors/AP_MotorsUGV.cpp | 12 ++++-------- libraries/AR_Motors/AP_MotorsUGV.h | 3 +++ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index a74af18176..849f8ea42d 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -135,7 +135,7 @@ void AP_MotorsUGV::init(uint8_t frtype) setup_safety_output(); // setup for omni vehicles - if (_frame_type != FRAME_TYPE_UNDEFINED) { + if (is_omni()) { setup_omni(); } } @@ -266,11 +266,7 @@ float AP_MotorsUGV::get_slew_limited_throttle(float throttle, float dt) const */ bool AP_MotorsUGV::have_skid_steering() const { - if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) && - SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) { - return true; - } - return false; + return (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) && SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) || is_omni(); } // true if the vehicle has a mainsail @@ -779,8 +775,8 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott // output for omni frames void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float lateral) { - // exit immediately if the frame type is set to UNDEFINED - if (_frame_type == FRAME_TYPE_UNDEFINED) { + // exit immediately if the vehicle is not omni + if (!is_omni()) { return; } diff --git a/libraries/AR_Motors/AP_MotorsUGV.h b/libraries/AR_Motors/AP_MotorsUGV.h index 667c9fe9ff..7496de5ebb 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.h +++ b/libraries/AR_Motors/AP_MotorsUGV.h @@ -112,6 +112,9 @@ public: // returns true if the configured PWM type is digital and should have fixed endpoints bool is_digital_pwm_type() const; + // returns true if the vehicle is omni + bool is_omni() const { return _frame_type != FRAME_TYPE_UNDEFINED && _motors_num > 0; } + // structure for holding motor limit flags struct AP_MotorsUGV_limit { uint8_t steer_left : 1; // we have reached the steering controller's left most limit