Browse Source

HAL_PX4: added rcout set_detault_rate()

mission-4.1.18
Andrew Tridgell 8 years ago
parent
commit
5b43698e25
  1. 9
      libraries/AP_HAL_PX4/RCOutput.cpp
  2. 3
      libraries/AP_HAL_PX4/RCOutput.h

9
libraries/AP_HAL_PX4/RCOutput.cpp

@ -641,4 +641,13 @@ void PX4RCOutput::set_output_mode(enum output_mode mode) @@ -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

3
libraries/AP_HAL_PX4/RCOutput.h

@ -40,6 +40,9 @@ public: @@ -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;

Loading…
Cancel
Save