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

AP_UAVCAN: move libuavcan to libcanard driver

This commit is contained in:
bugobliterator 2023-01-05 12:09:23 +11:00 committed by Andrew Tridgell
parent cb628e6875
commit 7067e9d917
13 changed files with 920 additions and 1406 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

309
libraries/AP_UAVCAN/AP_Canard_iface.cpp Normal file
View File

@ -0,0 +1,309 @@
#include "AP_Canard_iface.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_CANManager/AP_CANManager.h>
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <canard/handler_list.h>
#include <canard/transfer_object.h>
extern const AP_HAL::HAL& hal;
#define LOG_TAG "DroneCANIface"
#define DEBUG_PKTS 0
DEFINE_HANDLER_LIST_HEADS();
DEFINE_TRANSFER_OBJECT_HEADS();
#if AP_TEST_DRONECAN_DRIVERS
CanardInterface* CanardInterface::canard_ifaces[] = {nullptr, nullptr, nullptr};
CanardInterface CanardInterface::test_iface{2};
uint8_t test_node_mem_area[1024];
HAL_Semaphore test_iface_sem;
#endif
CanardInterface::CanardInterface(uint8_t iface_index) :
Interface(iface_index) {
#if AP_TEST_DRONECAN_DRIVERS
if (iface_index < 3) {
canard_ifaces[iface_index] = this;
}
if (iface_index == 0) {
test_iface.init(test_node_mem_area, sizeof(test_node_mem_area), 125);
}
#endif
}
void CanardInterface::init(void* mem_arena, size_t mem_arena_size, uint8_t node_id) {
canardInit(&canard, mem_arena, mem_arena_size, onTransferReception, shouldAcceptTransfer, this);
canardSetLocalNodeID(&canard, node_id);
initialized = true;
}
bool CanardInterface::broadcast(const Canard::Transfer &bcast_transfer) {
if (!initialized) {
return false;
}
WITH_SEMAPHORE(_sem);
#if AP_TEST_DRONECAN_DRIVERS
if (this == &test_iface) {
test_iface_sem.take_blocking();
}
#endif
// do canard broadcast
bool success = canardBroadcast(&canard,
bcast_transfer.data_type_signature,
bcast_transfer.data_type_id,
bcast_transfer.inout_transfer_id,
bcast_transfer.priority,
bcast_transfer.payload,
bcast_transfer.payload_len,
AP_HAL::native_micros64() + (bcast_transfer.timeout_ms * 1000)
#if CANARD_MULTI_IFACE
, ((1<<num_ifaces) - 1) // send over all ifaces
#endif
#if HAL_CANFD_SUPPORTED
, bcast_transfer.canfd
#endif
) > 0;
#if AP_TEST_DRONECAN_DRIVERS
if (this == &test_iface) {
test_iface_sem.give();
}
#endif
return success;
}
bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) {
if (!initialized) {
return false;
}
WITH_SEMAPHORE(_sem);
// do canard request
return canardRequestOrRespond(&canard,
destination_node_id,
req_transfer.data_type_signature,
req_transfer.data_type_id,
req_transfer.inout_transfer_id,
req_transfer.priority,
CanardRequest,
req_transfer.payload,
req_transfer.payload_len,
AP_HAL::native_micros64() + (req_transfer.timeout_ms * 1000)
#if CANARD_MULTI_IFACE
, ((1<<num_ifaces) - 1) // send over all ifaces
#endif
#if HAL_CANFD_SUPPORTED
, false
#endif
) > 0;
}
bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) {
if (!initialized) {
return false;
}
WITH_SEMAPHORE(_sem);
// do canard respond
return canardRequestOrRespond(&canard,
destination_node_id,
res_transfer.data_type_signature,
res_transfer.data_type_id,
res_transfer.inout_transfer_id,
res_transfer.priority,
CanardResponse,
res_transfer.payload,
res_transfer.payload_len,
AP_HAL::native_micros64() + (res_transfer.timeout_ms * 1000)
#if CANARD_MULTI_IFACE
, ((1<<num_ifaces) - 1) // send over all ifaces
#endif
#if HAL_CANFD_SUPPORTED
, false
#endif
) > 0;
}
void CanardInterface::onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer) {
CanardInterface* iface = (CanardInterface*) ins->user_reference;
iface->handle_message(*transfer);
}
bool CanardInterface::shouldAcceptTransfer(const CanardInstance* ins,
uint64_t* out_data_type_signature,
uint16_t data_type_id,
CanardTransferType transfer_type,
uint8_t source_node_id) {
CanardInterface* iface = (CanardInterface*) ins->user_reference;
return iface->accept_message(data_type_id, *out_data_type_signature);
}
#if AP_TEST_DRONECAN_DRIVERS
void CanardInterface::processTestRx() {
if (!test_iface.initialized) {
return;
}
WITH_SEMAPHORE(test_iface_sem);
for (const CanardCANFrame* txf = canardPeekTxQueue(&test_iface.canard); txf != NULL; txf = canardPeekTxQueue(&test_iface.canard)) {
if (canard_ifaces[0]) {
canardHandleRxFrame(&canard_ifaces[0]->canard, txf, AP_HAL::native_micros64());
}
canardPopTxQueue(&test_iface.canard);
}
}
#endif
void CanardInterface::processTx() {
WITH_SEMAPHORE(_sem);
for (uint8_t iface = 0; iface < num_ifaces; iface++) {
if (ifaces[iface] == NULL) {
continue;
}
auto txq = canard.tx_queue;
if (txq == nullptr) {
return;
}
AP_HAL::CANFrame txmsg {};
// scan through list of pending transfers
while (true) {
auto txf = &txq->frame;
txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len);
memcpy(txmsg.data, txf->data, txf->data_len);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
#if HAL_CANFD_SUPPORTED
txmsg.canfd = txf->canfd;
#endif
bool write = true;
bool read = false;
ifaces[iface]->select(read, write, &txmsg, 0);
if ((AP_HAL::native_micros64() < txf->deadline_usec) && (txf->iface_mask & (1U<<iface)) && write) {
// try sending to interfaces, clearing the mask if we succeed
if (ifaces[iface]->send(txmsg, txf->deadline_usec, 0) > 0) {
txf->iface_mask &= ~(1U<<iface);
} else {
// if we fail to send then we try sending on next interface
break;
}
}
// look at next transfer
txq = txq->next;
if (txq == nullptr) {
break;
}
}
}
// purge expired transfers
for (const CanardCANFrame* txf = canardPeekTxQueue(&canard); txf != NULL; txf = canardPeekTxQueue(&canard)) {
if ((AP_HAL::native_micros64() >= txf->deadline_usec) || (txf->iface_mask == 0)) {
canardPopTxQueue(&canard);
} else {
break;
}
}
}
void CanardInterface::processRx() {
AP_HAL::CANFrame rxmsg;
for (uint8_t i=0; i<num_ifaces; i++) {
while(true) {
if (ifaces[i] == NULL) {
break;
}
bool read_select = true;
bool write_select = false;
ifaces[i]->select(read_select, write_select, nullptr, 0);
if (!read_select) { // No data pending
break;
}
CanardCANFrame rx_frame {};
//palToggleLine(HAL_GPIO_PIN_LED);
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) {
break;
}
rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc);
memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len);
#if HAL_CANFD_SUPPORTED
rx_frame.canfd = rxmsg.canfd;
#endif
rx_frame.id = rxmsg.id;
#if CANARD_MULTI_IFACE
rx_frame.iface_id = i;
#endif
{
WITH_SEMAPHORE(_sem);
#if DEBUG_PKTS
const int16_t res =
#endif
canardHandleRxFrame(&canard, &rx_frame, timestamp);
#if DEBUG_PKTS
// hal.console->printf("DTID: %u\n", extractDataType(rx_frame.id));
// hal.console->printf("Rx %d, IF%d %lx: ", res, i, rx_frame.id);
if (res < 0 &&
res != -CANARD_ERROR_RX_NOT_WANTED &&
res != -CANARD_ERROR_RX_WRONG_ADDRESS) {
hal.console->printf("Rx error %d, IF%d %lx: \n", res, i, rx_frame.id);
// for (uint8_t index = 0; index < rx_frame.data_len; index++) {
// hal.console->printf("%02x", rx_frame.data[index]);
// }
// hal.console->printf("\n");
}
#endif
}
}
}
}
void CanardInterface::process(uint32_t duration_ms) {
#if AP_TEST_DRONECAN_DRIVERS
const uint64_t deadline = AP_HAL::micros64() + duration_ms*1000;
while (AP_HAL::micros64() < deadline) {
processTestRx();
hal.scheduler->delay_microseconds(1000);
}
#else
const uint64_t deadline = AP_HAL::native_micros64() + duration_ms*1000;
while (true) {
processRx();
processTx();
uint64_t now = AP_HAL::native_micros64();
if (now < deadline) {
_event_handle.wait(deadline - now);
} else {
break;
}
}
#endif
}
bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface)
{
if (num_ifaces > HAL_NUM_CAN_IFACES) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Num Ifaces Exceeded\n");
return false;
}
if (can_iface == nullptr) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface Null\n");
return false;
}
if (ifaces[num_ifaces] != nullptr) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface already added\n");
return false;
}
ifaces[num_ifaces] = can_iface;
if (ifaces[num_ifaces] == nullptr) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Can't alloc uavcan::iface\n");
return false;
}
if (!can_iface->set_event_handle(&_event_handle)) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Setting event handle failed\n");
return false;
}
AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "DroneCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces));
num_ifaces++;
return true;
}
#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS

