diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp index bd7a1df864..8d374ac108 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp @@ -23,26 +23,15 @@ #include #if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include -#include -#include -#include -#include -#include - #include -static uavcan::Publisher* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -// handle ArmStatus -UC_REGISTRY_BINDER(ArmStatusCb, dronecan::remoteid::ArmStatus); -static uavcan::Subscriber *arm_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -static void handle_arm_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ArmStatusCb &cb); +static void handle_arm_status(AP_UAVCAN* ap_uavcan, const CanardRxTransfer &transfer, const dronecan_remoteid_ArmStatus &msg); void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan) { @@ -53,46 +42,45 @@ void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan) return; } - dc_location[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_location[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_location[driver_index] == nullptr) { goto alloc_failed; } - dc_location[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_location[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_location[driver_index]->set_timeout_ms(20); + dc_location[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_basic_id[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_basic_id[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_basic_id[driver_index] == nullptr) { goto alloc_failed; } - dc_basic_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_basic_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_basic_id[driver_index]->set_timeout_ms(20); + dc_basic_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_self_id[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_self_id[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_self_id[driver_index] == nullptr) { goto alloc_failed; } - dc_self_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_self_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_self_id[driver_index]->set_timeout_ms(20); + dc_self_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_system[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_system[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_system[driver_index] == nullptr) { goto alloc_failed; } - dc_system[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_system[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_system[driver_index]->set_timeout_ms(20); + dc_system[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_operator_id[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_operator_id[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_operator_id[driver_index] == nullptr) { goto alloc_failed; } - dc_operator_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_operator_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_operator_id[driver_index]->set_timeout_ms(20); + dc_operator_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - arm_status_listener[driver_index] = new uavcan::Subscriber(*uavcan->get_node()); - if (arm_status_listener[driver_index] == nullptr) { + if (Canard::allocate_sub_arg_callback(uavcan, &handle_arm_status, driver_index) == nullptr) + { goto alloc_failed; } - arm_status_listener[driver_index]->start(ArmStatusCb(uavcan, &handle_arm_status)); dronecan_done_init |= driver_mask; return; @@ -149,12 +137,12 @@ void AP_OpenDroneID::dronecan_send(AP_UAVCAN *uavcan) } #define ODID_COPY(name) msg.name = pkt.name -#define ODID_COPY_STR(name) do { for (uint8_t i = 0; i