mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-04 23:18:29 -04:00
AP_HAL_Linux: Poller: allow to fail constructor
This commit is contained in:
parent
e5003c3116
commit
efe819e21e
@ -17,6 +17,7 @@
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <sys/epoll.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/uio.h>
|
||||
@ -28,6 +29,14 @@ extern const AP_HAL::HAL &hal;
|
||||
|
||||
namespace Linux {
|
||||
|
||||
Poller::Poller()
|
||||
{
|
||||
_epfd = epoll_create1(EPOLL_CLOEXEC);
|
||||
if (_epfd == -1) {
|
||||
fprintf(stderr, "Failed to create epoll: %m\n");
|
||||
}
|
||||
}
|
||||
|
||||
bool Poller::register_pollable(Pollable *p, uint32_t events)
|
||||
{
|
||||
/*
|
||||
|
@ -16,7 +16,6 @@
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <sys/epoll.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "AP_HAL/utility/RingBuffer.h"
|
||||
@ -63,9 +62,23 @@ protected:
|
||||
|
||||
class Poller {
|
||||
public:
|
||||
Poller() : _epfd(epoll_create1(EPOLL_CLOEXEC)) { }
|
||||
~Poller() { close(_epfd); }
|
||||
Poller();
|
||||
|
||||
~Poller() {
|
||||
if (_epfd >= 0) {
|
||||
close(_epfd);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Check if this Poller is not initialized
|
||||
*/
|
||||
bool operator!() const { return _epfd == -1; }
|
||||
|
||||
/*
|
||||
* Check if this Poller is succesfully initialized
|
||||
*/
|
||||
explicit operator bool() const { return _epfd != -1; }
|
||||
|
||||
/*
|
||||
* Register @p in this poller so calls to poll() will wait for
|
||||
@ -87,7 +100,8 @@ public:
|
||||
int poll() const;
|
||||
|
||||
private:
|
||||
int _epfd;
|
||||
|
||||
int _epfd = -1;
|
||||
};
|
||||
|
||||
}
|
||||
|
@ -92,6 +92,9 @@ TimerPollable *PollerThread::add_timer(TimerPollable::PeriodicCb cb,
|
||||
TimerPollable::WrapperCb *wrapper,
|
||||
uint32_t timeout_usec)
|
||||
{
|
||||
if (!_poller) {
|
||||
return nullptr;
|
||||
}
|
||||
TimerPollable *p = new TimerPollable(cb, wrapper);
|
||||
if (!p || !p->setup_timer(timeout_usec) ||
|
||||
!_poller.register_pollable(p, POLLIN)) {
|
||||
@ -117,6 +120,10 @@ bool PollerThread::adjust_timer(TimerPollable *p, uint32_t timeout_usec)
|
||||
|
||||
void PollerThread::_cleanup_timers()
|
||||
{
|
||||
if (!_poller) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (auto it = _timers.begin(); it != _timers.end(); it++) {
|
||||
TimerPollable *p = *it;
|
||||
if (p->_removeme) {
|
||||
@ -129,6 +136,10 @@ void PollerThread::_cleanup_timers()
|
||||
|
||||
void PollerThread::mainloop()
|
||||
{
|
||||
if (!_poller) {
|
||||
return;
|
||||
}
|
||||
|
||||
while (true) {
|
||||
_poller.poll();
|
||||
_cleanup_timers();
|
||||
|
Loading…
Reference in New Issue
Block a user