UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 21:40:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCANv1 Include Kconfig & Fix #18396

Browse Source
This commit is contained in:
Peter van der Perk
2021-10-11 15:00:53 +02:00
committed by Daniel Agar
parent a732ddaefb
commit ed394027b1
13 changed files with 464 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
boards/nxp/fmuk66-v3/socketcan.px4board
View File

@@ -1,2 +1,9 @@
CONFIG_DRIVERS_UAVCAN=n
CONFIG_DRIVERS_UAVCAN_V1=y
CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y
CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y
CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y
CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y
CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y

40
src/drivers/uavcan_v1/CMakeLists.txt
View File

@@ -50,6 +50,8 @@ else()
endif()
set(SRCS)
set(DPNDS)
if(${PX4_PLATFORM} MATCHES "nuttx")
if(CONFIG_NET_CAN)
list(APPEND SRCS
@@ -64,6 +66,40 @@ if(${PX4_PLATFORM} MATCHES "nuttx")
endif()
endif()
if(CONFIG_UAVCAN_V1_NODE_MANAGER)
list(APPEND SRCS
NodeManager.hpp
NodeManager.cpp
)
endif()
if(CONFIG_UAVCAN_V1_NODE_CLIENT)
list(APPEND SRCS
NodeClient.hpp
NodeClient.cpp
)
list(APPEND DPNDS
drivers_bootloaders
)
endif()
# Temporary measure to get Kconfig option as defines into this application
# Ideally we want nicely decouple and define this in kconfig.cmake
# But then we need to overhaul the src modules naming convention
set(DRIVERS_UAVCAN_V1_OPTIONS)
get_cmake_property(_variableNames VARIABLES)
foreach (_variableName ${_variableNames})
string(REGEX MATCH "^CONFIG_UAVCAN_V1_" UAVCAN_V1_OPTION ${_variableName})
if(UAVCAN_V1_OPTION)
if(${${_variableName}} STREQUAL "y")
list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=1")
else()
list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=${${_variableName}}")
endif()
endif()
endforeach()
px4_add_module(
MODULE drivers__uavcan_v1
MAIN uavcan_v1
@@ -72,6 +108,7 @@ px4_add_module(
#-DCANARD_ASSERT
-DUINT32_C\(x\)=__UINT32_C\(x\)
-O0
${DRIVERS_UAVCAN_V1_OPTIONS}
INCLUDES
${LIBCANARD_DIR}/libcanard/
${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
@@ -82,8 +119,6 @@ px4_add_module(
SubscriptionManager.hpp
ParamManager.hpp
ParamManager.cpp
NodeManager.hpp
NodeManager.cpp
Uavcan.cpp
Uavcan.hpp
Publishers/uORB/uorb_publisher.cpp
@@ -97,4 +132,5 @@ px4_add_module(
git_public_regulated_data_types
git_legacy_data_types
version
${DPNDS}
)

92
src/drivers/uavcan_v1/Kconfig
View File

@@ -20,15 +20,91 @@ if DRIVERS_UAVCAN_V1
endchoice
config UAVCAN_V1_GETINFO_RESPONDER
bool "GetInfo1.0 responder"
default n
config UAVCAN_V1_NODE_MANAGER
bool "Node manager"
default y
depends on UAVCAN_V1_FMU
help
Implement UAVCAN v1 PNP server functionality and manages discovered nodes
config UAVCAN_V1_EXECUTECOMMAND_RESPONDER
bool "ExecuteCommand1.0 responder"
default n
config UAVCAN_V1_NODE_CLIENT
bool "Node client"
default y
depends on UAVCAN_V1_CLIENT
help
Implement UAVCAN v1 PNP client functionality
config UAVCAN_V1_GNSS_PUBLISHER
bool "GNSS Publisher"
config UAVCAN_V1_APP_DESCRIPTOR
bool "UAVCAN v0 bootloader app descriptor"
default n
depends on UAVCAN_V1_CLIENT && DRIVERS_BOOTLOADERS
help
When the board uses the UAVCANv0 bootloader functionality you need a AppImageDescriptor defined
menu "Publisher support"
config UAVCAN_V1_GNSS_PUBLISHER
bool "GNSS Publisher"
default n
config UAVCAN_V1_ESC_CONTROLLER
bool "ESC Controller"
default n
config UAVCAN_V1_READINESS_PUBLISHER
bool "Readiness Publisher"
default n
config UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER
bool "uORB actuator_outputs publisher"
default n
config UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
bool "uORB sensor_gps publisher"
default n
endmenu
menu "Subscriber support"
config UAVCAN_V1_ESC_SUBSCRIBER
bool "ESC Subscriber"
default n
config UAVCAN_V1_GNSS_SUBSCRIBER_0
bool "GNSS Subscriber 0 "
default n
config UAVCAN_V1_GNSS_SUBSCRIBER_1
bool "GNSS Subscriber 1"
default n
config UAVCAN_V1_BMS_SUBSCRIBER
bool "BMS Subscriber"
default n
config UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER
bool "uORB sensor_gps Subscriber"
default n
endmenu
menu "Advertised Services"
config UAVCAN_V1_GETINFO_RESPONDER
bool "GetInfo1.0 responder"
default y
help
Responds to uavcan.node.GetInfo.1.0 request sending over node information
See https://github.com/UAVCAN/public_regulated_data_types/blob/master/uavcan/node/430.GetInfo.1.0.uavcan for full response
config UAVCAN_V1_EXECUTECOMMAND_RESPONDER
bool "ExecuteCommand1.0 responder"
default n
help
To be implemented
endmenu
menu "Service invokers"
endmenu
endif #DRIVERS_UAVCAN_V1

170
src/drivers/uavcan_v1/NodeClient.cpp Normal file
View File

@@ -0,0 +1,170 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file NodeClient.cpp
*
* Defines basic implementation of client UAVCAN PNP for requesting a Node ID
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#define PNP_UNIQUE_ID_SIZE 16 // 128 bit unique id
#include <crc64.h>
#include "NodeClient.hpp"
void NodeClient::callback(const CanardTransfer &receive)
{
if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id != CANARD_NODE_ID_UNSET) {
int32_t allocated = CANARD_NODE_ID_UNSET;
px4_guid_t px4_guid;
board_get_px4_guid(px4_guid);
if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) {
uavcan_pnp_NodeIDAllocationData_2_0 msg;
size_t msg_size_in_bytes = receive.payload_size;
uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, (const uint8_t *)receive.payload,
&msg_size_in_bytes);
if (memcmp(msg.unique_id, px4_guid, sizeof(msg.unique_id)) == 0) {
allocated = msg.node_id.value;
}
} else {
uavcan_pnp_NodeIDAllocationData_1_0 msg;
size_t msg_size_in_bytes = receive.payload_size;
uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload,
&msg_size_in_bytes);
if (msg.allocated_node_id.count > 0) {
if (msg.unique_id_hash == (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF)) {
allocated = msg.allocated_node_id.elements[0].value;
}
}
}
if (allocated == CANARD_NODE_ID_UNSET) {
return; // UID mismatch.
}
if (allocated <= 0 || allocated >= (int32_t)CANARD_NODE_ID_MAX)
// Allocated node-ID ignored because it exceeds max_node_id
{
return;
}
_canard_instance.node_id = allocated;
}
}
void NodeClient::update()
{
if (hrt_elapsed_time(&_nodealloc_request_last) >= hrt_abstime(2 *
1000000ULL)) { // Compiler hates me here, some 1_s doesn't work
int32_t result;
// Allocation already done, nothing to do
if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) {
return;
}
if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) {
// NodeIDAllocationData message
uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg;
uint8_t node_id_alloc_payload_buffer[PNP2_PAYLOAD_SIZE];
px4_guid_t px4_guid;
board_get_px4_guid(px4_guid);
memcpy(node_id_alloc_msg.unique_id, px4_guid, sizeof(node_id_alloc_msg.unique_id));
//node_id_alloc_msg.node_id.value = preffered_node_id; //FIXME preffered ID PX4 Param
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = PNP2_PORT_ID, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _node_id_alloc_transfer_id,
.payload_size = PNP2_PAYLOAD_SIZE,
.payload = &node_id_alloc_payload_buffer,
};
result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer,
&transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
canardTxPush(&_canard_instance, &transfer);
}
} else {
// NodeIDAllocationData message
uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg;
uavcan_pnp_NodeIDAllocationData_1_0_initialize_(&node_id_alloc_msg);
uint8_t node_id_alloc_payload_buffer[PNP1_PAYLOAD_SIZE];
px4_guid_t px4_guid;
board_get_px4_guid(px4_guid);
node_id_alloc_msg.unique_id_hash = (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF);
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = PNP1_PORT_ID, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _node_id_alloc_transfer_id,
.payload_size = PNP1_PAYLOAD_SIZE,
.payload = &node_id_alloc_payload_buffer,
};
result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer,
&transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
canardTxPush(&_canard_instance, &transfer);
}
}
_nodealloc_request_last = hrt_absolute_time();
}
}

