aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_Linux: support mavcan

Browse Source
This commit is contained in:
Andrew Tridgell 2022-02-07 09:43:38 +11:00
parent 5eb8b8d72e
commit b6f78ede2e
2 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL_Linux/CANSocketIface.cpp
View File

@ -167,17 +167,19 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline
tx_item.setup = true;
tx_item.index = _tx_frame_counter;
tx_item.deadline = tx_deadline;
WITH_SEMAPHORE(sem);
_tx_queue.emplace(tx_item);
_tx_frame_counter++;
stats.tx_requests++;
_pollRead(); // Read poll is necessary because it can release the pending TX flag
_pollWrite();
return 1;
return AP_HAL::CANIface::send(frame, tx_deadline, flags);
}
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
CANIface::CanIOFlags& out_flags)
{
WITH_SEMAPHORE(sem);
if (_rx_queue.empty()) {
_pollRead(); // This allows to use the socket not calling poll() explicitly.
if (_rx_queue.empty()) {
@ -191,16 +193,18 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
out_flags = rx.flags;
}
(void)_rx_queue.pop();
return 1;
return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
}
bool CANIface::_hasReadyTx() const
bool CANIface::_hasReadyTx()
{
WITH_SEMAPHORE(sem);
return !_tx_queue.empty() && (_frames_in_socket_tx_queue < _max_frames_in_socket_tx_queue);
}
bool CANIface::_hasReadyRx() const
bool CANIface::_hasReadyRx()
{
WITH_SEMAPHORE(sem);
return !_rx_queue.empty();
}
@ -263,6 +267,7 @@ uint32_t CANIface::getErrorCount() const
void CANIface::_pollWrite()
{
while (_hasReadyTx()) {
WITH_SEMAPHORE(sem);
const CanTxItem tx = _tx_queue.top();
uint64_t curr_time = AP_HAL::native_micros64();
if (tx.deadline >= curr_time) {
@ -309,6 +314,7 @@ bool CANIface::_pollRead()
stats.tx_confirmed++;
}
if (accept) {
WITH_SEMAPHORE(sem);
_rx_queue.push(rx);
stats.rx_received++;
return true;
@ -390,6 +396,7 @@ int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopb
// Might block forever, only to be used for testing
void CANIface::flush_tx()
{
WITH_SEMAPHORE(sem);
do {
_updateDownStatusFromPollResult(_pollfd);
_poll(true, true);
@ -398,6 +405,7 @@ void CANIface::flush_tx()
void CANIface::clear_rx()
{
WITH_SEMAPHORE(sem);
// Clean Rx Queue
std::queue<CanRxItem> empty;
std::swap( _rx_queue, empty );

15
libraries/AP_HAL_Linux/CANSocketIface.h
View File

@ -142,9 +142,9 @@ private:
bool _checkHWFilters(const can_frame& frame) const;
bool _hasReadyTx() const;
bool _hasReadyTx();
bool _hasReadyRx() const;
bool _hasReadyRx();
void _poll(bool read, bool write);
@ -190,6 +190,17 @@ private:
uint32_t num_poll_tx_events;
uint32_t num_poll_rx_events;
} stats;
protected:
bool add_to_rx_queue(const CanRxItem &rx_item) override {
_rx_queue.push(rx_item);
return true;
}
int8_t get_iface_num(void) const override {
return _self_index;
}
HAL_Semaphore sem;
};
}