32 #include <linux/can.h> 38 #include <unordered_set> 50 #define CAN_MAX_POLL_ITERATIONS_COUNT 100 51 #define CAN_MAX_INIT_TRIES_COUNT 100 52 #define CAN_FILTER_NUMBER 8 54 class CAN:
public AP_HAL::CAN {
58 , _frames_in_socket_tx_queue(0)
59 , _max_frames_in_socket_tx_queue(2)
63 bool begin(uint32_t bitrate)
override;
67 void reset()
override;
69 bool is_initialized()
override;
71 int32_t tx_pending()
override;
73 int32_t available()
override;
75 static int openSocket(
const std::string& iface_name);
79 int16_t send(
const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
80 uavcan::CanIOFlags flags)
override;
82 int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
83 uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
override;
85 bool hasReadyTx()
const;
87 bool hasReadyRx()
const;
91 int16_t configureFilters(
const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs)
override;
93 uint16_t getNumFilters()
const override;
95 uint64_t getErrorCount()
const override;
103 uavcan::CanIOFlags flags = 0;
104 std::uint64_t order = 0;
106 TxItem(
const uavcan::CanFrame& arg_frame, uavcan::MonotonicTime arg_deadline,
107 uavcan::CanIOFlags arg_flags, std::uint64_t arg_order)
109 , deadline(arg_deadline)
116 if (frame.priorityLowerThan(rhs.
frame)) {
119 if (frame.priorityHigherThan(rhs.
frame)) {
122 return order > rhs.
order;
142 int _write(
const uavcan::CanFrame& frame)
const;
144 int _read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc,
bool& loopback)
const;
146 void _incrementNumFramesInSocketTxQueue();
148 void _confirmSentFrame();
150 bool _wasInPendingLoopbackSet(
const uavcan::CanFrame& frame);
152 bool _checkHWFilters(
const can_frame& frame)
const;
173 class CANManager:
public AP_HAL::CANManager,
public uavcan::ICanDriver {
187 virtual bool begin(uint32_t bitrate, uint8_t can_number)
override;
189 virtual void initialized(
bool val);
190 virtual bool is_initialized();
194 virtual AP_UAVCAN *get_UAVCAN(
void)
override;
195 virtual void set_UAVCAN(
AP_UAVCAN *uavcan)
override;
199 virtual CAN* getIface(uint8_t iface_index)
override;
201 virtual uint8_t
getNumIfaces()
const override {
return _ifaces.size(); }
203 virtual int16_t select(uavcan::CanSelectMasks& inout_masks,
204 const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline)
override;
206 int init(uint8_t can_number);
208 int addIface(
const std::string& iface_name);
218 void updateDownStatusFromPollResult(
const pollfd& pfd);
225 std::vector<std::unique_ptr<IfaceWrapper>>
_ifaces;
std::vector< can_filter > _hw_filters_container
std::map< SocketCanError, uint64_t > _errors
void _registerError(SocketCanError e)
std::unordered_multiset< uint32_t > _pending_loopback_ids
ssize_t write(int fd, const void *buf, size_t count)
POSIX Write count bytes from *buf to fileno fd.
uavcan::MonotonicTime ts_mono
std::vector< std::unique_ptr< IfaceWrapper > > _ifaces
int _write(int fd, const char *buf, size_t cnt)
ssize_t read(int fd, void *buf, size_t count)
POSIX read count bytes from *buf to fileno fd.
uavcan::MonotonicTime deadline
unsigned _frames_in_socket_tx_queue
static CANManager * from(AP_HAL::CANManager *can)
uint64_t _tx_frame_counter
bool operator<(const TxItem &rhs) const
TxItem(const uavcan::CanFrame &arg_frame, uavcan::MonotonicTime arg_deadline, uavcan::CanIOFlags arg_flags, std::uint64_t arg_order)
std::priority_queue< TxItem > _tx_queue
void init()
Generic board initialization function.
int getFileDescriptor() const
virtual uint8_t getNumIfaces() const override
const unsigned _max_frames_in_socket_tx_queue
int _read(int fd, char *buf, size_t cnt)
std::queue< RxItem > _rx_queue