mirror of https://github.com/ArduPilot/ardupilot
AP_CANManager: update for new callback API
This commit is contained in:
parent
4a102e2f2b
commit
cc930bd49f
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue