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

HAL_ChibiOS: reimplement CAN with BinarySemaphore

This commit is contained in:
Andrew Tridgell 2023-12-30 08:30:41 +11:00
parent 8a027eddb0
commit c0d0aeee86
4 changed files with 30 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

@ -812,11 +812,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
rx_item.flags = AP_HAL::CANIface::Loopback; rx_item.flags = AP_HAL::CANIface::Loopback;
add_to_rx_queue(rx_item); add_to_rx_queue(rx_item);
} }
if (event_handle_ != nullptr) { stats.num_events++;
stats.num_events++; if (sem_handle != nullptr) {
#if CH_CFG_USE_EVENTS == TRUE sem_handle->signal_ISR();
evt_src_.signalI(1 << self_index_);
#endif
} }
} }
} }
@ -925,11 +923,9 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index)
while (readRxFIFO(fifo_index)) { while (readRxFIFO(fifo_index)) {
had_activity_ = true; had_activity_ = true;
} }
if (event_handle_ != nullptr) { stats.num_events++;
stats.num_events++; if (sem_handle != nullptr) {
#if CH_CFG_USE_EVENTS == TRUE sem_handle->signal_ISR();
evt_src_.signalI(1 << self_index_);
#endif
} }
} }
@ -995,16 +991,11 @@ uint32_t CANIface::getErrorCount() const
stats.tx_timedout; stats.tx_timedout;
} }
#if CH_CFG_USE_EVENTS == TRUE bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle)
ChibiOS::EventSource CANIface::evt_src_;
bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
{ {
CriticalSectionLocker lock; sem_handle = handle;
event_handle_ = handle; return true;
event_handle_->set_source(&evt_src_);
return event_handle_->register_event(1 << self_index_);
} }
#endif
bool CANIface::isRxBufferEmpty() const bool CANIface::isRxBufferEmpty() const
{ {
@ -1075,10 +1066,10 @@ bool CANIface::select(bool &read, bool &write,
return true; return true;
} }
while (time < blocking_deadline) { while (time < blocking_deadline) {
if (event_handle_ == nullptr) { if (sem_handle == nullptr) {
break; break;
} }
event_handle_->wait(blocking_deadline - time); // Block until timeout expires or any iface updates IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates
checkAvailable(read, write, pending_tx); // Check what we got checkAvailable(read, write, pending_tx); // Check what we got
if ((read && in_read) || (write && in_write)) { if ((read && in_read) || (write && in_write)) {
return true; return true;

11
libraries/AP_HAL_ChibiOS/CANFDIface.h
View File

@ -41,7 +41,6 @@
#pragma once #pragma once
#include "AP_HAL_ChibiOS.h" #include "AP_HAL_ChibiOS.h"
#include "EventSource.h"
#if HAL_NUM_CAN_IFACES #if HAL_NUM_CAN_IFACES
@ -120,10 +119,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
bool irq_init_; bool irq_init_;
bool initialised_; bool initialised_;
bool had_activity_; bool had_activity_;
AP_HAL::EventHandle* event_handle_; AP_HAL::BinarySemaphore *sem_handle;
#if CH_CFG_USE_EVENTS == TRUE
static ChibiOS::EventSource evt_src_;
#endif
const uint8_t self_index_; const uint8_t self_index_;
bool computeTimings(uint32_t target_bitrate, Timings& out_timings); bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
@ -224,10 +221,8 @@ public:
const AP_HAL::CANFrame* const pending_tx, const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override; uint64_t blocking_deadline) override;
#if CH_CFG_USE_EVENTS == TRUE
// setup event handle for waiting on events // 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;
#endif
#if !defined(HAL_BOOTLOADER_BUILD) #if !defined(HAL_BOOTLOADER_BUILD)
// fetch stats text and return the size of the same, // fetch stats text and return the size of the same,

12
libraries/AP_HAL_ChibiOS/CANIface.h
View File

@ -46,7 +46,6 @@
# else # else
#if HAL_NUM_CAN_IFACES #if HAL_NUM_CAN_IFACES
#include "bxcan.hpp" #include "bxcan.hpp"
#include "EventSource.h"
#ifndef HAL_CAN_RX_QUEUE_SIZE #ifndef HAL_CAN_RX_QUEUE_SIZE
#define HAL_CAN_RX_QUEUE_SIZE 128 #define HAL_CAN_RX_QUEUE_SIZE 128
@ -109,10 +108,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
bool irq_init_:1; bool irq_init_:1;
bool initialised_:1; bool initialised_:1;
bool had_activity_:1; bool had_activity_:1;
#if CH_CFG_USE_EVENTS == TRUE AP_HAL::BinarySemaphore *sem_handle;
AP_HAL::EventHandle* event_handle_;
static ChibiOS::EventSource evt_src_;
#endif
const uint8_t self_index_; const uint8_t self_index_;
bool computeTimings(uint32_t target_bitrate, Timings& out_timings); bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
@ -210,10 +207,9 @@ public:
const AP_HAL::CANFrame* const pending_tx, const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override; uint64_t blocking_deadline) override;
#if CH_CFG_USE_EVENTS == TRUE
// setup event handle for waiting on events // 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;
#endif
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
// fetch stats text and return the size of the same, // fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt

31
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

@ -522,12 +522,11 @@ void CANIface::handleTxInterrupt(const uint64_t utc_usec)
handleTxMailboxInterrupt(2, txok, utc_usec); handleTxMailboxInterrupt(2, txok, utc_usec);
} }
#if CH_CFG_USE_EVENTS == TRUE if (sem_handle != nullptr) {
if (event_handle_ != nullptr) {
PERF_STATS(stats.num_events); PERF_STATS(stats.num_events);
evt_src_.signalI(1 << self_index_); sem_handle->signal_ISR();
} }
#endif
pollErrorFlagsFromISR(); pollErrorFlagsFromISR();
} }
@ -590,12 +589,11 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
had_activity_ = true; had_activity_ = true;
#if CH_CFG_USE_EVENTS == TRUE if (sem_handle != nullptr) {
if (event_handle_ != nullptr) {
PERF_STATS(stats.num_events); PERF_STATS(stats.num_events);
evt_src_.signalI(1 << self_index_); sem_handle->signal_ISR();
} }
#endif
pollErrorFlagsFromISR(); pollErrorFlagsFromISR();
} }
@ -702,17 +700,12 @@ uint32_t CANIface::getErrorCount() const
#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) #endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
#if CH_CFG_USE_EVENTS == TRUE bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle)
ChibiOS::EventSource CANIface::evt_src_;
bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
{ {
CriticalSectionLocker lock; sem_handle = handle;
event_handle_ = handle; return true;
event_handle_->set_source(&evt_src_);
return event_handle_->register_event(1 << self_index_);
} }
#endif // #if CH_CFG_USE_EVENTS == TRUE
void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const
{ {
@ -745,13 +738,13 @@ bool CANIface::select(bool &read, bool &write,
return true; return true;
} }
#if CH_CFG_USE_EVENTS == TRUE #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
// we don't support blocking select in AP_Periph and bootloader // we don't support blocking select in AP_Periph and bootloader
while (time < blocking_deadline) { while (time < blocking_deadline) {
if (event_handle_ == nullptr) { if (sem_handle == nullptr) {
break; break;
} }
event_handle_->wait(blocking_deadline - time); // Block until timeout expires or any iface updates IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates
checkAvailable(read, write, pending_tx); // Check what we got checkAvailable(read, write, pending_tx); // Check what we got
if ((read && in_read) || (write && in_write)) { if ((read && in_read) || (write && in_write)) {
return true; return true;