HAL_QURT: implement cork() and push() for RCOutput

This commit is contained in:
Andrew Tridgell 2016-10-12 08:06:36 +11:00
parent 5530e3782d
commit 7ea75aaf95
2 changed files with 16 additions and 1 deletions

View File

@ -51,7 +51,9 @@ void RCOutput::write(uint8_t ch, uint16_t period_us)
return;
}
period[ch] = period_us;
need_write = true;
if (!corked) {
need_write = true;
}
}
uint16_t RCOutput::read(uint8_t ch)
@ -108,6 +110,16 @@ void RCOutput::timer_update(void)
::write(fd, (uint8_t *)&frame, sizeof(frame));
}
void RCOutput::cork(void)
{
corked = true;
}
void RCOutput::push(void)
{
need_write = true;
corked = false;
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

View File

@ -24,6 +24,8 @@ 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;
void timer_update(void);
@ -36,6 +38,7 @@ private:
uint16_t enable_mask;
uint16_t period[channel_count];
volatile bool need_write;
bool corked;
};
#endif // CONFIG_HAL_BOARD