68
libraries/AP_UAVCAN/AP_Canard_iface.h Normal file
View File

@ -0,0 +1,68 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <canard/interface.h>
class AP_UAVCAN;
class CanardInterface : public Canard::Interface {
friend class AP_UAVCAN;
public:
/// @brief delete copy constructor and assignment operator
CanardInterface(const CanardInterface&) = delete;
CanardInterface& operator=(const CanardInterface&) = delete;
CanardInterface(uint8_t driver_index);
void init(void* mem_arena, size_t mem_arena_size, uint8_t node_id);
/// @brief broadcast message to all listeners on Interface
/// @param bc_transfer
/// @return true if message was added to the queue
bool broadcast(const Canard::Transfer &bcast_transfer) override;
/// @brief request message from
/// @param destination_node_id
/// @param req_transfer
/// @return true if request was added to the queue
bool request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) override;
/// @brief respond to a request
/// @param destination_node_id
/// @param res_transfer
/// @return true if response was added to the queue
bool respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) override;
void processTx();
void processRx();
void process(uint32_t duration);
static void onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer);
static bool shouldAcceptTransfer(const CanardInstance* ins,
uint64_t* out_data_type_signature,
uint16_t data_type_id,
CanardTransferType transfer_type,
uint8_t source_node_id);
bool add_interface(AP_HAL::CANIface *can_drv);
#if AP_TEST_DRONECAN_DRIVERS
static CanardInterface& get_test_iface() { return test_iface; }
static void processTestRx();
#endif
uint8_t get_node_id() const override { return canard.node_id; }
private:
CanardInstance canard;
AP_HAL::CANIface* ifaces[HAL_NUM_CAN_IFACES];
#if AP_TEST_DRONECAN_DRIVERS
static CanardInterface* canard_ifaces[3];
static CanardInterface test_iface;
#endif
uint8_t num_ifaces;
HAL_EventHandle _event_handle;
bool initialized;
HAL_Semaphore _sem;
};
#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS

707
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

File diff suppressed because it is too large Load Diff

