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

AP_CANManager: update for new callback API

This commit is contained in:
Andrew Tridgell 2024-09-08 16:11:50 +10:00
parent 4a102e2f2b
commit cc930bd49f
2 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_CANManager/AP_CANManager.cpp
View File

@ -428,8 +428,8 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
const int8_t bus = int8_t(packet.param1)-1;
if (bus == -1) {
for (auto can_iface : hal.can) {
if (can_iface) {
can_iface->register_frame_callback(nullptr);
if (can_iface && can_forward.callback_id != 0) {
can_iface->unregister_frame_callback(can_forward.callback_id);
}
}
return true;
@ -437,8 +437,9 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) {
return false;
}
if (!hal.can[bus]->register_frame_callback(
FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &))) {
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)) {
return false;
}
can_forward.last_callback_enable_ms = AP_HAL::millis();
@ -448,8 +449,8 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
// 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) {
hal.can[i]->register_frame_callback(nullptr);
if (i != bus && hal.can[i] != nullptr && can_forward.callback_id != 0) {
hal.can[i]->unregister_frame_callback(can_forward.callback_id);
}
}
@ -643,8 +644,9 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
// check every 100 frames for disabling CAN_FRAME send
// we stop sending after 5s if the client stops
// sending MAV_CMD_CAN_FORWARD requests
if (AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) {
hal.can[bus]->register_frame_callback(nullptr);
if (can_forward.callback_id != 0 &&
AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) {
hal.can[bus]->unregister_frame_callback(can_forward.callback_id);
return;
}
can_forward.frame_counter = 0;

1
libraries/AP_CANManager/AP_CANManager.h
View File

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