Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: reimplement CAN with BinarySemaphore

This commit is contained in:
Andrew Tridgell 2023-12-30 08:31:09 +11:00
parent bdb8a08724
commit 8f8048e4cd
2 changed files with 11 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
libraries/AP_HAL_Linux/CANSocketIface.cpp
View File

@ -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"

15
libraries/AP_HAL_Linux/CANSocketIface.h
View File

@ -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<SocketCanError, uint64_t> _errors;