210
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -20,17 +20,20 @@
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <uavcan/uavcan.hpp>
#include "AP_UAVCAN_IfaceMgr.h"
#include "AP_UAVCAN_Clock.h"
#include "AP_Canard_iface.h"
#include <AP_CANManager/AP_CANManager.h>
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <SRV_Channel/SRV_Channel_config.h>
#include <canard/publisher.h>
#include <canard/subscriber.h>
#include <canard/service_client.h>
#include <canard/service_server.h>
#include <stdio.h>
#include "AP_UAVCAN_DNA_Server.h"
#include <canard.h>
#include <dronecan_msgs.h>
#ifndef UAVCAN_SRV_NUMBER
#define UAVCAN_SRV_NUMBER NUM_SERVO_CHANNELS
@ -49,63 +52,12 @@
#define AP_UAVCAN_MAX_LED_DEVICES 4
// fwd-declare callback classes
class ButtonCb;
class TrafficReportCb;
class ActuatorStatusCb;
class ActuatorStatusVolzCb;
class ESCStatusCb;
class DebugCb;
class ParamGetSetCb;
class ParamExecuteOpcodeCb;
class AP_PoolAllocator;
class AP_UAVCAN_DNA_Server;
#if defined(__GNUC__) && (__GNUC__ > 8)
#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
_Pragma("GCC diagnostic push") \
_Pragma("GCC diagnostic ignored \"-Wcast-function-type\"")
#define DISABLE_W_CAST_FUNCTION_TYPE_POP \
_Pragma("GCC diagnostic pop")
#else
#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH
#define DISABLE_W_CAST_FUNCTION_TYPE_POP
#endif
#if defined(__GNUC__) && (__GNUC__ >= 11)
#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID (void*)
#else
#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID
#endif
/*
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
the Callback will invoke registry to register the node as separate backend.
*/
#define UC_REGISTRY_BINDER(ClassName_, DataType_) \
class ClassName_ : public AP_UAVCAN::RegistryBinder<DataType_> { \
typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
public: \
ClassName_() : RegistryBinder() {} \
DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \
RegistryBinder(uc, (Registry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
DISABLE_W_CAST_FUNCTION_TYPE_POP \
}
#define UC_CLIENT_CALL_REGISTRY_BINDER(ClassName_, DataType_) \
class ClassName_ : public AP_UAVCAN::ClientCallRegistryBinder<DataType_> { \
typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
public: \
ClassName_() : ClientCallRegistryBinder() {} \
DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \
ClientCallRegistryBinder(uc, (ClientCallRegistry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
DISABLE_W_CAST_FUNCTION_TYPE_POP \
}
class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
friend class AP_UAVCAN_DNA_Server;
public:
AP_UAVCAN();
AP_UAVCAN(const int driver_index);
~AP_UAVCAN();
static const struct AP_Param::GroupInfo var_info[];
@ -117,13 +69,14 @@ public:
void init(uint8_t driver_index, bool enable_filters) override;
bool add_interface(AP_HAL::CANIface* can_iface) override;
uavcan::Node<0>* get_node() { return _node; }
uint8_t get_driver_index() const { return _driver_index; }
FUNCTOR_TYPEDEF(ParamGetSetIntCb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &);
FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_UAVCAN*, const uint8_t, const char*, float &);
FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_UAVCAN*, const uint8_t, bool);
void send_node_status();
///// SRV output /////
void SRV_push_servos(void);
@ -138,7 +91,7 @@ public:
// Send Reboot command
// Note: Do not call this from outside UAVCAN thread context,
// you can call this from uavcan callbacks and handlers.
// you can call this from dronecan callbacks and handlers.
// THIS IS NOT A THREAD SAFE API!
void send_reboot_request(uint8_t node_id);
@ -151,57 +104,6 @@ public:
// Save parameters
bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb);
template <typename DataType_>
class RegistryBinder {
protected:
typedef void (*Registry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const RegistryBinder& _cb);
AP_UAVCAN* _uc;
Registry _ffunc;
public:
RegistryBinder() :
_uc(),
_ffunc(),
msg() {}
RegistryBinder(AP_UAVCAN* uc, Registry ffunc) :
_uc(uc),
_ffunc(ffunc),
msg(nullptr) {}
void operator()(const uavcan::ReceivedDataStructure<DataType_>& _msg) {
msg = &_msg;
_ffunc(_uc, _msg.getSrcNodeID().get(), *this);
}
const uavcan::ReceivedDataStructure<DataType_> *msg;
};
// ClientCallRegistryBinder
template <typename DataType_>
class ClientCallRegistryBinder {
protected:
typedef void (*ClientCallRegistry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const ClientCallRegistryBinder& _cb);
AP_UAVCAN* _uc;
ClientCallRegistry _ffunc;
public:
ClientCallRegistryBinder() :
_uc(),
_ffunc(),
rsp() {}
ClientCallRegistryBinder(AP_UAVCAN* uc, ClientCallRegistry ffunc) :
_uc(uc),
_ffunc(ffunc),
rsp(nullptr) {}
void operator()(const uavcan::ServiceCallResult<DataType_>& _rsp) {
rsp = &_rsp;
_ffunc(_uc, _rsp.getCallID().server_node_id.get(), *this);
}
const uavcan::ServiceCallResult<DataType_> *rsp;
};
// options bitmask
enum class Options : uint16_t {
DNA_CLEAR_DATABASE = (1U<<0),
@ -221,9 +123,7 @@ public:
// 0. return true if it was set
bool check_and_reset_option(Options option);
// This will be needed to implement if UAVCAN is used with multithreading
// Such cases will be firmware update, etc.
class RaiiSynchronizer {};
CanardInterface& get_canard_iface() { return canard_iface; }
private:
void loop(void);
@ -270,7 +170,7 @@ private:
uint8_t param_save_request_node_id;
// UAVCAN parameters
AP_Int8 _uavcan_node;
AP_Int8 _dronecan_node;
AP_Int32 _servo_bm;
AP_Int32 _esc_bm;
AP_Int8 _esc_offset;
@ -279,14 +179,12 @@ private:
AP_Int16 _notify_state_hz;
AP_Int16 _pool_size;
AP_PoolAllocator *_allocator;
AP_UAVCAN_DNA_Server *_dna_server;
uint32_t *mem_pool;
uavcan::Node<0> *_node;
AP_UAVCAN_DNA_Server _dna_server;
uint8_t _driver_index;
uavcan::CanIfaceMgr* _iface_mgr;
char _thread_name[13];
bool _initialized;
///// SRV output /////
@ -355,22 +253,80 @@ private:
static HAL_Semaphore _telem_sem;
// node status send
uint32_t _node_status_last_send_ms;
// safety status send state
uint32_t _last_safety_state_ms;
// notify vehicle state
uint32_t _last_notify_state_ms;
uavcan_protocol_NodeStatus node_status_msg;
CanardInterface canard_iface;
Canard::Publisher<uavcan_protocol_NodeStatus> node_status{canard_iface};
Canard::Publisher<uavcan_equipment_actuator_ArrayCommand> act_out_array{canard_iface};
Canard::Publisher<uavcan_equipment_esc_RawCommand> esc_raw{canard_iface};
Canard::Publisher<uavcan_equipment_indication_LightsCommand> rgb_led{canard_iface};
Canard::Publisher<uavcan_equipment_indication_BeepCommand> buzzer{canard_iface};
Canard::Publisher<ardupilot_indication_SafetyState> safety_state{canard_iface};
Canard::Publisher<uavcan_equipment_safety_ArmingStatus> arming_status{canard_iface};
Canard::Publisher<uavcan_equipment_gnss_RTCMStream> rtcm_stream{canard_iface};
Canard::Publisher<ardupilot_indication_NotifyState> notify_state{canard_iface};
#if AP_DRONECAN_SEND_GPS
Canard::Publisher<uavcan_equipment_gnss_Fix2> gnss_fix2{canard_iface};
Canard::Publisher<uavcan_equipment_gnss_Auxiliary> gnss_auxiliary{canard_iface};
Canard::Publisher<ardupilot_gnss_Heading> gnss_heading{canard_iface};
Canard::Publisher<ardupilot_gnss_Status> gnss_status{canard_iface};
#endif
// incoming messages
Canard::ObjCallback<AP_UAVCAN, ardupilot_indication_Button> safety_button_cb{this, &AP_UAVCAN::handle_button};
Canard::Subscriber<ardupilot_indication_Button> safety_button_listener{safety_button_cb, _driver_index};
Canard::ObjCallback<AP_UAVCAN, ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_cb{this, &AP_UAVCAN::handle_traffic_report};
Canard::Subscriber<ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_listener{traffic_report_cb, _driver_index};
Canard::ObjCallback<AP_UAVCAN, uavcan_equipment_actuator_Status> actuator_status_cb{this, &AP_UAVCAN::handle_actuator_status};
Canard::Subscriber<uavcan_equipment_actuator_Status> actuator_status_listener{actuator_status_cb, _driver_index};
Canard::ObjCallback<AP_UAVCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_UAVCAN::handle_ESC_status};
Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_UAVCAN::handle_debug};
Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
// param client
Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_param_GetSetResponse> param_get_set_res_cb{this, &AP_UAVCAN::handle_param_get_set_response};
Canard::Client<uavcan_protocol_param_GetSetResponse> param_get_set_client{canard_iface, param_get_set_res_cb};
Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_param_ExecuteOpcodeResponse> param_save_res_cb{this, &AP_UAVCAN::handle_param_save_response};
Canard::Client<uavcan_protocol_param_ExecuteOpcodeResponse> param_save_client{canard_iface, param_save_res_cb};
// reboot client
void handle_restart_node_response(const CanardRxTransfer& transfer, const uavcan_protocol_RestartNodeResponse& msg) {}
Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_RestartNodeResponse> restart_node_res_cb{this, &AP_UAVCAN::handle_restart_node_response};
Canard::Client<uavcan_protocol_RestartNodeResponse> restart_node_client{canard_iface, restart_node_res_cb};
uavcan_protocol_param_ExecuteOpcodeRequest param_save_req;
uavcan_protocol_param_GetSetRequest param_getset_req;
// Node Info Server
Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_GetNodeInfoRequest> node_info_req_cb{this, &AP_UAVCAN::handle_node_info_request};
Canard::Server<uavcan_protocol_GetNodeInfoRequest> node_info_server{canard_iface, node_info_req_cb};
uavcan_protocol_GetNodeInfoResponse node_info_rsp;
// incoming button handling
static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
static void handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb);
static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg);
void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
static bool is_esc_data_index_valid(const uint8_t index);
static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);
static void handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb);
void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);
void handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp);
void handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req);
};
#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS

59
libraries/AP_UAVCAN/AP_UAVCAN_Clock.h
View File

@ -1,59 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
*/
#pragma once
#include <uavcan/uavcan.hpp>
namespace uavcan
{
class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable
{
public:
SystemClock() = default;
void adjustUtc(uavcan::UtcDuration adjustment) override
{
utc_adjustment_usec = adjustment.toUSec();
}
uavcan::MonotonicTime getMonotonic() const override
{
return uavcan::MonotonicTime::fromUSec(AP_HAL::native_micros64());
}
uavcan::UtcTime getUtc() const override
{
return uavcan::UtcTime::fromUSec(AP_HAL::native_micros64() + utc_adjustment_usec);
}
int64_t getAdjustUsec() const
{
return utc_adjustment_usec;
}
static SystemClock& instance()
{
static SystemClock inst;
return inst;
}
private:
int64_t utc_adjustment_usec;
};
}

262
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
View File

