diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 49d1d0f476..831d1941d2 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -641,4 +641,13 @@ void PX4RCOutput::set_output_mode(enum output_mode mode) } +// set default output update rate +void PX4RCOutput::set_default_rate(uint16_t rate_hz) +{ + ioctl(_pwm_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz); + if (_alt_fd != -1) { + ioctl(_alt_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz); + } +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h index 37038b7d10..9436882cc5 100644 --- a/libraries/AP_HAL_PX4/RCOutput.h +++ b/libraries/AP_HAL_PX4/RCOutput.h @@ -40,6 +40,9 @@ public: void timer_tick(void) override; bool enable_sbus_out(uint16_t rate_hz) override; + // set default output update rate + void set_default_rate(uint16_t rate_hz) override; + private: int _pwm_fd; int _alt_fd;