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

AP_CANManager: ensure we only remove our own fwd registrations 

keep a record of which bus we have registered a callback for and only
unregister with that bus. This prevents us unregistering a multicast
callback when disconnecting from MAVCAN
This commit is contained in:
Andrew Tridgell 2024-09-22 07:58:21 +10:00
parent b98c7c5296
commit 89c2b48286
2 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_CANManager/AP_CANManager.cpp
View File

@ -426,39 +426,43 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
{
WITH_SEMAPHORE(can_forward.sem);
const int8_t bus = int8_t(packet.param1)-1;
if (bus == -1) {
/*
a request to stop forwarding
*/
for (auto can_iface : hal.can) {
if (can_iface && can_forward.callback_id != 0) {
can_iface->unregister_frame_callback(can_forward.callback_id);
if (can_forward.callback_id != 0) {
hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id);
can_forward.callback_id = 0;
}
}
return true;
}
if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) {
return false;
}
if (can_forward.callback_id != 0 && can_forward.callback_bus != bus) {
/*
the client is changing which bus they are monitoring, unregister from the previous bus
*/
hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id);
can_forward.callback_id = 0;
}
if (can_forward.callback_id == 0 &&
!hal.can[bus]->register_frame_callback(
FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &), can_forward.callback_id)) {
// failed to register the callback
return false;
}
can_forward.callback_bus = bus;
can_forward.last_callback_enable_ms = AP_HAL::millis();
can_forward.chan = chan;
can_forward.system_id = msg.sysid;
can_forward.component_id = msg.compid;
// remove registration on other buses, allowing for bus change in the GUI tool
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
if (i != bus && hal.can[i] != nullptr && can_forward.callback_id != 0) {
hal.can[i]->unregister_frame_callback(can_forward.callback_id);
can_forward.callback_id = 0;
}
}
return true;
}
@ -645,6 +649,10 @@ void AP_CANManager::handle_can_filter_modify(const mavlink_message_t &msg)
void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
{
WITH_SEMAPHORE(can_forward.sem);
if (bus != can_forward.callback_bus) {
// we are not registered for forwarding this bus, discard frame
return;
}
if (can_forward.frame_counter++ == 100) {
// check every 100 frames for disabling CAN_FRAME send
// we stop sending after 5s if the client stops

1
libraries/AP_CANManager/AP_CANManager.h
View File

@ -186,6 +186,7 @@ private:
uint16_t num_filter_ids;
uint16_t *filter_ids;
uint8_t callback_id;
uint8_t callback_bus;
} can_forward;
// buffer for MAVCAN frames