From a80eea5de4261a8473442a8f72354b4e3fefc6a6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Oct 2016 07:58:40 +1100 Subject: [PATCH] HAL_Linux: implement cork()/push() for HAL_Linux RCOutput_Sysfs --- libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp | 27 +++++++++++++++++++++-- libraries/AP_HAL_Linux/RCOutput_Sysfs.h | 7 ++++++ 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp b/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp index 82d8d3e9bf..8450509675 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp @@ -28,6 +28,7 @@ RCOutput_Sysfs::RCOutput_Sysfs(uint8_t chip, uint8_t channel_base, uint8_t chann , _channel_base(channel_base) , _channel_count(channel_count) , _pwm_channels(new PWM_Sysfs_Base *[_channel_count]) + , _pending(new uint16_t[_channel_count]) { } @@ -101,8 +102,12 @@ void RCOutput_Sysfs::write(uint8_t ch, uint16_t period_us) if (ch >= _channel_count) { return; } - - _pwm_channels[ch]->set_duty_cycle(usec_to_nsec(period_us)); + if (_corked) { + _pending[ch] = period_us; + _pending_mask |= (1U<set_duty_cycle(usec_to_nsec(period_us)); + } } uint16_t RCOutput_Sysfs::read(uint8_t ch) @@ -123,4 +128,22 @@ void RCOutput_Sysfs::read(uint16_t *period_us, uint8_t len) period_us[i] = 1000; } } + +void RCOutput_Sysfs::cork(void) +{ + _corked = true; } + +void RCOutput_Sysfs::push(void) +{ + for (uint8_t i=0; i<_channel_count; i++) { + if ((1U<set_duty_cycle(usec_to_nsec(_pending[i])); + } + } + _pending_mask = 0; + _corked = false; +} + +} + diff --git a/libraries/AP_HAL_Linux/RCOutput_Sysfs.h b/libraries/AP_HAL_Linux/RCOutput_Sysfs.h index 8591627434..5270be4b50 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Sysfs.h +++ b/libraries/AP_HAL_Linux/RCOutput_Sysfs.h @@ -23,12 +23,19 @@ public: void write(uint8_t ch, uint16_t period_us); uint16_t read(uint8_t ch); void read(uint16_t *period_us, uint8_t len); + void cork(void) override; + void push(void) override; private: const uint8_t _chip; const uint8_t _channel_base; const uint8_t _channel_count; PWM_Sysfs_Base **_pwm_channels; + + // for handling cork()/push() + bool _corked; + uint16_t *_pending; + uint32_t _pending_mask; }; }