Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_OpenDroneID: replace libuavcan with libcanard based driver

Browse Source
This commit is contained in:
bugobliterator 2023-01-05 17:28:18 +11:00 committed by Andrew Tridgell
parent fcfc4ce889
commit 4262f506c1
1 changed files with 31 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp
View File

@ -23,26 +23,15 @@
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
#if HAL_ENABLE_LIBUAVCAN_DRIVERS #if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <dronecan/remoteid/Location.hpp>
#include <dronecan/remoteid/BasicID.hpp>
#include <dronecan/remoteid/SelfID.hpp>
#include <dronecan/remoteid/OperatorID.hpp>
#include <dronecan/remoteid/System.hpp>
#include <dronecan/remoteid/ArmStatus.hpp>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
static uavcan::Publisher<dronecan::remoteid::Location>* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static Canard::Publisher<dronecan_remoteid_Location>* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::BasicID>* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static Canard::Publisher<dronecan_remoteid_BasicID>* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::SelfID>* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static Canard::Publisher<dronecan_remoteid_SelfID>* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::System>* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static Canard::Publisher<dronecan_remoteid_System>* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::OperatorID>* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static Canard::Publisher<dronecan_remoteid_OperatorID>* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// handle ArmStatus static void handle_arm_status(AP_UAVCAN* ap_uavcan, const CanardRxTransfer &transfer, const dronecan_remoteid_ArmStatus &msg);
UC_REGISTRY_BINDER(ArmStatusCb, dronecan::remoteid::ArmStatus);
static uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusCb> *arm_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static void handle_arm_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ArmStatusCb &cb);
void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan) void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan)
{ {
@ -53,46 +42,45 @@ void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan)
return; return;
} }
dc_location[driver_index] = new uavcan::Publisher<dronecan::remoteid::Location>(*uavcan->get_node()); dc_location[driver_index] = new Canard::Publisher<dronecan_remoteid_Location>(uavcan->get_canard_iface());
if (dc_location[driver_index] == nullptr) { if (dc_location[driver_index] == nullptr) {
goto alloc_failed; goto alloc_failed;
} }
dc_location[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); dc_location[driver_index]->set_timeout_ms(20);
dc_location[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); dc_location[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
dc_basic_id[driver_index] = new uavcan::Publisher<dronecan::remoteid::BasicID>(*uavcan->get_node()); dc_basic_id[driver_index] = new Canard::Publisher<dronecan_remoteid_BasicID>(uavcan->get_canard_iface());
if (dc_basic_id[driver_index] == nullptr) { if (dc_basic_id[driver_index] == nullptr) {
goto alloc_failed; goto alloc_failed;
} }
dc_basic_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); dc_basic_id[driver_index]->set_timeout_ms(20);
dc_basic_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); dc_basic_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
dc_self_id[driver_index] = new uavcan::Publisher<dronecan::remoteid::SelfID>(*uavcan->get_node()); dc_self_id[driver_index] = new Canard::Publisher<dronecan_remoteid_SelfID>(uavcan->get_canard_iface());
if (dc_self_id[driver_index] == nullptr) { if (dc_self_id[driver_index] == nullptr) {
goto alloc_failed; goto alloc_failed;
} }
dc_self_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); dc_self_id[driver_index]->set_timeout_ms(20);
dc_self_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); dc_self_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
dc_system[driver_index] = new uavcan::Publisher<dronecan::remoteid::System>(*uavcan->get_node()); dc_system[driver_index] = new Canard::Publisher<dronecan_remoteid_System>(uavcan->get_canard_iface());
if (dc_system[driver_index] == nullptr) { if (dc_system[driver_index] == nullptr) {
goto alloc_failed; goto alloc_failed;
} }
dc_system[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); dc_system[driver_index]->set_timeout_ms(20);
dc_system[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); dc_system[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
dc_operator_id[driver_index] = new uavcan::Publisher<dronecan::remoteid::OperatorID>(*uavcan->get_node()); dc_operator_id[driver_index] = new Canard::Publisher<dronecan_remoteid_OperatorID>(uavcan->get_canard_iface());
if (dc_operator_id[driver_index] == nullptr) { if (dc_operator_id[driver_index] == nullptr) {
goto alloc_failed; goto alloc_failed;
} }
dc_operator_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); dc_operator_id[driver_index]->set_timeout_ms(20);
dc_operator_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); dc_operator_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
arm_status_listener[driver_index] = new uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusCb>(*uavcan->get_node()); if (Canard::allocate_sub_arg_callback(uavcan, &handle_arm_status, driver_index) == nullptr)
if (arm_status_listener[driver_index] == nullptr) { {
goto alloc_failed; goto alloc_failed;
} }
arm_status_listener[driver_index]->start(ArmStatusCb(uavcan, &handle_arm_status));
dronecan_done_init |= driver_mask; dronecan_done_init |= driver_mask;
return; return;
@ -149,12 +137,12 @@ void AP_OpenDroneID::dronecan_send(AP_UAVCAN *uavcan)
} }
#define ODID_COPY(name) msg.name = pkt.name #define ODID_COPY(name) msg.name = pkt.name
#define ODID_COPY_STR(name) do { for (uint8_t i = 0; i<sizeof(pkt.name) && pkt.name[i]; i++) msg.name.push_back(pkt.name[i]); } while(0) #define ODID_COPY_STR(name) do { msg.name.len = strncpy_noterm((char*)msg.name.data, (const char*)pkt.name, sizeof(msg.name.data)); } while(0)
void AP_OpenDroneID::dronecan_send_location(AP_UAVCAN *uavcan) void AP_OpenDroneID::dronecan_send_location(AP_UAVCAN *uavcan)
{ {
dronecan::remoteid::Location msg {}; dronecan_remoteid_Location msg {};
const auto &pkt = pkt_location; const auto &pkt = pkt_location;
ODID_COPY_STR(id_or_mac); ODID_COPY_STR(id_or_mac);
ODID_COPY(status); ODID_COPY(status);
@ -178,7 +166,7 @@ void AP_OpenDroneID::dronecan_send_location(AP_UAVCAN *uavcan)
void AP_OpenDroneID::dronecan_send_basic_id(AP_UAVCAN *uavcan) void AP_OpenDroneID::dronecan_send_basic_id(AP_UAVCAN *uavcan)
{ {
dronecan::remoteid::BasicID msg {}; dronecan_remoteid_BasicID msg {};
const auto &pkt = pkt_basic_id; const auto &pkt = pkt_basic_id;
ODID_COPY_STR(id_or_mac); ODID_COPY_STR(id_or_mac);
ODID_COPY(id_type); ODID_COPY(id_type);
@ -189,7 +177,7 @@ void AP_OpenDroneID::dronecan_send_basic_id(AP_UAVCAN *uavcan)
void AP_OpenDroneID::dronecan_send_system(AP_UAVCAN *uavcan) void AP_OpenDroneID::dronecan_send_system(AP_UAVCAN *uavcan)
{ {
dronecan::remoteid::System msg {}; dronecan_remoteid_System msg {};
const auto &pkt = pkt_system; const auto &pkt = pkt_system;
ODID_COPY_STR(id_or_mac); ODID_COPY_STR(id_or_mac);
ODID_COPY(operator_location_type); ODID_COPY(operator_location_type);
@ -209,7 +197,7 @@ void AP_OpenDroneID::dronecan_send_system(AP_UAVCAN *uavcan)
void AP_OpenDroneID::dronecan_send_self_id(AP_UAVCAN *uavcan) void AP_OpenDroneID::dronecan_send_self_id(AP_UAVCAN *uavcan)
{ {
dronecan::remoteid::SelfID msg {}; dronecan_remoteid_SelfID msg {};
const auto &pkt = pkt_self_id; const auto &pkt = pkt_self_id;
ODID_COPY_STR(id_or_mac); ODID_COPY_STR(id_or_mac);
ODID_COPY(description_type); ODID_COPY(description_type);
@ -219,7 +207,7 @@ void AP_OpenDroneID::dronecan_send_self_id(AP_UAVCAN *uavcan)
void AP_OpenDroneID::dronecan_send_operator_id(AP_UAVCAN *uavcan) void AP_OpenDroneID::dronecan_send_operator_id(AP_UAVCAN *uavcan)
{ {
dronecan::remoteid::OperatorID msg {}; dronecan_remoteid_OperatorID msg {};
const auto &pkt = pkt_operator_id; const auto &pkt = pkt_operator_id;
ODID_COPY_STR(id_or_mac); ODID_COPY_STR(id_or_mac);
ODID_COPY(operator_id_type); ODID_COPY(operator_id_type);
@ -230,13 +218,12 @@ void AP_OpenDroneID::dronecan_send_operator_id(AP_UAVCAN *uavcan)
/* /*
handle ArmStatus message from DroneCAN handle ArmStatus message from DroneCAN
*/ */
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)
{ {
const auto &msg = *cb.msg;
mavlink_open_drone_id_arm_status_t status {}; mavlink_open_drone_id_arm_status_t status {};
status.status = msg.status; status.status = msg.status;
strncpy_noterm(status.error, msg.error.c_str(), sizeof(status.error)); strncpy_noterm((char*)status.error, (const char*)msg.error.data, sizeof(status.error));
AP::opendroneid().set_arm_status(status); AP::opendroneid().set_arm_status(status);