43
src/drivers/uavcan_v1/PublicationManager.hpp
View File

@@ -42,6 +42,35 @@
#pragma once
#include <px4_platform_common/px4_config.h>
#ifndef CONFIG_UAVCAN_V1_GNSS_PUBLISHER
#define CONFIG_UAVCAN_V1_GNSS_PUBLISHER 0
#endif
#ifndef CONFIG_UAVCAN_V1_ESC_CONTROLLER
#define CONFIG_UAVCAN_V1_ESC_CONTROLLER 0
#endif
#ifndef CONFIG_UAVCAN_V1_READINESS_PUBLISHER
#define CONFIG_UAVCAN_V1_READINESS_PUBLISHER 0
#endif
#ifndef CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER
#define CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0
#endif
#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER 0
#endif
/* Preprocessor calculation of publisher count */
#define UAVCAN_PUB_COUNT CONFIG_UAVCAN_V1_GNSS_PUBLISHER + \
CONFIG_UAVCAN_V1_ESC_CONTROLLER + \
CONFIG_UAVCAN_V1_READINESS_PUBLISHER + \
CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
@@ -79,7 +108,9 @@ private:
UavcanParamManager &_param_manager;
List<UavcanPublisher *> _dynpublishers;
const UavcanDynPubBinder _uavcan_pubs[5] {
const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] {
#if CONFIG_UAVCAN_V1_GNSS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
{
@@ -88,6 +119,8 @@ private:
"gps",
0
},
#endif
#if CONFIG_UAVCAN_V1_ESC_CONTROLLER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
{
@@ -96,6 +129,8 @@ private:
"esc",
0
},
#endif
#if CONFIG_UAVCAN_V1_READINESS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
{
@@ -104,6 +139,8 @@ private:
"readiness",
0
},
#endif
#if CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
{
@@ -112,7 +149,8 @@ private:
"uorb.actuator_outputs",
0
},
#endif
#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
{
@@ -121,5 +159,6 @@ private:
"uorb.sensor_gps",
0
},
#endif
};
};