@ -24,11 +24,10 @@
#include "AP_UAVCAN.h"
#include <StorageManager/StorageManager.h>
#include <AP_Math/AP_Math.h>
#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_UAVCAN_Clock.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <stdio.h>
extern const AP_HAL::HAL& hal;
#define NODEDATA_MAGIC 0xAC01
@ -37,56 +36,15 @@ extern const AP_HAL::HAL& hal;
#define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0)
//Callback Object Definitions
UC_REGISTRY_BINDER(AllocationCb, uavcan::protocol::dynamic_node_id::Allocation);
UC_REGISTRY_BINDER(NodeStatusCb, uavcan::protocol::NodeStatus);
UC_CLIENT_CALL_REGISTRY_BINDER(GetNodeInfoCb, uavcan::protocol::GetNodeInfo);
static uavcan::ServiceClient<uavcan::protocol::GetNodeInfo, GetNodeInfoCb>* getNodeInfo_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>* allocation_pub[HAL_MAX_CAN_PROTOCOL_DRIVERS];
AP_UAVCAN_DNA_Server::AP_UAVCAN_DNA_Server(AP_UAVCAN *ap_uavcan, StorageAccess _storage) :
AP_UAVCAN_DNA_Server::AP_UAVCAN_DNA_Server(AP_UAVCAN &ap_uavcan) :
_ap_uavcan(ap_uavcan),
storage(_storage)
_canard_iface(ap_uavcan.canard_iface),
storage(StorageManager::StorageCANDNA),
allocation_sub(allocation_cb, _ap_uavcan.get_driver_index()),
node_status_sub(node_status_cb, _ap_uavcan.get_driver_index()),
node_info_client(_canard_iface, node_info_cb)
{}
/* Subscribe to all the messages we are going to handle for
Server registry and Node allocation. */
void AP_UAVCAN_DNA_Server::subscribe_msgs(AP_UAVCAN* ap_uavcan)
{
if (ap_uavcan == nullptr) {
return;
}
auto* node = ap_uavcan->get_node();
//Register Allocation Message Handler
uavcan::Subscriber<uavcan::protocol::dynamic_node_id::Allocation, AllocationCb> *AllocationListener;
AllocationListener = new uavcan::Subscriber<uavcan::protocol::dynamic_node_id::Allocation, AllocationCb>(*node);
if (AllocationListener == nullptr) {
AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: AllocationListener");
}
const int alloc_listener_res = AllocationListener->start(AllocationCb(ap_uavcan, &trampoline_handleAllocation));
if (alloc_listener_res < 0) {
AP_HAL::panic("Allocation Subscriber start problem\n\r");
return;
}
//We allow anonymous transfers, as they are specifically for Node Allocation
AllocationListener->allowAnonymousTransfers();
//Register Node Status Listener
uavcan::Subscriber<uavcan::protocol::NodeStatus, NodeStatusCb> *NodeStatusListener;
NodeStatusListener = new uavcan::Subscriber<uavcan::protocol::NodeStatus, NodeStatusCb>(*node);
if (NodeStatusListener == nullptr) {
AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: NodeStatusListener");
}
const int nodestatus_listener_res = NodeStatusListener->start(NodeStatusCb(ap_uavcan, &trampoline_handleNodeStatus));
if (nodestatus_listener_res < 0) {
AP_HAL::panic("NodeStatus Subscriber start problem\n\r");
return;
}
}
/* Method to generate 6byte hash from the Unique ID.
We return it packed inside the referenced NodeData structure */
void AP_UAVCAN_DNA_Server::getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const
@ -255,43 +213,10 @@ bool AP_UAVCAN_DNA_Server::isValidNodeDataAvailable(uint8_t node_id)
Also resets the Server Record in case there is a mismatch
between specified node id and unique id against the existing
Server Record. */
bool AP_UAVCAN_DNA_Server::init()
bool AP_UAVCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id)
{
//Read the details from ap_uavcan
uavcan::Node<0>* _node = _ap_uavcan->get_node();
uint8_t node_id = _node->getNodeID().get();
uint8_t own_unique_id[16] = {0};
uint8_t own_unique_id_len = 16;
driver_index = _ap_uavcan->get_driver_index();
//copy unique id from node to uint8_t array
uavcan::copy(_node->getHardwareVersion().unique_id.begin(),
_node->getHardwareVersion().unique_id.end(),
own_unique_id);
//Read the details from AP_UAVCAN
server_state = HEALTHY;
//Setup publisher for this driver index
allocation_pub[driver_index] = new uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>(*_node, true);
if (allocation_pub[driver_index] == nullptr) {
AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: allocation_pub[%d]", driver_index);
}
int res = allocation_pub[driver_index]->init(uavcan::TransferPriority::Default);
if (res < 0) {
return false;
}
allocation_pub[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::dynamic_node_id::Allocation::FOLLOWUP_TIMEOUT_MS));
//Setup GetNodeInfo Client
getNodeInfo_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::GetNodeInfo, GetNodeInfoCb>(*_node, GetNodeInfoCb(_ap_uavcan, &trampoline_handleNodeInfo));
if (getNodeInfo_client[driver_index] == nullptr) {
AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: getNodeInfo_client[%d]", driver_index);
}
/* Go through our records and look for valid NodeData, to initialise
occupation mask */
for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
@ -310,7 +235,7 @@ bool AP_UAVCAN_DNA_Server::init()
//Its not there a reset should write it in the Storage
reset();
}
if (_ap_uavcan->check_and_reset_option(AP_UAVCAN::Options::DNA_CLEAR_DATABASE)) {
if (_ap_uavcan.check_and_reset_option(AP_UAVCAN::Options::DNA_CLEAR_DATABASE)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UC DNA database reset");
reset();
}
@ -350,7 +275,7 @@ bool AP_UAVCAN_DNA_Server::init()
addToSeenNodeMask(node_id);
setVerificationMask(node_id);
node_healthy_mask.set(node_id);
self_node_id[driver_index] = node_id;
self_node_id = node_id;
return true;
}
@ -422,7 +347,7 @@ seen list, So that we can raise issue if there are duplicates
on the bus. */
void AP_UAVCAN_DNA_Server::verify_nodes()
{
uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec();
uint32_t now = AP_HAL::millis();
if ((now - last_verification_request) < 5000) {
return;
}
@ -435,7 +360,7 @@ void AP_UAVCAN_DNA_Server::verify_nodes()
//Check if we got acknowledgement from previous request
//except for requests using our own node_id
if (curr_verifying_node == self_node_id[driver_index]) {
if (curr_verifying_node == self_node_id) {
nodeInfo_resp_rcvd = true;
}
@ -456,13 +381,16 @@ void AP_UAVCAN_DNA_Server::verify_nodes()
//Find the next registered Node ID to be verified.
for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
curr_verifying_node = (curr_verifying_node + 1) % (MAX_NODE_ID + 1);
if ((curr_verifying_node == self_node_id) || (curr_verifying_node == 0)) {
continue;
}
if (isNodeSeen(curr_verifying_node)) {
break;
}
}
if (getNodeInfo_client[driver_index] != nullptr && isNodeIDOccupied(curr_verifying_node)) {
uavcan::protocol::GetNodeInfo::Request request;
getNodeInfo_client[driver_index]->call(curr_verifying_node, request);
if (isNodeIDOccupied(curr_verifying_node)) {
uavcan_protocol_GetNodeInfoRequest request;
node_info_client.request(curr_verifying_node, request);
nodeInfo_resp_rcvd = false;
}
}
@ -470,63 +398,50 @@ void AP_UAVCAN_DNA_Server::verify_nodes()
/* Handles Node Status Message, adds to the Seen Node list
Also starts the Service call for Node Info to complete the
Verification process. */
void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb)
void AP_UAVCAN_DNA_Server::handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg)
{
if (node_id > MAX_NODE_ID) {
if (transfer.source_node_id > MAX_NODE_ID || transfer.source_node_id == 0) {
return;
}
if ((cb.msg->health != uavcan::protocol::NodeStatus::HEALTH_OK ||
cb.msg->mode != uavcan::protocol::NodeStatus::MODE_OPERATIONAL) &&
!_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
if ((msg.health != UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK ||
msg.mode != UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL) &&
!_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
//if node is not healthy or operational, clear resp health mask, and set fault_node_id
fault_node_id = node_id;
fault_node_id = transfer.source_node_id;
server_state = NODE_STATUS_UNHEALTHY;
node_healthy_mask.clear(node_id);
node_healthy_mask.clear(transfer.source_node_id);
} else {
node_healthy_mask.set(node_id);
node_healthy_mask.set(transfer.source_node_id);
if (node_healthy_mask == verified_mask) {
server_state = HEALTHY;
}
}
if (!isNodeIDVerified(node_id)) {
if (!isNodeIDVerified(transfer.source_node_id)) {
//immediately begin verification of the node_id
if (getNodeInfo_client[driver_index] != nullptr) {
uavcan::protocol::GetNodeInfo::Request request;
getNodeInfo_client[driver_index]->call(node_id, request);
}
uavcan_protocol_GetNodeInfoRequest request;
node_info_client.request(transfer.source_node_id, request);
}
//Add node to seen list if not seen before
addToSeenNodeMask(node_id);
addToSeenNodeMask(transfer.source_node_id);
}
//Trampoline call for handleNodeStatus member method
void AP_UAVCAN_DNA_Server::trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb)
{
if (ap_uavcan == nullptr) {
return;
}
ap_uavcan->_dna_server->handleNodeStatus(node_id, cb);
}
/* Node Info message handler
Handle responses from GetNodeInfo Request. We verify the node info
against our records. Marks Verification mask if already recorded,
Or register if the node id is available and not recorded for the
received Unique ID */
void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit)
void AP_UAVCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp)
{
if (node_id > MAX_NODE_ID) {
if (transfer.source_node_id > MAX_NODE_ID) {
return;
}
/*
if we haven't logged this node then log it now
*/
if (!logged.get(node_id) && AP::logger().logging_started()) {
logged.set(node_id);
if (!logged.get(transfer.source_node_id) && AP::logger().logging_started()) {
logged.set(transfer.source_node_id);
uint64_t uid[2];
memcpy(uid, unique_id, sizeof(uid));
memcpy(uid, rsp.hardware_version.unique_id, sizeof(rsp.hardware_version.unique_id));
// @LoggerMessage: CAND
// @Description: Info from GetNodeInfo request
// @Field: TimeUS: Time since system startup
@ -540,105 +455,82 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[],
AP::logger().Write("CAND", "TimeUS,NodeId,UID1,UID2,Name,Major,Minor,Version",
"s#------", "F-------", "QBQQZBBI",
AP_HAL::micros64(),
node_id,
transfer.source_node_id,
uid[0], uid[1],
name,
major,
minor,
vcs_commit);
rsp.name.data,
rsp.software_version.major,
rsp.software_version.minor,
rsp.software_version.vcs_commit);
}
if (isNodeIDOccupied(node_id)) {
if (isNodeIDOccupied(transfer.source_node_id)) {
//if node_id already registered, just verify if Unique ID matches as well
if (node_id == getNodeIDForUniqueID(unique_id, 16)) {
if (node_id == curr_verifying_node) {
if (transfer.source_node_id == getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) {
if (transfer.source_node_id == curr_verifying_node) {
nodeInfo_resp_rcvd = true;
}
setVerificationMask(node_id);
} else if (!_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
setVerificationMask(transfer.source_node_id);
} else if (!_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
/* This is a device with node_id already registered
for another device */
server_state = DUPLICATE_NODES;
fault_node_id = node_id;
memcpy(fault_node_name, name, sizeof(fault_node_name));
fault_node_id = transfer.source_node_id;
memcpy(fault_node_name, rsp.name.data, sizeof(fault_node_name));
}
} else {
/* Node Id was not allocated by us, or during this boot, let's register this in our records
Check if we allocated this Node before */
uint8_t prev_node_id = getNodeIDForUniqueID(unique_id, 16);
uint8_t prev_node_id = getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16);
if (prev_node_id != 255) {
//yes we did, remove this registration
freeNodeID(prev_node_id);
}
//add a new server record
addNodeIDForUniqueID(node_id, unique_id, 16);
addNodeIDForUniqueID(transfer.source_node_id, rsp.hardware_version.unique_id, 16);
//Verify as well
setVerificationMask(node_id);
if (node_id == curr_verifying_node) {
setVerificationMask(transfer.source_node_id);
if (transfer.source_node_id == curr_verifying_node) {
nodeInfo_resp_rcvd = true;
}
}
}
//Trampoline call for handleNodeInfo member call
void AP_UAVCAN_DNA_Server::trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp)
{
uint8_t unique_id[16] = {0};
char name[50] = {0};
//copy the unique id from message to uint8_t array
auto &r = resp.rsp->getResponse();
uavcan::copy(r.hardware_version.unique_id.begin(),
r.hardware_version.unique_id.end(),
unique_id);
strncpy_noterm(name, r.name.c_str(), sizeof(name)-1);
ap_uavcan->_dna_server->handleNodeInfo(node_id, unique_id, name,
r.software_version.major,
r.software_version.minor,
r.software_version.vcs_commit);
}
/* Handle the allocation message from the devices supporting
dynamic node allocation. */
void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t node_id, const AllocationCb &cb)
void AP_UAVCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg)
{
if (allocation_pub[driver_index] == nullptr) {
//init has not been called for this driver.
return;
}
if (!cb.msg->isAnonymousTransfer()) {
if (transfer.source_node_id != 0) {
//Ignore Allocation messages that are not DNA requests
return;
}
uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec();
uint32_t now = AP_HAL::millis();
if (rcvd_unique_id_offset == 0 ||
(now - last_alloc_msg_ms) > 500) {
if (cb.msg->first_part_of_unique_id) {
if (msg.first_part_of_unique_id) {
rcvd_unique_id_offset = 0;
memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
} else {
//we are only accepting first part
return;
}
} else if (cb.msg->first_part_of_unique_id) {
} else if (msg.first_part_of_unique_id) {
// we are only accepting follow up messages
return;
}
if (rcvd_unique_id_offset) {
debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting Followup part! %u\n",
long(uavcan::SystemClock::instance().getMonotonic().toUSec()/1000),
(long int)AP_HAL::millis(),
unsigned((now - last_alloc_msg_ms)));
} else {
debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting First part! %u\n",
long(uavcan::SystemClock::instance().getMonotonic().toUSec()/1000),
(long int)AP_HAL::millis(),
unsigned((now - last_alloc_msg_ms)));
}
last_alloc_msg_ms = now;
if ((rcvd_unique_id_offset + cb.msg->unique_id.size()) > 16) {
if ((rcvd_unique_id_offset + msg.unique_id.len) > 16) {
//This request is malformed, Reset!
rcvd_unique_id_offset = 0;
memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
@ -646,50 +538,40 @@ void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t node_id, const AllocationCb
}
//copy over the unique_id
for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + cb.msg->unique_id.size()); i++) {
rcvd_unique_id[i] = cb.msg->unique_id[i - rcvd_unique_id_offset];
for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + msg.unique_id.len); i++) {
rcvd_unique_id[i] = msg.unique_id.data[i - rcvd_unique_id_offset];
}
rcvd_unique_id_offset += cb.msg->unique_id.size();
rcvd_unique_id_offset += msg.unique_id.len;
//send follow up message
uavcan::protocol::dynamic_node_id::Allocation msg;
uavcan_protocol_dynamic_node_id_Allocation rsp;
/* Respond with the message containing the received unique ID so far
or with node id if we successfully allocated one. */
for (uint8_t i = 0; i < rcvd_unique_id_offset; i++) {
msg.unique_id.push_back(rcvd_unique_id[i]);
}
memcpy(rsp.unique_id.data, rcvd_unique_id, rcvd_unique_id_offset);
rsp.unique_id.len = rcvd_unique_id_offset;
if (rcvd_unique_id_offset == 16) {
//We have received the full Unique ID, time to do allocation
uint8_t resp_node_id = getNodeIDForUniqueID((const uint8_t*)rcvd_unique_id, 16);
if (resp_node_id == 255) {
resp_node_id = findFreeNodeID(cb.msg->node_id);
resp_node_id = findFreeNodeID(msg.node_id);
if (resp_node_id != 255) {
if (addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16)) {
msg.node_id = resp_node_id;
rsp.node_id = resp_node_id;
}
} else {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
}
} else {
msg.node_id = resp_node_id;
rsp.node_id = resp_node_id;
}
//reset states as well
rcvd_unique_id_offset = 0;
memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
}
allocation_pub[driver_index]->broadcast(msg);
}
//Trampoline Call for handleAllocation member call
void AP_UAVCAN_DNA_Server::trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb)
{
if (ap_uavcan == nullptr) {
return;
}
ap_uavcan->_dna_server->handleAllocation(node_id, cb);
allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD
}
//report the server state, along with failure message if any
@ -703,7 +585,7 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co
return false;
}
case DUPLICATE_NODES: {
if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
if (_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
// ignore error
return true;
}
@ -715,7 +597,7 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co
return false;
}
case NODE_STATUS_UNHEALTHY: {
if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
if (_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
// ignore error
return true;
}

45
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
View File

@ -3,18 +3,17 @@
#include <AP_HAL/Semaphores.h>
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <uavcan/uavcan.hpp>
#include <AP_Common/Bitmask.h>
#include <StorageManager/StorageManager.h>
#include <AP_CANManager/AP_CANManager.h>
#include <canard/publisher.h>
#include <canard/subscriber.h>
#include <canard/service_client.h>
#include "AP_Canard_iface.h"
#include <dronecan_msgs.h>
//Forward declaring classes
class AllocationCb;
class NodeStatusCb;
class NodeInfoCb;
class GetNodeInfoCb;
class AP_UAVCAN;
//Forward declaring classes
class AP_UAVCAN_DNA_Server
{
StorageAccess storage;
@ -34,7 +33,7 @@ class AP_UAVCAN_DNA_Server
uint32_t last_verification_request;
uint8_t curr_verifying_node;
uint8_t self_node_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
uint8_t self_node_id;
bool nodeInfo_resp_rcvd;
Bitmask<128> occupation_mask;
@ -89,24 +88,30 @@ class AP_UAVCAN_DNA_Server
//Look in the storage and check if there's a valid Server Record there
bool isValidNodeDataAvailable(uint8_t node_id);
static void trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp);
static void trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb);
static void trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb);
HAL_Semaphore storage_sem;
AP_UAVCAN *_ap_uavcan;
uint8_t driver_index;
AP_UAVCAN &_ap_uavcan;
CanardInterface &_canard_iface;
Canard::Publisher<uavcan_protocol_dynamic_node_id_Allocation> allocation_pub{_canard_iface};
Canard::ObjCallback<AP_UAVCAN_DNA_Server, uavcan_protocol_dynamic_node_id_Allocation> allocation_cb{this, &AP_UAVCAN_DNA_Server::handleAllocation};
Canard::Subscriber<uavcan_protocol_dynamic_node_id_Allocation> allocation_sub;
Canard::ObjCallback<AP_UAVCAN_DNA_Server, uavcan_protocol_NodeStatus> node_status_cb{this, &AP_UAVCAN_DNA_Server::handleNodeStatus};
Canard::Subscriber<uavcan_protocol_NodeStatus> node_status_sub;
Canard::ObjCallback<AP_UAVCAN_DNA_Server, uavcan_protocol_GetNodeInfoResponse> node_info_cb{this, &AP_UAVCAN_DNA_Server::handleNodeInfo};
Canard::Client<uavcan_protocol_GetNodeInfoResponse> node_info_client;
public:
AP_UAVCAN_DNA_Server(AP_UAVCAN *ap_uavcan, StorageAccess _storage);
AP_UAVCAN_DNA_Server(AP_UAVCAN &ap_uavcan);
// Do not allow copies
CLASS_NO_COPY(AP_UAVCAN_DNA_Server);
//Initialises publisher and Server Record for specified uavcan driver
bool init();
bool init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id);
//Reset the Server Record
void reset();
@ -125,9 +130,9 @@ public:
bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
//Callbacks
void handleAllocation(uint8_t node_id, const AllocationCb &cb);
void handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb);
void handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit);
void handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg);
void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg);
void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp);
//Run through the list of seen node ids for verification
void verify_nodes();

