diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 6792739bad..428bdd54c5 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -483,6 +483,17 @@ int16_t AP_MotorsMulticopter::get_pwm_output_max() const return _throttle_radio_max; } +// parameter checks for MOT_PWM_MIN/MAX +bool AP_MotorsMulticopter::check_mot_pwm_params() +{ + if ((_pwm_min == 0 && _pwm_max !=0) || (_pwm_min != 0 && _pwm_max == 0) || + (_pwm_min < 0 || _pwm_min < 0) || (_pwm_min > _pwm_max)) + { + return true; + } + return false; +} + // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) // also sets throttle channel minimum and maximum pwm void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_max) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index f9d73a9dd5..5d310561a9 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -84,6 +84,9 @@ public: int16_t get_pwm_output_min() const; int16_t get_pwm_output_max() const; + // parameter check for MOT_PWM_MIN/MAX + bool check_mot_pwm_params(); + // set thrust compensation callback FUNCTOR_TYPEDEF(thrust_compensation_fn_t, void, float *, uint8_t); void set_thrust_compensation_callback(thrust_compensation_fn_t callback) {