4
src/drivers/uavcan_v1/ServiceClients/Access.hpp
View File

@@ -108,7 +108,7 @@ public:
.transfer_kind = CanardTransferKindResponse,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = access_response_transfer_id, /// TODO: track register Access _response_ separately?
.transfer_id = receive.transfer_id,
.payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
};
@@ -117,7 +117,6 @@ public:
if (result == 0) {
// set the data ready in the buffer and chop if needed
++access_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
@@ -127,6 +126,5 @@ public:
private:
UavcanParamManager &_param_manager;
CanardTransferID access_response_transfer_id = 0;
};

6
src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp
View File

@@ -112,7 +112,7 @@ public:
.transfer_kind = CanardTransferKindResponse,
.port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Send back to request Node
.transfer_id = getinfo_response_transfer_id,
.transfer_id = receive.transfer_id,
.payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
};
@@ -122,7 +122,6 @@ public:
if (result == 0) {
// set the data ready in the buffer and chop if needed
++getinfo_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &response);
}
@@ -136,7 +135,4 @@ public:
};
private:
CanardTransferID getinfo_response_transfer_id = 0;
};

12
src/drivers/uavcan_v1/Services/ServiceRequest.hpp
View File

@@ -32,9 +32,9 @@
****************************************************************************/
/**
* @file List.hpp
* @file ServiceRequest.hpp
*
* Defines a List Service invoker and process List responses
* Defines a Service invoker base class and process responses
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
@@ -45,8 +45,6 @@
#include <px4_platform_common/module.h>
#include <version/version.h>
#include "../NodeManager.hpp"
#include <uavcan/_register/List_1_0.h>
#include "../Subscribers/BaseSubscriber.hpp"
@@ -79,6 +77,7 @@ public:
bool request(CanardTransfer *transfer, UavcanServiceRequestInterface *handler)
{
_response_callback = handler;
remote_node_id = transfer->remote_node_id;
++request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
return canardTxPush(&_canard_instance, transfer) > 0;
}
@@ -87,7 +86,9 @@ public:
{
PX4_INFO("Response");
if (_response_callback != nullptr) {
if (_response_callback != nullptr &&
receive.transfer_id == (request_transfer_id - 1) &&
receive.remote_node_id == remote_node_id) {
_response_callback->response_callback(receive);
}
};
@@ -96,6 +97,7 @@ public:
protected:
CanardTransferID request_transfer_id = 0;
CanardNodeID remote_node_id = CANARD_NODE_ID_UNSET;
const CanardPortID _portID;
const size_t _extent;

5
src/drivers/uavcan_v1/SubscriptionManager.cpp
View File

@@ -55,11 +55,14 @@ SubscriptionManager::~SubscriptionManager()
}
}
void SubscriptionManager::subscribe()
{
_heartbeat_sub.subscribe();
#if CONFIG_UAVCAN_V1_GETINFO_RESPONDER
_getinfo_rsp.subscribe();
#endif
_access_rsp.subscribe();
updateDynamicSubscriptions();

43
src/drivers/uavcan_v1/SubscriptionManager.hpp
View File

@@ -41,6 +41,33 @@
#pragma once
#ifndef CONFIG_UAVCAN_V1_ESC_SUBSCRIBER
#define CONFIG_UAVCAN_V1_ESC_SUBSCRIBER 0
#endif
#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0
#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 0
#endif
#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1
#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 0
#endif
#ifndef CONFIG_UAVCAN_V1_BMS_SUBSCRIBER
#define CONFIG_UAVCAN_V1_BMS_SUBSCRIBER 0
#endif
#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER
#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER 0
#endif
/* Preprocessor calculation of Subscribers count */
#define UAVCAN_SUB_COUNT CONFIG_UAVCAN_V1_ESC_SUBSCRIBER + \
CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 + \
CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 + \
CONFIG_UAVCAN_V1_BMS_SUBSCRIBER + \
CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
@@ -82,13 +109,16 @@ private:
UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance};
#if UAVCAN_V1_GETINFO_RESPONDER
// GetInfo response
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
#endif
// Process register requests
UavcanAccessResponse _access_rsp {_canard_instance, _param_manager};
const UavcanDynSubBinder _uavcan_subs[6] {
const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] {
#if CONFIG_UAVCAN_V1_ESC_SUBSCRIBER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
@@ -97,6 +127,8 @@ private:
"esc",
0
},
#endif
#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
@@ -105,6 +137,8 @@ private:
"gps",
0
},
#endif
#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 //FIXME decouple instanceing
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
@@ -113,6 +147,8 @@ private:
"gps",
1
},
#endif
#if CONFIG_UAVCAN_V1_BMS_SUBSCRIBER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
@@ -121,6 +157,8 @@ private:
"energy_source",
0
},
#endif
#if 0 //Obsolete to be removed
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
@@ -129,6 +167,8 @@ private:
"legacy_bms",
0
},
#endif
#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
@@ -137,5 +177,6 @@ private:
"uorb.sensor_gps",
0
},
#endif
};
};

