diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 91ac7e005e..04f8e3f0bb 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -208,6 +208,25 @@ void AP_Motors::set_limit_flag_pitch_roll_yaw(bool flag) limit.yaw = flag; } +// returns true if the configured PWM type is digital and should have fixed endpoints +bool AP_Motors::is_digital_pwm_type() const +{ + switch (_pwm_type) { + case PWM_TYPE_DSHOT150: + case PWM_TYPE_DSHOT300: + case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT1200: + return true; + case PWM_TYPE_NORMAL: + case PWM_TYPE_ONESHOT: + case PWM_TYPE_ONESHOT125: + case PWM_TYPE_BRUSHED: + case PWM_TYPE_PWM_RANGE: + break; + } + return false; +} + namespace AP { AP_Motors *motors() { diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 08af747179..eb9a0ba120 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -219,16 +219,14 @@ public: // This function is overriden in motors_heli class. Always true for multicopters. virtual bool init_targets_on_arming() const { return true; } - enum pwm_type { PWM_TYPE_NORMAL = 0, - PWM_TYPE_ONESHOT = 1, - PWM_TYPE_ONESHOT125 = 2, - PWM_TYPE_BRUSHED = 3, - PWM_TYPE_DSHOT150 = 4, - PWM_TYPE_DSHOT300 = 5, - PWM_TYPE_DSHOT600 = 6, - PWM_TYPE_DSHOT1200 = 7, - PWM_TYPE_PWM_RANGE = 8 }; - pwm_type get_pwm_type(void) const { return (pwm_type)_pwm_type.get(); } + // returns true if the configured PWM type is digital and should have fixed endpoints + bool is_digital_pwm_type() const; + + // returns true is pwm type is brushed + bool is_brushed_pwm_type() const { return _pwm_type == PWM_TYPE_BRUSHED; } + + // returns true is pwm type is normal + bool is_normal_pwm_type() const { return (_pwm_type == PWM_TYPE_NORMAL) || (_pwm_type == PWM_TYPE_PWM_RANGE); } MAV_TYPE get_frame_mav_type() const { return _mav_type; } @@ -300,6 +298,16 @@ protected: MAV_TYPE _mav_type; // MAV_TYPE_GENERIC = 0; + enum pwm_type { PWM_TYPE_NORMAL = 0, + PWM_TYPE_ONESHOT = 1, + PWM_TYPE_ONESHOT125 = 2, + PWM_TYPE_BRUSHED = 3, + PWM_TYPE_DSHOT150 = 4, + PWM_TYPE_DSHOT300 = 5, + PWM_TYPE_DSHOT600 = 6, + PWM_TYPE_DSHOT1200 = 7, + PWM_TYPE_PWM_RANGE = 8 }; + private: bool _armed; // 0 if disarmed, 1 if armed