From b1c4cf9923f2f5487c88185e58fc82217e008258 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 17 Apr 2023 21:33:56 +1000 Subject: [PATCH] AP_DroneCAN: fix DroneCAN run for SITL --- libraries/AP_DroneCAN/AP_Canard_iface.cpp | 4 +++- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 12 +++--------- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index 8f302e1c0f..57f5c0bb61 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -4,6 +4,8 @@ #if HAL_ENABLE_DRONECAN_DRIVERS #include #include +#include + extern const AP_HAL::HAL& hal; #define LOG_TAG "DroneCANIface" @@ -274,7 +276,7 @@ void CanardInterface::process(uint32_t duration_ms) { processTx(); uint64_t now = AP_HAL::native_micros64(); if (now < deadline) { - _event_handle.wait(deadline - now); + _event_handle.wait(MIN(UINT16_MAX - 2U, deadline - now)); } else { break; } diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 657e7f62b2..1d7c68b0a7 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -312,15 +312,9 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) node_status_msg.sub_mode = 0; // Spin node for device discovery - uint32_t start_loop_ms = AP_HAL::millis(); - uint32_t last_send_ms = 0; - while (AP_HAL::millis() - start_loop_ms < 5000) { - uint32_t now = AP_HAL::millis(); - if (now - last_send_ms >= 1000) { - send_node_status(); - last_send_ms = now; - } - canard_iface.process(50); + for (uint8_t i = 0; i < 5; i++) { + send_node_status(); + canard_iface.process(1000); } hal.util->snprintf(_thread_name, sizeof(_thread_name), "dronecan_%u", driver_index);