61
src/drivers/uavcan_v1/Uavcan.cpp
View File

@@ -36,6 +36,28 @@
#include <lib/geo/geo.h>
#include <lib/version/version.h>
#ifdef CONFIG_UAVCAN_V1_APP_DESCRIPTOR
#include "boot_app_shared.h"
/*
* This is the AppImageDescriptor used
* by the make_can_boot_descriptor.py tool to set
* the application image's descriptor so that the
* uavcan bootloader has the ability to validate the
* image crc, size etc of this application
*/
boot_app_shared_section app_descriptor_t AppDescriptor = {
.signature = APP_DESCRIPTOR_SIGNATURE,
.image_crc = 0,
.image_size = 0,
.git_hash = 0,
.major_version = APP_VERSION_MAJOR,
.minor_version = APP_VERSION_MINOR,
.board_id = HW_VERSION_MAJOR << 8 | HW_VERSION_MINOR,
.reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
};
#endif
using namespace time_literals;
UavcanNode *UavcanNode::_instance;
@@ -80,7 +102,15 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
_canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
}
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
_node_manager.subscribe();
#endif
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
_node_client = new NodeClient(_canard_instance, _param_manager);
_node_client->subscribe();
#endif
_pub_manager.updateParams();
@@ -195,13 +225,30 @@ void UavcanNode::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
// send uavcan::node::Heartbeat_1_0 @ 1 Hz
sendHeartbeat();
if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) {
// send uavcan::node::Heartbeat_1_0 @ 1 Hz
sendHeartbeat();
// Check all publishers
_pub_manager.update();
// Check all publishers
_pub_manager.update();
_node_manager.update();
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
_node_manager.update();
#endif
}
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
else if (_node_client != nullptr) {
if (_canard_instance.node_id == CANARD_NODE_ID_UNSET) {
_node_client->update();
} else {
delete _node_client;
}
}
#endif
transmit();
@@ -390,6 +437,10 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
int32_t node_id = 0;
param_get(param_find("UAVCAN_V1_ID"), &node_id);
if (node_id == -1) {
node_id = CANARD_NODE_ID_UNSET;
}
// Start
PX4_INFO("Node ID %" PRIu32 ", bitrate %" PRIu32, node_id, bitrate);
return UavcanNode::start(node_id, bitrate);

12
src/drivers/uavcan_v1/Uavcan.hpp
View File

@@ -61,7 +61,13 @@
#include "Publishers/Publisher.hpp"
#include "Publishers/uORB/uorb_publisher.hpp"
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
#include "NodeManager.hpp"
#endif
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
#include "NodeClient.hpp"
#endif
#include "PublicationManager.hpp"
#include "SubscriptionManager.hpp"
@@ -179,7 +185,13 @@ private:
UavcanParamManager _param_manager;
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
NodeManager _node_manager {_canard_instance, _param_manager};
#endif
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
NodeClient *_node_client {nullptr};
#endif
PublicationManager _pub_manager {_canard_instance, _param_manager};
SubscriptionManager _sub_manager {_canard_instance, _param_manager};

2
src/drivers/uavcan_v1/parameters.c
View File

@@ -48,7 +48,7 @@ PARAM_DEFINE_INT32(UAVCAN_V1_ENABLE, 0);
*
* Read the specs at http://uavcan.org to learn more about Node ID.
*
* @min 1
* @min -1
* @max 125
* @reboot_required true
* @group UAVCANv1