diff --git a/libraries/AP_HAL_Linux/CANSocketIface.cpp b/libraries/AP_HAL_Linux/CANSocketIface.cpp index 03f3be88da..05a02280ce 100644 --- a/libraries/AP_HAL_Linux/CANSocketIface.cpp +++ b/libraries/AP_HAL_Linux/CANSocketIface.cpp @@ -51,8 +51,6 @@ using namespace Linux; #define Debug(fmt, args...) #endif -CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES]; - static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) { can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } }; @@ -193,6 +191,9 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u out_flags = rx.flags; } (void)_rx_queue.pop(); + if (sem_handle != nullptr) { + sem_handle->signal(); + } return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags); } @@ -507,8 +508,9 @@ bool CANIface::select(bool &read_select, bool &write_select, stats.num_tx_poll_req++; } } - if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) { - _evt_handle->wait(blocking_deadline - AP_HAL::micros64()); + const uint64_t now_us = AP_HAL::micros64(); + if (sem_handle != nullptr && blocking_deadline > now_us) { + IGNORE_RETURN(sem_handle->wait(blocking_deadline - now_us)); } } @@ -528,67 +530,13 @@ bool CANIface::select(bool &read_select, bool &write_select, return true; } -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) { - _evt_handle = handle; - evt_can_socket[_self_index]._ifaces[_self_index] = this; - _evt_handle->set_source(&evt_can_socket[_self_index]); - return true; -} - - -bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) { - if (evt_handle == nullptr) { - return false; - } - pollfd pollfds[HAL_NUM_CAN_IFACES] {}; - uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {}; - unsigned long int num_pollfds = 0; - - // Poll FD set setup - for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) { - if (_ifaces[i] == nullptr) { - continue; - } - if (_ifaces[i]->_down) { - continue; - } - pollfds[num_pollfds] = _ifaces[i]->_pollfd; - pollfd_iface_map[num_pollfds] = i; - num_pollfds++; - _ifaces[i]->stats.num_poll_waits++; - } - - if (num_pollfds == 0) { - return true; - } - - // Timeout conversion - auto ts = timespec(); - ts.tv_sec = 0; - ts.tv_nsec = duration_us * 1000UL; - - // Blocking here - const int res = ppoll(pollfds, num_pollfds, &ts, nullptr); - - if (res < 0) { - return false; - } - - // Handling poll output - for (unsigned i = 0; i < num_pollfds; i++) { - if (_ifaces[pollfd_iface_map[i]] == nullptr) { - continue; - } - _ifaces[pollfd_iface_map[i]]->_updateDownStatusFromPollResult(pollfds[i]); - - const bool poll_read = pollfds[i].revents & POLLIN; - const bool poll_write = pollfds[i].revents & POLLOUT; - _ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write); - } + sem_handle = handle; return true; } + void CANIface::get_stats(ExpandingString &str) { str.printf("tx_requests: %u\n" diff --git a/libraries/AP_HAL_Linux/CANSocketIface.h b/libraries/AP_HAL_Linux/CANSocketIface.h index a7e85f4575..1e4d01a7b4 100644 --- a/libraries/AP_HAL_Linux/CANSocketIface.h +++ b/libraries/AP_HAL_Linux/CANSocketIface.h @@ -109,22 +109,12 @@ public: uint64_t blocking_deadline) override; // setup event handle for waiting on events - bool set_event_handle(AP_HAL::EventHandle* handle) override; + bool set_event_handle(AP_HAL::BinarySemaphore *handle) override; // fetch stats text and return the size of the same, // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt void get_stats(ExpandingString &str) override; - class CANSocketEventSource : public AP_HAL::EventSource { - friend class CANIface; - CANIface *_ifaces[HAL_NUM_CAN_IFACES]; - - public: - // we just poll fd, no signaling is done - void signal(uint32_t evt_mask) override { return; } - bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override; - }; - private: void _pollWrite(); @@ -164,8 +154,7 @@ private: const unsigned _max_frames_in_socket_tx_queue; unsigned _frames_in_socket_tx_queue; uint32_t _tx_frame_counter; - AP_HAL::EventHandle *_evt_handle; - static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES]; + AP_HAL::BinarySemaphore *sem_handle; pollfd _pollfd; std::map _errors;