Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: implement cork()/push() for HAL_Linux RCOutput_Sysfs

Browse Source
This commit is contained in:
Andrew Tridgell 2016-10-12 07:58:40 +11:00
parent 97d0553938
commit a80eea5de4
2 changed files with 32 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp
View File

@ -28,6 +28,7 @@ RCOutput_Sysfs::RCOutput_Sysfs(uint8_t chip, uint8_t channel_base, uint8_t chann
, _channel_base(channel_base) , _channel_base(channel_base)
, _channel_count(channel_count) , _channel_count(channel_count)
, _pwm_channels(new PWM_Sysfs_Base *[_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) { if (ch >= _channel_count) {
return; return;
} }
if (_corked) {
_pwm_channels[ch]->set_duty_cycle(usec_to_nsec(period_us)); _pending[ch] = period_us;
_pending_mask |= (1U<<ch);
} else {
_pwm_channels[ch]->set_duty_cycle(usec_to_nsec(period_us));
}
} }
uint16_t RCOutput_Sysfs::read(uint8_t ch) 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; 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<<i) & _pending_mask) {
_pwm_channels[i]->set_duty_cycle(usec_to_nsec(_pending[i]));
}
}
_pending_mask = 0;
_corked = false;
}
}

7
libraries/AP_HAL_Linux/RCOutput_Sysfs.h
View File

@ -23,12 +23,19 @@ public:
void write(uint8_t ch, uint16_t period_us); void write(uint8_t ch, uint16_t period_us);
uint16_t read(uint8_t ch); uint16_t read(uint8_t ch);
void read(uint16_t *period_us, uint8_t len); void read(uint16_t *period_us, uint8_t len);
void cork(void) override;
void push(void) override;
private: private:
const uint8_t _chip; const uint8_t _chip;
const uint8_t _channel_base; const uint8_t _channel_base;
const uint8_t _channel_count; const uint8_t _channel_count;
PWM_Sysfs_Base **_pwm_channels; PWM_Sysfs_Base **_pwm_channels;
// for handling cork()/push()
bool _corked;
uint16_t *_pending;
uint32_t _pending_mask;
}; };
} }