From 6e8501603c3a4aa2aeef97aa7cd9d1aafaeb8c97 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Jan 2013 07:29:59 +1100 Subject: [PATCH] HAL_PX4: avoid RCOutput ioctl unless values change this lowers latency somewhat --- libraries/AP_HAL_PX4/RCOutput.cpp | 19 ++++++++++++++----- libraries/AP_HAL_PX4/RCOutput.h | 3 +++ 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 2ddc8994b0..b47d32cab6 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -27,6 +27,10 @@ void PX4RCOutput::init(void* unused) void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) { + if (freq_hz == _freq_hz) { + // avoid the ioctl() if possible + return; + } // we can't set this per channel yet if (ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) == 0) { _freq_hz = freq_hz; @@ -60,7 +64,13 @@ void PX4RCOutput::disable_mask(uint32_t chmask) void PX4RCOutput::write(uint8_t ch, uint16_t period_us) { - ioctl(_pwm_fd, PWM_SERVO_SET(ch), (unsigned long)period_us); + if (ch >= PX4_NUM_OUTPUT_CHANNELS) { + return; + } + if (period_us != _period[ch]) { + ioctl(_pwm_fd, PWM_SERVO_SET(ch), (unsigned long)period_us); + _period[ch] = period_us; + } } void PX4RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len) @@ -72,11 +82,10 @@ void PX4RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len) uint16_t PX4RCOutput::read(uint8_t ch) { - servo_position_t pos; - if (ioctl(_pwm_fd, PWM_SERVO_GET(ch), (unsigned long)&pos) == 0) { - return pos; + if (ch >= PX4_NUM_OUTPUT_CHANNELS) { + return 0; } - return 0; + return _period[ch]; } void PX4RCOutput::read(uint16_t* period_us, uint8_t len) diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h index b228cfecb1..9754900f6f 100644 --- a/libraries/AP_HAL_PX4/RCOutput.h +++ b/libraries/AP_HAL_PX4/RCOutput.h @@ -4,6 +4,8 @@ #include +#define PX4_NUM_OUTPUT_CHANNELS 16 + class PX4::PX4RCOutput : public AP_HAL::RCOutput { public: @@ -22,6 +24,7 @@ public: private: int _pwm_fd; uint16_t _freq_hz; + uint16_t _period[PX4_NUM_OUTPUT_CHANNELS]; }; #endif // __AP_HAL_PX4_RCOUTPUT_H__