238
libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp
View File

@ -1,238 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
*/
#include "AP_UAVCAN_IfaceMgr.h"
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include "AP_UAVCAN_Clock.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_CANManager/AP_CANManager.h>
using namespace uavcan;
extern const AP_HAL::HAL& hal;
#define LOG_TAG "UAVCANIface"
/*****************************************************
* *
* CAN Iface *
* *
* ***************************************************/
/**
* Non-blocking transmission.
*
* If the frame wasn't transmitted upon TX deadline, the driver should discard it.
*
* Note that it is LIKELY that the library will want to send the frames that were passed into the select()
* method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new
* frames between the calls.
*
* @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
*/
int16_t CanIface::send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags)
{
if (can_iface_ == UAVCAN_NULLPTR) {
return -1;
}
return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.isCanFDFrame()), tx_deadline.toUSec(), flags);
}
/**
* Non-blocking reception.
*
* Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller.
*
* Monotonic timestamp is required and can be not precise since it is needed only for
* protocol timing validation (transfer timeouts and inter-transfer intervals).
*
* UTC timestamp is optional, if available it will be used for precise time synchronization;
* must be set to zero if not available.
*
* Refer to @ref ISystemClock to learn more about timestamps.
*
* @param [out] out_ts_monotonic Monotonic timestamp, mandatory.
* @param [out] out_ts_utc UTC timestamp, optional, zero if unknown.
* @return 1 = one frame received, 0 = RX buffer empty, negative for error.
*/
int16_t CanIface::receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc,
CanIOFlags& out_flags)
{
if (can_iface_ == UAVCAN_NULLPTR) {
return -1;
}
AP_HAL::CANFrame frame;
uint64_t rx_timestamp;
uint16_t flags;
out_ts_monotonic = SystemClock::instance().getMonotonic();
int16_t ret = can_iface_->receive(frame, rx_timestamp, flags);
if (ret < 0) {
return ret;
}
out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.canfd);
out_flags = flags;
if (rx_timestamp != 0) {
out_ts_utc = uavcan::UtcTime::fromUSec(SystemClock::instance().getAdjustUsec() + rx_timestamp);
} else {
out_ts_utc = uavcan::UtcTime::fromUSec(0);
}
return ret;
}
/**
* Number of available hardware filters.
*/
uint16_t CanIface::getNumFilters() const
{
if (can_iface_ == UAVCAN_NULLPTR) {
return 0;
}
return can_iface_->getNumFilters();
}
/**
* Continuously incrementing counter of hardware errors.
* Arbitration lost should not be treated as a hardware error.
*/
uint64_t CanIface::getErrorCount() const
{
if (can_iface_ == UAVCAN_NULLPTR) {
return 0;
}
return can_iface_->getErrorCount();
}
/*****************************************************
* *
* CAN Driver *
* *
* ***************************************************/
bool CanIfaceMgr::add_interface(AP_HAL::CANIface *can_iface)
{
if (num_ifaces > HAL_NUM_CAN_IFACES) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Num Ifaces Exceeded\n");
return false;
}
if (can_iface == nullptr) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface Null\n");
return false;
}
if (ifaces[num_ifaces] != nullptr) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface already added\n");
return false;
}
ifaces[num_ifaces] = new CanIface(can_iface);
if (ifaces[num_ifaces] == nullptr) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Can't alloc uavcan::iface\n");
return false;
}
if (!ifaces[num_ifaces]->can_iface_->set_event_handle(&_event_handle)) {
AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Setting event handle failed\n");
return false;
}
AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "UAVCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces));
num_ifaces++;
return true;
}
/**
* Returns an interface by index, or null pointer if the index is out of range.
*/
ICanIface* CanIfaceMgr::getIface(uint8_t iface_index)
{
if (iface_index >= num_ifaces) {
return UAVCAN_NULLPTR;
}
return ifaces[iface_index];
}
/**
* Total number of available CAN interfaces.
* This value shall not change after initialization.
*/
uint8_t CanIfaceMgr::getNumIfaces() const
{
return num_ifaces;
}
CanSelectMasks CanIfaceMgr::makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const
{
CanSelectMasks msk;
for (uint8_t i = 0; i < num_ifaces; i++) {
bool read = in_mask.read & (1 << i);
bool write = in_mask.write & (1 << i);
CanIface* iface = ifaces[i];
if (iface == nullptr) {
continue;
}
if (pending_tx[i] == UAVCAN_NULLPTR) {
if (iface->can_iface_->select(read, write, nullptr, 0)) {
msk.read |= (read ? 1 : 0) << i;
msk.write |= (write ? 1 : 0) << i;
}
} else {
AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, AP_HAL::CANFrame::dlcToDataLength(pending_tx[i]->dlc)};
if (iface->can_iface_->select(read, write, &frame, 0)) {
msk.read |= (read ? 1 : 0) << i;
msk.write |= (write ? 1 : 0) << i;
}
}
}
return msk;
}
/**
* Block until the deadline, or one of the specified interfaces becomes available for read or write.
*
* Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO.
*
* Bit position in the masks defines interface index.
*
* Note that it is allowed to return from this method even if no requested events actually happened, or if
* there are events that were not requested by the library.
*
* The pending TX argument contains an array of pointers to CAN frames that the library wants to transmit
* next, per interface. This is intended to allow the driver to properly prioritize transmissions; many
* drivers will not need to use it. If a write flag for the given interface is set to one in the select mask
* structure, then the corresponding pointer is guaranteed to be valid (not UAVCAN_NULLPTR).
*
* @param [in,out] inout_masks Masks indicating which interfaces are needed/available for IO.
* @param [in] pending_tx Array of frames, per interface, that are likely to be transmitted next.
* @param [in] blocking_deadline Zero means non-blocking operation.
* @return Positive number of ready interfaces or negative error code.
*/
int16_t CanIfaceMgr::select(CanSelectMasks& inout_masks,
const CanFrame* (& pending_tx)[MaxCanIfaces],
const MonotonicTime blocking_deadline)
{
const CanSelectMasks in_masks = inout_masks;
const uint64_t time = SystemClock::instance().getMonotonic().toUSec();
inout_masks = makeSelectMasks(in_masks, pending_tx); // Check if we already have some of the requested events
if ((inout_masks.read & in_masks.read) != 0 ||
(inout_masks.write & in_masks.write) != 0) {
return 1;
}
if (time < blocking_deadline.toUSec()) {
_event_handle.wait(blocking_deadline.toUSec() - time); // Block until timeout expires or any iface updates
}
inout_masks = makeSelectMasks(in_masks, pending_tx); // Return what we got even if none of the requested events are set
return 1; // Return value doesn't matter as long as it is non-negative
}
#endif //HAL_ENABLE_LIBUAVCAN_DRIVERSs

