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

AP_HAL: support more than 1 registered CAN callback

Browse Source 
this allows for CAN MCAST and MAVCAN at the same time
This commit is contained in:
Andrew Tridgell 2024-09-08 16:11:30 +10:00
parent 21b6b1b229
commit 4a102e2f2b
2 changed files with 58 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_HAL/CANIface.cpp
View File

@ -58,9 +58,16 @@ bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const
*/
int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags)
{
auto cb = frame_callback;
if (cb && (out_flags & IsMAVCAN)==0) {
cb(get_iface_num(), out_frame);
if ((out_flags & IsMAVCAN) != 0) {
return 1;
}
#ifndef HAL_BOOTLOADER_BUILD
WITH_SEMAPHORE(callbacks.sem);
#endif
for (auto &cb : callbacks.cb) {
if (cb != nullptr) {
cb(get_iface_num(), out_frame);
}
}
return 1;
}
@ -70,28 +77,59 @@ int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotoni
*/
int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags)
{
auto cb = frame_callback;
if (cb) {
#ifndef HAL_BOOTLOADER_BUILD
WITH_SEMAPHORE(callbacks.sem);
#endif
bool added_to_rx_queue = false;
for (auto &cb : callbacks.cb) {
if (cb == nullptr) {
continue;
}
if ((flags & IsMAVCAN) == 0) {
cb(get_iface_num(), frame);
} else {
} else if (!added_to_rx_queue) {
CanRxItem rx_item;
rx_item.frame = frame;
rx_item.timestamp_us = AP_HAL::micros64();
rx_item.flags = AP_HAL::CANIface::IsMAVCAN;
add_to_rx_queue(rx_item);
added_to_rx_queue = true;
}
}
return 1;
}
/*
register a callback for for sending CAN_FRAME messages
register a callback for for sending CAN_FRAME messages.
On success the returned callback_id can be used to unregister the callback
*/
bool AP_HAL::CANIface::register_frame_callback(FrameCb cb)
bool AP_HAL::CANIface::register_frame_callback(FrameCb cb, uint8_t &callback_id)
{
frame_callback = cb;
return true;
#ifndef HAL_BOOTLOADER_BUILD
WITH_SEMAPHORE(callbacks.sem);
#endif
for (uint8_t i=0; i<ARRAY_SIZE(callbacks.cb); i++) {
if (callbacks.cb[i] == nullptr) {
callbacks.cb[i] = cb;
callback_id = i+1;
return true;
}
}
return false;
}
/*
unregister a callback for for sending CAN_FRAME messages
*/
void AP_HAL::CANIface::unregister_frame_callback(uint8_t callback_id)
{
#ifndef HAL_BOOTLOADER_BUILD
WITH_SEMAPHORE(callbacks.sem);
#endif
const uint8_t idx = callback_id - 1;
if (idx < ARRAY_SIZE(callbacks.cb)) {
callbacks.cb[idx] = nullptr;
}
}
AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) :

12
libraries/AP_HAL/CANIface.h
View File

@ -262,13 +262,21 @@ public:
FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
// register a frame callback function
virtual bool register_frame_callback(FrameCb cb);
virtual bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
virtual void unregister_frame_callback(uint8_t cb_id);
protected:
virtual int8_t get_iface_num() const = 0;
virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
FrameCb frame_callback;
struct {
#ifndef HAL_BOOTLOADER_BUILD
HAL_Semaphore sem;
#endif
// allow up to 2 callbacks per interface
FrameCb cb[2];
} callbacks;
uint32_t bitrate_;
OperatingMode mode_;
};