75
libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h
View File

@ -1,75 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <uavcan/uavcan.hpp>
namespace uavcan
{
class CanDriver;
class CanIface : public ICanIface, Noncopyable
{
friend class CanIfaceMgr;
AP_HAL::CANIface* can_iface_;
public:
CanIface(AP_HAL::CANIface *can_iface) : can_iface_(can_iface) {}
virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline,
CanIOFlags flags) override;
virtual int16_t receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic,
UtcTime& out_ts_utc, CanIOFlags& out_flags) override;
virtual int16_t configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs) override {
return 0;
}
uint16_t getNumFilters() const override;
uint64_t getErrorCount() const override;
};
/**
* Generic CAN driver.
*/
class CanIfaceMgr : public ICanDriver, Noncopyable
{
CanIface* ifaces[HAL_NUM_CAN_IFACES];
uint8_t num_ifaces;
HAL_EventHandle _event_handle;
public:
bool add_interface(AP_HAL::CANIface *can_drv);
ICanIface* getIface(uint8_t iface_index) override;
uint8_t getNumIfaces() const override;
CanSelectMasks makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const;
int16_t select(CanSelectMasks& inout_masks,
const CanFrame* (& pending_tx)[MaxCanIfaces],
const MonotonicTime blocking_deadline) override;
};
}
#endif

90
libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp
View File

@ -1,90 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
/*
based on dynamic_memory.hpp which is:
Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <AP_HAL/AP_HAL.h>
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include "AP_UAVCAN.h"
#include "AP_UAVCAN_pool.h"
#include <AP_InternalError/AP_InternalError.h>
AP_PoolAllocator::AP_PoolAllocator(uint16_t _pool_size) :
num_blocks(_pool_size / UAVCAN_NODE_POOL_BLOCK_SIZE)
{
}
bool AP_PoolAllocator::init(void)
{
WITH_SEMAPHORE(sem);
pool_nodes = (Node *)calloc(num_blocks, UAVCAN_NODE_POOL_BLOCK_SIZE);
if (pool_nodes == nullptr) {
return false;
}
for (uint16_t i=0; i<(num_blocks-1); i++) {
pool_nodes[i].next = &pool_nodes[i+1];
}
free_list = &pool_nodes[0];
return true;
}
void* AP_PoolAllocator::allocate(std::size_t size)
{
WITH_SEMAPHORE(sem);
if (free_list == nullptr || size > UAVCAN_NODE_POOL_BLOCK_SIZE) {
return nullptr;
}
Node *ret = free_list;
const uint32_t blk = ret - pool_nodes;
if (blk >= num_blocks) {
INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
return nullptr;
}
free_list = free_list->next;
used++;
if (used > max_used) {
max_used = used;
}
return ret;
}
void AP_PoolAllocator::deallocate(const void* ptr)
{
if (ptr == nullptr) {
return;
}
WITH_SEMAPHORE(sem);
Node *p = reinterpret_cast<Node*>(const_cast<void*>(ptr));
const uint32_t blk = p - pool_nodes;
if (blk >= num_blocks) {
INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
return;
}
p->next = free_list;
free_list = p;
used--;
}
#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS

60
libraries/AP_UAVCAN/AP_UAVCAN_pool.h
View File

@ -1,60 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
/*
based on dynamic_memory.hpp which is:
Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include "AP_UAVCAN.h"
#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
#if HAL_CANFD_SUPPORTED
#define UAVCAN_NODE_POOL_BLOCK_SIZE 128
#else
#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
#endif
#endif
class AP_PoolAllocator : public uavcan::IPoolAllocator
{
public:
AP_PoolAllocator(uint16_t _pool_size);
bool init(void);
void *allocate(std::size_t size) override;
void deallocate(const void* ptr) override;
uint16_t getBlockCapacity() const override {
return num_blocks;
}
private:
const uint16_t num_blocks;
union Node {
uint8_t data[UAVCAN_NODE_POOL_BLOCK_SIZE];
Node* next;
};
Node *free_list;
Node *pool_nodes;
HAL_Semaphore sem;
uint16_t used;
uint16_t max_used;
};

185
libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp
View File

@ -4,42 +4,18 @@
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <AP_HAL/Semaphores.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
#include <uavcan/equipment/air_data/StaticPressure.hpp>
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
#include <uavcan/equipment/actuator/ArrayCommand.hpp>
#include <uavcan/equipment/actuator/Command.hpp>
#include <uavcan/equipment/actuator/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/indication/LightsCommand.hpp>
#include <uavcan/equipment/indication/SingleLightCommand.hpp>
#include <uavcan/equipment/indication/RGB565.hpp>
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <com/hex/equipment/flow/Measurement.hpp>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_HAL_Linux/CANSocketIface.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_HAL_SITL/CANSocketIface.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <hal.h>
#include <AP_HAL_ChibiOS/CANIface.h>
#endif
@ -49,10 +25,8 @@ void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define UAVCAN_NODE_POOL_SIZE 8192
#ifdef UAVCAN_NODE_POOL_BLOCK_SIZE
#undef UAVCAN_NODE_POOL_BLOCK_SIZE
#endif
#define UAVCAN_NODE_POOL_BLOCK_SIZE 256
static uint8_t node_memory_pool[UAVCAN_NODE_POOL_SIZE];
#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)
@ -67,22 +41,6 @@ public:
private:
uint8_t driver_index = 0;
uavcan::Node<0> *_node;
// This will be needed to implement if UAVCAN is used with multithreading
// Such cases will be firmware update, etc.
class RaiiSynchronizer {
public:
RaiiSynchronizer()
{
}
~RaiiSynchronizer()
{
}
};
uavcan::HeapBasedPoolAllocator<UAVCAN_NODE_POOL_BLOCK_SIZE, UAVCAN_sniffer::RaiiSynchronizer> _node_allocator;
AP_CANManager can_mgr;
};
@ -108,20 +66,35 @@ static void count_msg(const char *name)
}
#define MSG_CB(mtype, cbname) \
static void cb_ ## cbname(const uavcan::ReceivedDataStructure<mtype>& msg) { count_msg(msg.getDataTypeFullName()); }
static void cb_ ## cbname(const CanardRxTransfer &transfer, const mtype& msg) { count_msg(#mtype); }
MSG_CB(uavcan::protocol::NodeStatus, NodeStatus)
MSG_CB(uavcan::equipment::gnss::Fix2, Fix2)
MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary)
MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength)
MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2);
MSG_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure)
MSG_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature)
MSG_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo);
MSG_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand)
MSG_CB(uavcan::equipment::esc::RawCommand, RawCommand)
MSG_CB(uavcan::equipment::indication::LightsCommand, LightsCommand);
MSG_CB(com::hex::equipment::flow::Measurement, Measurement);
MSG_CB(uavcan_protocol_NodeStatus, NodeStatus)
MSG_CB(uavcan_equipment_gnss_Fix2, Fix2)
MSG_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary)
MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength)
MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);
MSG_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure)
MSG_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature)
MSG_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);
MSG_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand)
MSG_CB(uavcan_equipment_esc_RawCommand, RawCommand)
MSG_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);
MSG_CB(com_hex_equipment_flow_Measurement, Measurement);
uavcan_protocol_NodeStatus node_status;
uavcan_protocol_GetNodeInfoResponse node_info;
CanardInterface *_uavcan_iface_mgr;
Canard::Publisher<uavcan_protocol_NodeStatus> *node_status_pub;
Canard::Server<uavcan_protocol_GetNodeInfoRequest> *node_info_srv;
static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan_protocol_GetNodeInfoRequest& msg)
{
if (node_info_srv == nullptr) {
return;
}
node_info_srv->respond(transfer, node_info);
}
void UAVCAN_sniffer::init(void)
{
@ -137,7 +110,7 @@ void UAVCAN_sniffer::init(void)
debug_uavcan("Can not initialised\n");
return;
}
uavcan::CanIfaceMgr *_uavcan_iface_mgr = new uavcan::CanIfaceMgr;
_uavcan_iface_mgr = new CanardInterface{driver_index};
if (_uavcan_iface_mgr == nullptr) {
return;
@ -148,74 +121,63 @@ void UAVCAN_sniffer::init(void)
return;
}
_node = new uavcan::Node<0>(*_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
_uavcan_iface_mgr->init(node_memory_pool, sizeof(node_memory_pool), 9);
if (_node == nullptr) {
node_status_pub = new Canard::Publisher<uavcan_protocol_NodeStatus>{*_uavcan_iface_mgr};
if (node_status_pub == nullptr) {
return;
}
if (_node->isStarted()) {
node_info_srv = new Canard::Server<uavcan_protocol_GetNodeInfoRequest>{*_uavcan_iface_mgr, *Canard::allocate_static_callback(cb_GetNodeInfoRequest)};
if (node_info_srv == nullptr) {
return;
}
uavcan::NodeID self_node_id(9);
_node->setNodeID(self_node_id);
node_info.name.len = snprintf((char*)node_info.name.data, sizeof(node_info.name.data), "org.ardupilot:%u", driver_index);
char ndname[20];
snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index);
node_info.software_version.major = AP_UAVCAN_SW_VERS_MAJOR;
node_info.software_version.minor = AP_UAVCAN_SW_VERS_MINOR;
uavcan::NodeStatusProvider::NodeName name(ndname);
_node->setName(name);
node_info.hardware_version.major = AP_UAVCAN_HW_VERS_MAJOR;
node_info.hardware_version.minor = AP_UAVCAN_HW_VERS_MINOR;
uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
sw_version.major = AP_UAVCAN_SW_VERS_MAJOR;
sw_version.minor = AP_UAVCAN_SW_VERS_MINOR;
_node->setSoftwareVersion(sw_version);
#define START_CB(mtype, cbname) Canard::allocate_sub_static_callback(cb_ ## cbname,driver_index)
uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion
hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
_node->setHardwareVersion(hw_version);
int start_res = _node->start();
if (start_res < 0) {
debug_uavcan("UAVCAN: node start problem\n\r");
return;
}
#define START_CB(mtype, cbname) (new uavcan::Subscriber<mtype>(*_node))->start(cb_ ## cbname)
START_CB(uavcan::protocol::NodeStatus, NodeStatus);
START_CB(uavcan::equipment::gnss::Fix2, Fix2);
START_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary);
START_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength);
START_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2);
START_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure);
START_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature);
START_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo);
START_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand);
START_CB(uavcan::equipment::esc::RawCommand, RawCommand);
START_CB(uavcan::equipment::indication::LightsCommand, LightsCommand);
START_CB(com::hex::equipment::flow::Measurement, Measurement);
/*
* Informing other nodes that we're ready to work.
* Default mode is INITIALIZING.
*/
_node->setModeOperational();
START_CB(uavcan_protocol_NodeStatus, NodeStatus);
START_CB(uavcan_equipment_gnss_Fix2, Fix2);
START_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary);
START_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength);
START_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);
START_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure);
START_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature);
START_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);
START_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand);
START_CB(uavcan_equipment_esc_RawCommand, RawCommand);
START_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);
START_CB(com_hex_equipment_flow_Measurement, Measurement);
debug_uavcan("UAVCAN: init done\n\r");
}
static void send_node_status()
{
uavcan_protocol_NodeStatus msg;
msg.uptime_sec = AP_HAL::millis() / 1000;
msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
msg.sub_mode = 0;
msg.vendor_specific_status_code = 0;
node_status_pub->broadcast(msg);
}
uint32_t last_status_send = 0;
void UAVCAN_sniffer::loop(void)
{
if (_node == nullptr) {
return;
if (AP_HAL::millis() - last_status_send > 1000) {
last_status_send = AP_HAL::millis();
send_node_status();
}
_node->spin(uavcan::MonotonicDuration::fromMSec(1));
_uavcan_iface_mgr->process(1);
}
void UAVCAN_sniffer::print_stats(void)
@ -233,8 +195,7 @@ void UAVCAN_sniffer::print_stats(void)
static UAVCAN_sniffer sniffer;
UAVCAN_sniffer::UAVCAN_sniffer() :
_node_allocator(UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE)
UAVCAN_sniffer::UAVCAN_sniffer()
{}
UAVCAN_sniffer::~UAVCAN_sniffer()

18
libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript
View File

@ -1,7 +1,21 @@
#!/usr/bin/env python
# encoding: utf-8
from waflib.TaskGen import after_method, before_method, feature
def build(bld):
bld.ap_example(
use='ap',
vehicle = bld.path.name
bld.ap_stlib(
name=vehicle + '_libs',
ap_vehicle='UNKNOWN',
dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c',
ap_libraries=bld.ap_common_vehicle_libraries() + [
'AP_OSD',
'AP_RCMapper',
'AP_Arming'
],
)
bld.ap_program(
program_groups=['tool'],
use=[vehicle + '_libs'],
)