dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Upgrade libcanard to 3.0 and rename uavcan_v1 to cyphal 

Did some prep work for redundant interfaces by introducing CanardHandle class to decouple physical interfaces from cyphal.cpp
This commit is contained in:
Peter van der Perk 2022-05-10 10:10:04 +02:00 committed by Daniel Agar
parent 3ac8fdbe29
commit b1ad4e8864
60 changed files with 967 additions and 802 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.gitmodules vendored
View File

@ -36,14 +36,14 @@
[submodule "Tools/jsbsim_bridge"]
path = Tools/jsbsim_bridge
url = https://github.com/PX4/px4-jsbsim-bridge.git
[submodule "src/drivers/uavcan_v1/libcanard"]
path = src/drivers/uavcan_v1/libcanard
url = https://github.com/UAVCAN/libcanard.git
[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
path = src/drivers/uavcan_v1/public_regulated_data_types
url = https://github.com/UAVCAN/public_regulated_data_types.git
[submodule "src/drivers/uavcan_v1/legacy_data_types"]
path = src/drivers/uavcan_v1/legacy_data_types
[submodule "src/drivers/cyphal/libcanard"]
path = src/drivers/cyphal/libcanard
url = https://github.com/opencyphal/libcanard.git
[submodule "src/drivers/cyphal/public_regulated_data_types"]
path = src/drivers/cyphal/public_regulated_data_types
url = https://github.com/opencyphal/public_regulated_data_types.git
[submodule "src/drivers/cyphal/legacy_data_types"]
path = src/drivers/cyphal/legacy_data_types
url = https://github.com/PX4/public_regulated_data_types.git
branch = legacy
[submodule "src/lib/crypto/monocypher"]

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -298,9 +298,9 @@ else
tune_control play error
fi
else
if param greater -s UAVCAN_V1_ENABLE 0
if param greater -s CYPHAL_ENABLE 0
then
uavcan_v1 start
cyphal start
fi
fi

3
Tools/astyle/files_to_check_code_style.sh
View File

@ -13,8 +13,7 @@ exec find boards msg src platforms test \
-path platforms/qurt/dspal -prune -o \
-path src/drivers/uavcan/libuavcan -prune -o \
-path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
-path src/drivers/uavcan_v1/libcanard -prune -o \
-path src/drivers/uavcannode_gps_demo/libcanard -prune -o \
-path src/drivers/cyphal/libcanard -prune -o \
-path src/lib/crypto/monocypher -prune -o \
-path src/lib/events/libevents -prune -o \
-path src/lib/parameters/uthash -prune -o \

6
boards/nxp/fmuk66-e/socketcan.px4board
View File

@ -1,4 +1,4 @@
CONFIG_DRIVERS_UAVCAN=n
CONFIG_DRIVERS_UAVCAN_V1=y
CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
CONFIG_DRIVERS_CYPHAL=y
CONFIG_CYPHAL_BMS_SUBSCRIBER=y
CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y

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

@ -1,9 +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
CONFIG_DRIVERS_CYPHAL=y
CONFIG_CYPHAL_BMS_SUBSCRIBER=y
CONFIG_CYPHAL_ESC_SUBSCRIBER=y
CONFIG_CYPHAL_GNSS_SUBSCRIBER_0=y
CONFIG_CYPHAL_GNSS_SUBSCRIBER_1=y
CONFIG_CYPHAL_READINESS_PUBLISHER=y
CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y

8
boards/nxp/ucans32k146/default.px4board
View File

@ -7,10 +7,10 @@ CONFIG_DRIVERS_BOOTLOADERS=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_DRIVERS_UAVCAN_V1=y
CONFIG_UAVCAN_V1_CLIENT=y
CONFIG_UAVCAN_V1_APP_DESCRIPTOR=y
CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y
CONFIG_DRIVERS_CYPHAL=y
CONFIG_CYPHAL_CLIENT=y
CONFIG_CYPHAL_APP_DESCRIPTOR=y
CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y

2
boards/nxp/ucans32k146/init/rc.board_defaults
View File

@ -6,4 +6,4 @@
pwm_out mode_pwm1 start
ifup can0
uavcan_v1 start
cyphal start

6
boards/px4/fmu-v5/uavcanv1.px4board → boards/px4/fmu-v5/cyphal.px4board
View File

@ -2,6 +2,6 @@
CONFIG_DRIVERS_HEATER=n
CONFIG_DRIVERS_OSD=n
CONFIG_DRIVERS_UAVCAN=n
CONFIG_DRIVERS_UAVCAN_V1=y
CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
CONFIG_DRIVERS_CYPHAL=y
CONFIG_CYPHAL_BMS_SUBSCRIBER=y
CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y

64
src/drivers/uavcan_v1/Actuators/EscClient.hpp → src/drivers/cyphal/Actuators/EscClient.hpp
View File

@ -34,13 +34,13 @@
/**
* @file EscClient.hpp
*
* Client-side implementation of DS-15 specification ESC service
* Client-side implementation of UDRAL specification ESC service
*
* Publishes the following UAVCAN v1 messages:
* Publishes the following Cyphal messages:
* reg.drone.service.actuator.common.sp.Value8.0.1
* reg.drone.service.common.Readiness.0.1
*
* Subscribes to the following UAVCAN v1 messages:
* Subscribes to the following Cyphal messages:
* reg.drone.service.actuator.common.Feedback.0.1
* reg.drone.service.actuator.common.Status.0.1
*
@ -57,9 +57,9 @@
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
// DS-15 Specification Messages
#include <reg/drone/service/actuator/common/sp/Vector31_0_1.h>
#include <reg/drone/service/common/Readiness_0_1.h>
// UDRAL Specification Messages
#include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
#include <reg/udral/service/common/Readiness_0_1.h>
/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
class UavcanEscController : public UavcanPublisher
@ -67,8 +67,8 @@ class UavcanEscController : public UavcanPublisher
public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanPublisher(ins, pmgr, "ds_015", "esc") { };
UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanPublisher(handle, pmgr, "udral", "esc") { };
~UavcanEscController() {};
@ -80,45 +80,47 @@ public:
if (new_arming.armed != _armed.armed) {
_armed = new_arming;
size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
// Only publish if we have a valid publication ID set
if (_port_id == 0) {
return;
}
reg_drone_service_common_Readiness_0_1 msg_arming {};
reg_udral_service_common_Readiness_0_1 msg_arming {};
if (_armed.armed) {
msg_arming.value = reg_drone_service_common_Readiness_0_1_ENGAGED;
msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
} else if (_armed.prearmed) {
msg_arming.value = reg_drone_service_common_Readiness_0_1_STANDBY;
msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
} else {
msg_arming.value = reg_drone_service_common_Readiness_0_1_SLEEP;
msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
}
uint8_t arming_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = arming_pid, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _arming_transfer_id,
.payload_size = reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &arming_payload_buffer,
};
int result = reg_drone_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
&transfer.payload_size);
int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
&payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&arming_payload_buffer
);
}
}
}
@ -127,7 +129,8 @@ public:
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
if (_port_id > 0) {
reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
if (i < num_outputs) {
@ -139,29 +142,30 @@ public:
}
}
PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1],
(double)msg_sp.value[2], (double)msg_sp.value[3]);
uint8_t esc_sp_payload_buffer[reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = _port_id, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _transfer_id,
.payload_size = reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &esc_sp_payload_buffer,
};
int result = reg_drone_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer,
&transfer.payload_size);
int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer,
&payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&esc_sp_payload_buffer);
}
}
};
@ -175,7 +179,7 @@ private:
/**
* ESC status message reception will be reported via this callback.
*/
void esc_status_sub_cb(const CanardTransfer &msg);
void esc_status_sub_cb(const CanardRxTransfer &msg);
uint8_t _rotor_count {0};

8
src/drivers/uavcan_v1/Actuators/EscServer.hpp → src/drivers/cyphal/Actuators/EscServer.hpp
View File

@ -34,14 +34,14 @@
/**
* @file EscServer.hpp
*
* Server-side implementation of DS-15 specification ESC service
* Server-side implementation of UDRAL specification ESC service
* (Used for CANNode control of an ESC via PWM output)
*
* Subscribes to the following UAVCAN v1 messages:
* Subscribes to the following Cyphal messages:
* reg.drone.service.actuator.common.sp.Value8.0.1
* reg.drone.service.common.Readiness.0.1
*
* Publishes to the following UAVCAN v1 messages:
* Publishes to the following Cyphal messages:
* reg.drone.service.actuator.common.Feedback.0.1
* reg.drone.service.actuator.common.Status.0.1
*
@ -62,7 +62,7 @@
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
// DS-15 Specification Messages
// UDRAL Specification Messages
#include <reg/drone/service/actuator/common/sp/Vector31_0_1.h>
#include <reg/drone/service/common/Readiness_0_1.h>

32
src/drivers/uavcan_v1/CMakeLists.txt → src/drivers/cyphal/CMakeLists.txt
View File

@ -41,7 +41,7 @@ px4_add_git_submodule(TARGET git_legacy_data_types PATH ${LEGACY_DSDL_DIR})
find_program(NNVG_PATH nnvg)
if(NNVG_PATH)
message("Generating UAVCANv1 DSDL headers using Nunavut")
message("Generating Cyphal DSDL headers using Nunavut")
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg)
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${LEGACY_DSDL_DIR}/uavcan ${LEGACY_DSDL_DIR}/legacy)
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan)
@ -66,14 +66,14 @@ if(${PX4_PLATFORM} MATCHES "nuttx")
endif()
endif()
if(CONFIG_UAVCAN_V1_NODE_MANAGER)
if(CONFIG_CYPHAL_NODE_MANAGER)
list(APPEND SRCS
NodeManager.hpp
NodeManager.cpp
)
endif()
if(CONFIG_UAVCAN_V1_NODE_CLIENT)
if(CONFIG_CYPHAL_NODE_CLIENT)
list(APPEND SRCS
NodeClient.hpp
NodeClient.cpp
@ -86,29 +86,31 @@ 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)
set(DRIVERS_CYPHAL_OPTIONS)
get_cmake_property(_variableNames VARIABLES)
foreach (_variableName ${_variableNames})
string(REGEX MATCH "^CONFIG_UAVCAN_V1_" UAVCAN_V1_OPTION ${_variableName})
string(REGEX MATCH "^CONFIG_CYPHAL_" CYPHAL_OPTION ${_variableName})
if(UAVCAN_V1_OPTION)
if(CYPHAL_OPTION)
if(${${_variableName}} STREQUAL "y")
list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=1")
list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=1")
else()
list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=${${_variableName}}")
list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=${${_variableName}}")
endif()
endif()
endforeach()
remove_definitions(-Werror)
px4_add_module(
MODULE drivers__uavcan_v1
MAIN uavcan_v1
MODULE drivers__cyphal
MAIN cyphal
STACK_MAIN 4096
COMPILE_FLAGS
#-DCANARD_ASSERT
-DUINT32_C\(x\)=__UINT32_C\(x\)
-O0
${DRIVERS_UAVCAN_V1_OPTIONS}
${DRIVERS_CYPHAL_OPTIONS}
INCLUDES
${LIBCANARD_DIR}/libcanard/
${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
@ -119,8 +121,9 @@ px4_add_module(
SubscriptionManager.hpp
ParamManager.hpp
ParamManager.cpp
Uavcan.cpp
Uavcan.hpp
Cyphal.cpp
Cyphal.hpp
CanardHandle.cpp
Publishers/uORB/uorb_publisher.cpp
Subscribers/uORB/uorb_subscriber.cpp
${SRCS}
@ -137,3 +140,6 @@ px4_add_module(
version
${DPNDS}
)
# libcanard 3.0 introduces this warning, for now no intention to fix it thus we ignore this warning
set_source_files_properties(${LIBCANARD_DIR}/libcanard/canard.c PROPERTIES COMPILE_FLAGS -Wno-cast-align)

216
src/drivers/cyphal/CanardHandle.cpp Normal file
View File

@ -0,0 +1,216 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#include "CanardHandle.hpp"
#include <net/if.h>
#include <sys/ioctl.h>
#include <string.h>
#include <px4_platform_common/log.h>
#include "o1heap/o1heap.h"
#include "Subscribers/BaseSubscriber.hpp"
#if defined(__PX4_NUTTX)
# if defined(CONFIG_NET_CAN)
# include "CanardSocketCAN.hpp"
# elif defined(CONFIG_CAN)
# include "CanardNuttXCDev.hpp"
# endif // CONFIG_CAN
#endif // NuttX
O1HeapInstance *uavcan_allocator{nullptr};
static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); }
static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); }
CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes)
{
_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
if (uavcan_allocator == nullptr) {
PX4_ERR("o1heapInit failed with size %u", HeapSize);
}
_canard_instance = canardInit(&memAllocate, &memFree);
_canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point.
_queue = canardTxInit(capacity, mtu_bytes);
#if defined(__PX4_NUTTX)
# if defined(CONFIG_NET_CAN)
_can_interface = new CanardSocketCAN();
# elif defined(CONFIG_CAN)
_can_interface = new CanardNuttXCDev();
# endif // CONFIG_CAN
#endif // NuttX
}
CanardHandle::~CanardHandle()
{
_can_interface->close();
delete _can_interface;
_can_interface = nullptr;
delete static_cast<uint8_t *>(_uavcan_heap);
_uavcan_heap = nullptr;
}
bool CanardHandle::init()
{
if (_can_interface) {
if (_can_interface->init() == PX4_OK) {
return true;
}
}
return false;
}
void CanardHandle::receive()
{
/* Process received messages */
uint8_t data[64] {};
CanardRxFrame received_frame{};
received_frame.frame.payload = &data;
while (_can_interface->receive(&received_frame) > 0) {
CanardRxTransfer receive{};
CanardRxSubscription *subscription = nullptr;
int32_t result = canardRxAccept(&_canard_instance, received_frame.timestamp_usec, &received_frame.frame, 0, &receive,
&subscription);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if
// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
// Reception of an invalid frame is NOT an error.
PX4_ERR("Receive error %" PRId32" \n", result);
} else if (result == 1) {
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (subscription != nullptr) {
UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
sub_instance->callback(receive);
} else {
PX4_ERR("No matching sub for %d", receive.metadata.port_id);
}
// Deallocate the dynamic memory afterwards.
_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
} else {
//PX4_INFO("RX canard %d", result);
}
}
}
void CanardHandle::transmit()
{
// Look at the top of the TX queue.
for (const CanardTxQueueItem *ti = NULL; (ti = canardTxPeek(&_queue)) != NULL;) { // Peek at the top of the queue.
if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > hrt_absolute_time())) { // Check the deadline.
// Send the frame. Redundant interfaces may be used here.
const int tx_res = _can_interface->transmit(*ti);
if (tx_res < 0) {
PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno));
} else if (tx_res == 0) {
// Timeout - just exit and try again later
break;
}
}
// After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
_canard_instance.memory_free(&_canard_instance, canardTxPop(&_queue, ti));
}
}
int32_t CanardHandle::TxPush(const CanardMicrosecond tx_deadline_usec,
const CanardTransferMetadata *const metadata,
const size_t payload_size,
const void *const payload)
{
return canardTxPush(&_queue, &_canard_instance, tx_deadline_usec, metadata, payload_size, payload);
}
int8_t CanardHandle::RxSubscribe(const CanardTransferKind transfer_kind,
const CanardPortID port_id,
const size_t extent,
const CanardMicrosecond transfer_id_timeout_usec,
CanardRxSubscription *const out_subscription)
{
return canardRxSubscribe(&_canard_instance, transfer_kind, port_id, extent, transfer_id_timeout_usec, out_subscription);
}
int8_t CanardHandle::RxUnsubscribe(const CanardTransferKind transfer_kind,
const CanardPortID port_id)
{
return canardRxUnsubscribe(&_canard_instance, transfer_kind, port_id);
}
CanardTreeNode *CanardHandle::getRxSubscriptions(CanardTransferKind kind)
{
return _canard_instance.rx_subscriptions[kind];
}
int32_t CanardHandle::mtu()
{
return _queue.mtu_bytes;
}
CanardNodeID CanardHandle::node_id()
{
return _canard_instance.node_id;
}
void CanardHandle::set_node_id(CanardNodeID id)
{
_canard_instance.node_id = id;
}

85
src/drivers/cyphal/CanardHandle.hpp Normal file
View File

@ -0,0 +1,85 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
#pragma once
#include <canard.h>
#include "CanardInterface.hpp"
class CanardHandle
{
/*
* This memory is allocated for the 01Heap allocator used by
* libcanard to store incoming/outcoming data
* Current size of 8192 bytes is arbitrary, should be optimized further
* when more nodes and messages are on the CAN bus
*/
static constexpr unsigned HeapSize = 8192;
public:
CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes);
~CanardHandle();
bool init();
void receive();
void transmit();
int32_t TxPush(const CanardMicrosecond tx_deadline_usec,
const CanardTransferMetadata *const metadata,
const size_t payload_size,
const void *const payload);
int8_t RxSubscribe(const CanardTransferKind transfer_kind,
const CanardPortID port_id,
const size_t extent,
const CanardMicrosecond transfer_id_timeout_usec,
CanardRxSubscription *const out_subscription);
int8_t RxUnsubscribe(const CanardTransferKind transfer_kind,
const CanardPortID port_id);
CanardTreeNode *getRxSubscriptions(CanardTransferKind kind);
int32_t mtu();
CanardNodeID node_id();
void set_node_id(CanardNodeID id);
private:
CanardInterface *_can_interface;
CanardInstance _canard_instance;
CanardTxQueue _queue;
void *_uavcan_heap{nullptr};
};

12
src/drivers/uavcan_v1/CanardInterface.hpp → src/drivers/cyphal/CanardInterface.hpp
View File

@ -35,6 +35,14 @@
#include <canard.h>
/// One frame stored in the transmission queue along with its metadata.
struct CanardRxFrame {
CanardMicrosecond timestamp_usec;
/// The actual CAN frame data.
CanardFrame frame;
};
class CanardInterface
{
public:
@ -48,12 +56,12 @@ public:
/// Send a CanardFrame
/// This function is blocking
/// The return value is number of bytes transferred, negative value on error.
virtual int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0) = 0;
virtual int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0) = 0;
/// Receive a CanardFrame
/// This function is blocking
/// The return value is number of bytes received, negative value on error.
virtual int16_t receive(CanardFrame *rxf) = 0;
virtual int16_t receive(CanardRxFrame *rxf) = 0;
private:

16
src/drivers/uavcan_v1/CanardNuttXCDev.cpp → src/drivers/cyphal/CanardNuttXCDev.cpp
View File

@ -65,7 +65,7 @@ int CanardNuttXCDev::init()
return 0;
}
int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms)
int16_t CanardNuttXCDev::transmit(const CanardTxQueueItem &txf, int timeout_ms)
{
if (_fd < 0) {
return -1;
@ -93,13 +93,13 @@ int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms)
struct can_msg_s transmit_msg {};
transmit_msg.cm_hdr.ch_id = txf.extended_can_id;
transmit_msg.cm_hdr.ch_id = txf.frame.extended_can_id;
transmit_msg.cm_hdr.ch_dlc = txf.payload_size;
transmit_msg.cm_hdr.ch_dlc = txf.frame.payload_size;
transmit_msg.cm_hdr.ch_extid = 1;
memcpy(transmit_msg.cm_data, txf.payload, txf.payload_size);
memcpy(transmit_msg.cm_data, txf.frame.payload, txf.frame.payload_size);
const size_t msg_len = CAN_MSGLEN(transmit_msg.cm_hdr.ch_dlc);
@ -112,7 +112,7 @@ int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms)
return 1;
}
int16_t CanardNuttXCDev::receive(CanardFrame *received_frame)
int16_t CanardNuttXCDev::receive(CanardRxFrame *received_frame)
{
if ((_fd < 0) || (received_frame == nullptr)) {
return -1;
@ -140,9 +140,9 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame)
return -1;
} else {
received_frame->extended_can_id = receive_msg.cm_hdr.ch_id;
received_frame->payload_size = receive_msg.cm_hdr.ch_dlc;
memcpy((void *)received_frame->payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc);
received_frame->frame.extended_can_id = receive_msg.cm_hdr.ch_id;
received_frame->frame.payload_size = receive_msg.cm_hdr.ch_dlc;
memcpy((void *)received_frame->frame.payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc);
return nbytes;
}
}

8
src/drivers/uavcan_v1/CanardNuttXCDev.hpp → src/drivers/cyphal/CanardNuttXCDev.hpp
View File

@ -51,15 +51,15 @@ public:
/// The return value is 0 on succes and -1 on error
int init();
/// Send a CanardFrame to the CanardSocketInstance socket
/// Send a CanardTxQueueItem to the CanardSocketInstance socket
/// This function is blocking
/// The return value is number of bytes transferred, negative value on error.
int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0);
int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0);
/// Receive a CanardFrame from the CanardSocketInstance socket
/// Receive a CanardRxFrame from the CanardSocketInstance socket
/// This function is blocking
/// The return value is number of bytes received, negative value on error.
int16_t receive(CanardFrame *rxf);
int16_t receive(CanardRxFrame *rxf);
private:
int _fd{-1};

30
src/drivers/uavcan_v1/CanardSocketCAN.cpp → src/drivers/cyphal/CanardSocketCAN.cpp
View File

@ -152,22 +152,22 @@ int CanardSocketCAN::init()
return 0;
}
int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
int16_t CanardSocketCAN::transmit(const CanardTxQueueItem &txf, int timeout_ms)
{
/* Copy CanardFrame to can_frame/canfd_frame */
if (_can_fd) {
_send_frame.can_id = txf.extended_can_id | CAN_EFF_FLAG;
_send_frame.len = txf.payload_size;
memcpy(&_send_frame.data, txf.payload, txf.payload_size);
_send_frame.can_id = txf.frame.extended_can_id | CAN_EFF_FLAG;
_send_frame.len = txf.frame.payload_size;
memcpy(&_send_frame.data, txf.frame.payload, txf.frame.payload_size);
} else {
struct can_frame *frame = (struct can_frame *)&_send_frame;
frame->can_id = txf.extended_can_id | CAN_EFF_FLAG;
frame->can_dlc = txf.payload_size;
memcpy(&frame->data, txf.payload, txf.payload_size);
frame->can_id = txf.frame.extended_can_id | CAN_EFF_FLAG;
frame->can_dlc = txf.frame.payload_size;
memcpy(&frame->data, txf.frame.payload, txf.frame.payload_size);
}
uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.timestamp_usec - hrt_absolute_time()) +
uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.tx_deadline_usec - hrt_absolute_time()) +
CONFIG_USEC_PER_TICK; // Compensate for precision loss when converting hrt to systick
/* Set CAN_RAW_TX_DEADLINE timestamp */
@ -177,7 +177,7 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
return sendmsg(_fd, &_send_msg, 0);
}
int16_t CanardSocketCAN::receive(CanardFrame *rxf)
int16_t CanardSocketCAN::receive(CanardRxFrame *rxf)
{
int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT);
@ -189,15 +189,15 @@ int16_t CanardSocketCAN::receive(CanardFrame *rxf)
if (_can_fd) {
struct canfd_frame *recv_frame = (struct canfd_frame *)&_recv_frame;
rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
rxf->payload_size = recv_frame->len;
rxf->payload = &recv_frame->data;
rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
rxf->frame.payload_size = recv_frame->len;
rxf->frame.payload = &recv_frame->data;
} else {
struct can_frame *recv_frame = (struct can_frame *)&_recv_frame;
rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
rxf->payload_size = recv_frame->can_dlc;
rxf->payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference
rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
rxf->frame.payload_size = recv_frame->can_dlc;
rxf->frame.payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference
}
/* Read SO_TIMESTAMP value */

4
src/drivers/uavcan_v1/CanardSocketCAN.hpp → src/drivers/cyphal/CanardSocketCAN.hpp
View File

@ -71,12 +71,12 @@ public:
/// Send a CanardFrame to the CanardSocketInstance socket
/// This function is blocking
/// The return value is number of bytes transferred, negative value on error.
int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0);
int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0);
/// Receive a CanardFrame from the CanardSocketInstance socket
/// This function is blocking
/// The return value is number of bytes received, negative value on error.
int16_t receive(CanardFrame *rxf);
int16_t receive(CanardRxFrame *rxf);
// TODO implement ioctl for CAN filter
//int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters);

262
src/drivers/uavcan_v1/Uavcan.cpp → src/drivers/cyphal/Cyphal.cpp
View File

@ -31,13 +31,13 @@
*
****************************************************************************/
#include "Uavcan.hpp"
#include "Cyphal.hpp"
#include <lib/geo/geo.h>
#include <lib/version/version.h>
#ifdef CONFIG_UAVCAN_V1_APP_DESCRIPTOR
#ifdef CONFIG_CYPHAL_APP_DESCRIPTOR
#include "boot_app_shared.h"
/*
* This is the AppImageDescriptor used
@ -60,54 +60,22 @@ boot_app_shared_section app_descriptor_t AppDescriptor = {
using namespace time_literals;
UavcanNode *UavcanNode::_instance;
CyphalNode *CyphalNode::_instance;
O1HeapInstance *uavcan_allocator{nullptr};
static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); }
static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); }
#if defined(__PX4_NUTTX)
# if defined(CONFIG_NET_CAN)
# include "CanardSocketCAN.hpp"
# elif defined(CONFIG_CAN)
# include "CanardNuttXCDev.hpp"
# endif // CONFIG_CAN
#endif // NuttX
UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
_can_interface(interface)
_canard_handle(node_id, capacity, mtu_bytes)
{
pthread_mutex_init(&_node_mutex, nullptr);
_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
if (uavcan_allocator == nullptr) {
PX4_ERR("o1heapInit failed with size %u", HeapSize);
}
_canard_instance = canardInit(&memAllocate, &memFree);
_canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point.
bool can_fd = false;
if (can_fd) {
_canard_instance.mtu_bytes = CANARD_MTU_CAN_FD;
} else {
_canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
}
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
#ifdef CONFIG_CYPHAL_NODE_MANAGER
_node_manager.subscribe();
#endif
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
_node_client = new NodeClient(_canard_instance, _param_manager);
#ifdef CONFIG_CYPHAL_NODE_CLIENT
_node_client = new NodeClient(_canard_handle, _param_manager);
_node_client->subscribe();
#endif
@ -117,7 +85,7 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
_sub_manager.subscribe();
}
UavcanNode::~UavcanNode()
CyphalNode::~CyphalNode()
{
if (_instance) {
/* tell the task we want it to go away */
@ -138,32 +106,25 @@ UavcanNode::~UavcanNode()
} while (_instance);
}
delete _can_interface;
_can_interface = nullptr;
perf_free(_cycle_perf);
perf_free(_interval_perf);
delete static_cast<uint8_t *>(_uavcan_heap);
_uavcan_heap = nullptr;
}
int UavcanNode::start(uint32_t node_id, uint32_t bitrate)
int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
PX4_WARN("Already started");
return -1;
}
#if defined(__PX4_NUTTX)
# if defined(CONFIG_NET_CAN)
CanardInterface *interface = new CanardSocketCAN();
# elif defined(CONFIG_CAN)
CanardInterface *interface = new CanardNuttXCDev();
# endif // CONFIG_CAN
#endif // NuttX
bool can_fd = false;
_instance = new UavcanNode(interface, node_id);
if (can_fd) {
_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);
} else {
_instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC);
}
if (_instance == nullptr) {
PX4_ERR("Out of memory");
@ -178,17 +139,16 @@ int UavcanNode::start(uint32_t node_id, uint32_t bitrate)
return PX4_OK;
}
void UavcanNode::init()
void CyphalNode::init()
{
// interface init
if (_can_interface) {
if (_can_interface->init() == PX4_OK) {
_initialized = true;
}
if (_canard_handle.init()) {
_initialized = true;
}
}
void UavcanNode::Run()
void CyphalNode::Run()
{
pthread_mutex_lock(&_node_mutex);
@ -221,22 +181,22 @@ void UavcanNode::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) {
if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) {
// send uavcan::node::Heartbeat_1_0 @ 1 Hz
sendHeartbeat();
// Check all publishers
_pub_manager.update();
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
#ifdef CONFIG_CYPHAL_NODE_MANAGER
_node_manager.update();
#endif
}
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
#ifdef CONFIG_CYPHAL_NODE_CLIENT
else if (_node_client != nullptr) {
if (_canard_instance.node_id == CANARD_NODE_ID_UNSET) {
if (_canard_handle.node_id() == CANARD_NODE_ID_UNSET) {
_node_client->update();
} else {
@ -246,56 +206,19 @@ void UavcanNode::Run()
#endif
transmit();
_canard_handle.transmit();
/* Process received messages */
_canard_handle.receive();
uint8_t data[64] {};
CanardFrame received_frame{};
received_frame.payload = &data;
while (!_task_should_exit.load() && _can_interface->receive(&received_frame) > 0) {
CanardTransfer receive{};
CanardRxSubscription *subscription = nullptr;
int32_t result = canardRxAccept2(&_canard_instance, &received_frame, 0, &receive, &subscription);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if
// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
// Reception of an invalid frame is NOT an error.
PX4_ERR("Receive error %" PRId32" \n", result);
} else if (result == 1) {
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (subscription != nullptr) {
UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
sub_instance->callback(receive);
} else {
PX4_ERR("No matching sub for %d", receive.port_id);
}
// Deallocate the dynamic memory afterwards.
_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
} else {
//PX4_INFO("RX canard %d", result);
}
}
// Pop canardTx queue to send out responses to requets
transmit();
// Pop canardTx queue to send out responses to requests
_canard_handle.transmit();
perf_end(_cycle_perf);
if (_instance && _task_should_exit.load()) {
ScheduleClear();
if (_initialized && _can_interface != nullptr) {
_can_interface->close();
if (_initialized) {
_initialized = false;
}
@ -305,101 +228,62 @@ void UavcanNode::Run()
pthread_mutex_unlock(&_node_mutex);
}
void UavcanNode::transmit()
template <typename As, typename F>
static void traverseTree(const CanardTreeNode *const root, const F &op) // NOLINT this recursion is tightly bounded
{
// Look at the top of the TX queue.
for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
// Otherwise just drop it and move on to the next one.
const hrt_abstime now = hrt_absolute_time();
if (txf->timestamp_usec == 0 || txf->timestamp_usec > now) {
// Send the frame. Redundant interfaces may be used here.
const int tx_res = _can_interface->transmit(*txf);
if (tx_res < 0) {
// Failure - drop the frame and report
canardTxPop(&_canard_instance);
// Deallocate the dynamic memory afterwards.
_canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf);
PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno));
} else if (tx_res > 0) {
// Success - just drop the frame
canardTxPop(&_canard_instance);
// Deallocate the dynamic memory afterwards.
_canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf);
} else {
// Timeout - just exit and try again later
break;
}
} else if (txf->timestamp_usec <= now) {
// Transmission timed out -- remove from queue and deallocate its memory
canardTxPop(&_canard_instance);
_canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf);
}
if (root != nullptr) {
traverseTree<As, F>(root->lr[0], op);
op(static_cast<const As *>(static_cast<const void *>(root)));
traverseTree<As, F>(root->lr[1], op);
}
}
void UavcanNode::print_info()
void CyphalNode::print_info()
{
pthread_mutex_lock(&_node_mutex);
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
/*O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
heap_diagnostics.allocated, heap_diagnostics.capacity,
heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
heap_diagnostics.oom_count);
heap_diagnostics.oom_count);*/
_pub_manager.printInfo();
CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage];
while (rxs != nullptr) {
if (rxs->user_reference == nullptr) {
PX4_INFO("Message port id %d", rxs->port_id);
traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindMessage),
[&](const CanardRxSubscription * const sub) {
if (sub->user_reference == nullptr) {
PX4_INFO("Message port id %d", sub->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
}
});
rxs = rxs->next;
}
rxs = _canard_instance.rx_subscriptions[CanardTransferKindRequest];
while (rxs != nullptr) {
if (rxs->user_reference == nullptr) {
PX4_INFO("Service response port id %d", rxs->port_id);
traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindRequest),
[&](const CanardRxSubscription * const sub) {
if (sub->user_reference == nullptr) {
PX4_INFO("Service response port id %d", sub->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
}
});
rxs = rxs->next;
}
rxs = _canard_instance.rx_subscriptions[CanardTransferKindResponse];
while (rxs != nullptr) {
if (rxs->user_reference == nullptr) {
PX4_INFO("Service request port id %d", rxs->port_id);
traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindResponse),
[&](const CanardRxSubscription * const sub) {
if (sub->user_reference == nullptr) {
PX4_INFO("Service request port id %d", sub->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
}
rxs = rxs->next;
}
});
_mixing_output.printInfo();
@ -412,7 +296,7 @@ static void print_usage()
"\tuavcannode {start|status|stop}");
}
extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
extern "C" __EXPORT int cyphal_main(int argc, char *argv[])
{
if (argc < 2) {
print_usage();
@ -420,18 +304,18 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
if (CyphalNode::instance()) {
PX4_ERR("already started");
return 1;
}
// CAN bitrate
int32_t bitrate = 0;
param_get(param_find("UAVCAN_V1_BAUD"), &bitrate);
param_get(param_find("CYPHAL_BAUD"), &bitrate);
// Node ID
int32_t node_id = 0;
param_get(param_find("UAVCAN_V1_ID"), &node_id);
param_get(param_find("CYPHAL_ID"), &node_id);
if (node_id == -1) {
node_id = CANARD_NODE_ID_UNSET;
@ -439,11 +323,11 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
// Start
PX4_INFO("Node ID %" PRIu32 ", bitrate %" PRIu32, node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
return CyphalNode::start(node_id, bitrate);
}
/* commands below require the app to be started */
UavcanNode *const inst = UavcanNode::instance();
CyphalNode *const inst = CyphalNode::instance();
if (!inst) {
PX4_ERR("application not running");
@ -464,7 +348,7 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
return 1;
}
void UavcanNode::sendHeartbeat()
void CyphalNode::sendHeartbeat()
{
if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) {
@ -473,21 +357,23 @@ void UavcanNode::sendHeartbeat()
heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;
const hrt_abstime now = hrt_absolute_time();
size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
CanardTransfer transfer = {
.timestamp_usec = now + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _uavcan_node_heartbeat_transfer_id++,
.payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &_uavcan_node_heartbeat_buffer,
.transfer_id = _uavcan_node_heartbeat_transfer_id++
};
uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size);
uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &payload_size);
int32_t result = canardTxPush(&_canard_instance, &transfer);
int32_t result = _canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&_uavcan_node_heartbeat_buffer
);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.

58
src/drivers/uavcan_v1/Uavcan.hpp → src/drivers/cyphal/Cyphal.hpp
View File

@ -51,21 +51,18 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include "o1heap/o1heap.h"
#include <canard.h>
#include <canard_dsdl.h>
#include "CanardInterface.hpp"
#include "Publishers/Publisher.hpp"
#include "Publishers/uORB/uorb_publisher.hpp"
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
#ifdef CONFIG_CYPHAL_NODE_MANAGER
#include "NodeManager.hpp"
#endif
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
#ifdef CONFIG_CYPHAL_NODE_CLIENT
#include "NodeClient.hpp"
#endif
@ -76,8 +73,8 @@
/**
* UAVCAN mixing class.
* It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling
* (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at
* It is separate from CyphalNode to have 2 WorkItems and therefore allowing independent scheduling
* (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas CyphalNode runs at
* a fixed rate or upon bus updates).
* Both work items are expected to run on the same work queue.
*/
@ -106,22 +103,15 @@ public:
protected:
void Run() override;
private:
friend class UavcanNode;
friend class CyphalNode;
pthread_mutex_t &_node_mutex;
UavcanEscController &_esc_controller;
// UavcanServoController &_servo_controller;
MixingOutput _mixing_output{"UCAN1_ESC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
};
class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem
{
/*
* This memory is allocated for the 01Heap allocator used by
* libcanard to store incoming/outcoming data
* Current size of 8192 bytes is arbitrary, should be optimized further
* when more nodes and messages are on the CAN bus
*/
static constexpr unsigned HeapSize = 8192;
/*
* Base interval, has to be complemented with events from the CAN driver
@ -131,14 +121,14 @@ class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
public:
UavcanNode(CanardInterface *interface, uint32_t node_id);
~UavcanNode() override;
CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes);
~CyphalNode() override;
static int start(uint32_t node_id, uint32_t bitrate);
void print_info();
static UavcanNode *instance() { return _instance; }
static CyphalNode *instance() { return _instance; }
/* The bit rate that can be passed back to the bootloader */
int32_t active_bitrate{0};
@ -148,22 +138,16 @@ private:
void Run() override;
void fill_node_info();
void transmit();
// Sends a heartbeat at 1s intervals
void sendHeartbeat();
void *_uavcan_heap{nullptr};
CanardInterface *_can_interface;
CanardInstance _canard_instance;
px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver
bool _initialized{false}; ///< number of actuators currently available
static UavcanNode *_instance;
static CyphalNode *_instance;
CanardHandle _canard_handle;
pthread_mutex_t _node_mutex;
@ -178,26 +162,26 @@ private:
CanardTransferID _uavcan_node_heartbeat_transfer_id{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud
(ParamInt<px4::params::CYPHAL_ENABLE>) _param_uavcan_v1_enable,
(ParamInt<px4::params::CYPHAL_ID>) _param_uavcan_v1_id,
(ParamInt<px4::params::CYPHAL_BAUD>) _param_uavcan_v1_baud
)
UavcanParamManager _param_manager;
#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER
NodeManager _node_manager {_canard_instance, _param_manager};
#ifdef CONFIG_CYPHAL_NODE_MANAGER
NodeManager _node_manager {_canard_handle, _param_manager};
#endif
#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT
#ifdef CONFIG_CYPHAL_NODE_CLIENT
NodeClient *_node_client {nullptr};
#endif
PublicationManager _pub_manager {_canard_instance, _param_manager};
SubscriptionManager _sub_manager {_canard_instance, _param_manager};
PublicationManager _pub_manager {_canard_handle, _param_manager};
SubscriptionManager _sub_manager {_canard_handle, _param_manager};
/// TODO: Integrate with PublicationManager
UavcanEscController _esc_controller {_canard_instance, _param_manager};
UavcanEscController _esc_controller {_canard_handle, _param_manager};
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};

56
src/drivers/uavcan_v1/Kconfig → src/drivers/cyphal/Kconfig
View File

@ -2,65 +2,65 @@
# For a description of the syntax of this configuration file,
# see the file kconfig-language.txt in the NuttX tools repository.
#
menuconfig DRIVERS_UAVCAN_V1
bool "UAVCANv1"
menuconfig DRIVERS_CYPHAL
bool "Cyphal"
default n
---help---
Enable support for UAVCANv1
Enable support for Cyphal
if DRIVERS_UAVCAN_V1
if DRIVERS_CYPHAL
choice
prompt "UAVCANv1 Mode"
prompt "Cyphal Mode"
config UAVCAN_V1_FMU
config CYPHAL_FMU
bool "Server (FMU)"
config UAVCAN_V1_CLIENT
config CYPHAL_CLIENT
bool "Client (Peripheral)"
endchoice
config UAVCAN_V1_NODE_MANAGER
config CYPHAL_NODE_MANAGER
bool "Node manager"
default y
depends on UAVCAN_V1_FMU
depends on CYPHAL_FMU
help
Implement UAVCAN v1 PNP server functionality and manages discovered nodes
Implement Cyphal PNP server functionality and manages discovered nodes
config UAVCAN_V1_NODE_CLIENT
config CYPHAL_NODE_CLIENT
bool "Node client"
default y
depends on UAVCAN_V1_CLIENT
depends on CYPHAL_CLIENT
help
Implement UAVCAN v1 PNP client functionality
Implement Cyphal PNP client functionality
config UAVCAN_V1_APP_DESCRIPTOR
config CYPHAL_APP_DESCRIPTOR
bool "UAVCAN v0 bootloader app descriptor"
default n
depends on UAVCAN_V1_CLIENT && DRIVERS_BOOTLOADERS
depends on CYPHAL_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
config CYPHAL_GNSS_PUBLISHER
bool "GNSS Publisher"
default n
config UAVCAN_V1_ESC_CONTROLLER
config CYPHAL_ESC_CONTROLLER
bool "ESC Controller"
default n
config UAVCAN_V1_READINESS_PUBLISHER
config CYPHAL_READINESS_PUBLISHER
bool "Readiness Publisher"
default n
config UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER
config CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER
bool "uORB actuator_outputs publisher"
default n
config UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
config CYPHAL_UORB_SENSOR_GPS_PUBLISHER
bool "uORB sensor_gps publisher"
default n
@ -68,36 +68,36 @@ if DRIVERS_UAVCAN_V1
menu "Subscriber support"
config UAVCAN_V1_ESC_SUBSCRIBER
config CYPHAL_ESC_SUBSCRIBER
bool "ESC Subscriber"
default n
config UAVCAN_V1_GNSS_SUBSCRIBER_0
config CYPHAL_GNSS_SUBSCRIBER_0
bool "GNSS Subscriber 0"
default n
config UAVCAN_V1_GNSS_SUBSCRIBER_1
config CYPHAL_GNSS_SUBSCRIBER_1
bool "GNSS Subscriber 1"
default n
config UAVCAN_V1_BMS_SUBSCRIBER
config CYPHAL_BMS_SUBSCRIBER
bool "BMS Subscriber"
default n
config UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER
config CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
bool "uORB sensor_gps Subscriber"
default n
endmenu
menu "Advertised Services"
config UAVCAN_V1_GETINFO_RESPONDER
config CYPHAL_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
config CYPHAL_EXECUTECOMMAND_RESPONDER
bool "ExecuteCommand1.0 responder"
default n
help
@ -107,4 +107,4 @@ if DRIVERS_UAVCAN_V1
menu "Service invokers"
endmenu
endif #DRIVERS_UAVCAN_V1
endif #DRIVERS_CYPHAL

40
src/drivers/uavcan_v1/NodeClient.cpp → src/drivers/cyphal/NodeClient.cpp
View File

@ -44,15 +44,15 @@
#include <crc64.h>
#include "NodeClient.hpp"
void NodeClient::callback(const CanardTransfer &receive)
void NodeClient::callback(const CanardRxTransfer &receive)
{
if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id == CANARD_NODE_ID_UNSET) {
if (receive.metadata.remote_node_id != CANARD_NODE_ID_UNSET && _canard_handle.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) {
if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) {
uavcan_pnp_NodeIDAllocationData_2_0 msg;
size_t msg_size_in_bytes = receive.payload_size;
@ -87,9 +87,9 @@ void NodeClient::callback(const CanardTransfer &receive)
return;
}
_canard_instance.node_id = allocated;
_canard_handle.set_node_id(allocated);
PX4_INFO("Allocated Node ID %d", _canard_instance.node_id);
PX4_INFO("Allocated Node ID %d", _canard_handle.node_id());
}
}
@ -103,38 +103,39 @@ void NodeClient::update()
int32_t result;
// Allocation already done, nothing to do
if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) {
if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) {
return;
}
if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) {
if (_canard_handle.mtu() == 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];
size_t payload_size = 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.
const CanardTransferMetadata transfer_metadata = {
.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);
&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);
_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&node_id_alloc_payload_buffer);
}
} else {
@ -142,29 +143,30 @@ void NodeClient::update()
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];
size_t payload_size = 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.
const CanardTransferMetadata transfer_metadata = {
.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);
&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);
_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&node_id_alloc_payload_buffer);
}
}

20
src/drivers/uavcan_v1/NodeClient.hpp → src/drivers/cyphal/NodeClient.hpp
View File

@ -62,30 +62,30 @@
class NodeClient : public UavcanBaseSubscriber
{
public:
NodeClient(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
_canard_instance(ins) { };
NodeClient(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData",
0),
_canard_handle(handle) { };
void subscribe() override
{
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
(_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID
(_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
}
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
void callback(const CanardTransfer &receive); // NodeIDAllocation callback
void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback
void update();
private:
CanardInstance &_canard_instance;
CanardHandle &_canard_handle;
CanardTransferID _node_id_alloc_transfer_id{0};
hrt_abstime _nodealloc_request_last{0};

27
src/drivers/uavcan_v1/NodeManager.cpp → src/drivers/cyphal/NodeManager.cpp
View File

@ -43,9 +43,9 @@
#include "NodeManager.hpp"
void NodeManager::callback(const CanardTransfer &receive)
void NodeManager::callback(const CanardRxTransfer &receive)
{
if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) {
if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) {
uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg {};
size_t msg_size_in_bytes = receive.payload_size;
uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
@ -73,7 +73,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
/* Search for an available NodeID to assign */
for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
if (i == _canard_instance.node_id) {
if (i == _canard_handle.node_id()) {
continue; // Don't give our NodeID to a node
} else if (nodeid_registry[i].node_id == 0) { // Unused
@ -96,24 +96,25 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id,
.payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &node_id_alloc_payload_buffer,
};
int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);
int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&node_id_alloc_payload_buffer);
}
_register_request_last = hrt_absolute_time();
@ -132,7 +133,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg)
}
void NodeManager::HandleListResponse(const CanardTransfer &receive)
void NodeManager::HandleListResponse(const CanardRxTransfer &receive)
{
uavcan_register_List_Response_1_0 msg;
@ -142,21 +143,21 @@ void NodeManager::HandleListResponse(const CanardTransfer &receive)
if (msg.name.name.count == 0) {
// Index doesn't exist, we've parsed through all registers
for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
if (nodeid_registry[i].node_id == receive.remote_node_id) {
if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) {
nodeid_registry[i].register_setup = true; // Don't update anymore
}
}
} else {
for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
if (nodeid_registry[i].node_id == receive.remote_node_id) {
if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) {
nodeid_registry[i].register_index++; // Increment index counter for next update()
nodeid_registry[i].register_setup = false;
nodeid_registry[i].retry_count = 0;
}
}
if (_access_request.setPortId(receive.remote_node_id, msg.name, NULL)) { //FIXME confirm handler
if (_access_request.setPortId(receive.metadata.remote_node_id, msg.name, NULL)) { //FIXME confirm handler
PX4_INFO("Set portID succesfull");
} else {

24
src/drivers/uavcan_v1/NodeManager.hpp → src/drivers/cyphal/NodeManager.hpp
View File

@ -70,37 +70,37 @@ typedef struct {
class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface
{
public:
NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "", "NodeIDAllocationData", 0),
_canard_instance(ins), _access_request(ins, pmgr), _list_request(ins) { };
NodeManager(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData",
0),
_canard_handle(handle), _access_request(handle, pmgr), _list_request(handle) { };
void subscribe() override
{
_access_request.subscribe();
_list_request.subscribe();
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
(_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID
(_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
}
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
void response_callback(const CanardTransfer &receive) override
void response_callback(const CanardRxTransfer &receive) override
{
HandleListResponse(receive);
}
void callback(const CanardTransfer &receive); // NodeIDAllocation callback
void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback
void HandleListResponse(const CanardTransfer &receive);
void HandleListResponse(const CanardRxTransfer &receive);
void update();
private:
CanardInstance &_canard_instance;
CanardHandle &_canard_handle;
CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
UavcanNodeEntry nodeid_registry[16] {0}; //TODO configurable or just rewrite

0
src/drivers/uavcan_v1/ParamManager.cpp → src/drivers/cyphal/ParamManager.cpp
View File

0
src/drivers/uavcan_v1/ParamManager.hpp → src/drivers/cyphal/ParamManager.hpp
View File

7
src/drivers/uavcan_v1/PublicationManager.cpp → src/drivers/cyphal/PublicationManager.cpp
View File

@ -56,10 +56,11 @@ void PublicationManager::updateDynamicPublications()
for (auto &dynpub : _dynpublishers) {
// Check if subscriber has already been created
const char *subj_name = dynpub->getSubjectName();
char full_subj_name[200];
snprintf(full_subj_name, sizeof(full_subj_name), "%s%s", dynpub->getPrefixName(), dynpub->getSubjectName());
const uint8_t instance = dynpub->getInstance();
if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) {
if (strcmp(full_subj_name, sub.subject_name) == 0 && instance == sub.instance) {
found_publisher = true;
break;
}
@ -77,7 +78,7 @@ void PublicationManager::updateDynamicPublications()
uint16_t port_id = value.natural16.value.elements[0];
if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber
UavcanPublisher *dynpub = sub.create_pub(_canard_instance, _param_manager);
UavcanPublisher *dynpub = sub.create_pub(_canard_handle, _param_manager);
if (dynpub == nullptr) {
PX4_ERR("Out of memory");

70
src/drivers/uavcan_v1/PublicationManager.hpp → src/drivers/cyphal/PublicationManager.hpp
View File

@ -44,33 +44,33 @@
#include <px4_platform_common/px4_config.h>
#ifndef CONFIG_UAVCAN_V1_GNSS_PUBLISHER
#define CONFIG_UAVCAN_V1_GNSS_PUBLISHER 0
#ifndef CONFIG_CYPHAL_GNSS_PUBLISHER
#define CONFIG_CYPHAL_GNSS_PUBLISHER 0
#endif
#ifndef CONFIG_UAVCAN_V1_ESC_CONTROLLER
#define CONFIG_UAVCAN_V1_ESC_CONTROLLER 0
#ifndef CONFIG_CYPHAL_ESC_CONTROLLER
#define CONFIG_CYPHAL_ESC_CONTROLLER 0
#endif
#ifndef CONFIG_UAVCAN_V1_READINESS_PUBLISHER
#define CONFIG_UAVCAN_V1_READINESS_PUBLISHER 0
#ifndef CONFIG_CYPHAL_READINESS_PUBLISHER
#define CONFIG_CYPHAL_READINESS_PUBLISHER 0
#endif
#ifndef CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER
#define CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0
#ifndef CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER
#define CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0
#endif
#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER 0
#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
#define CONFIG_CYPHAL_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
#define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
CONFIG_CYPHAL_ESC_CONTROLLER + \
CONFIG_CYPHAL_READINESS_PUBLISHER + \
CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
@ -81,12 +81,12 @@
#include <uORB/topics/sensor_gps.h>
#include "Actuators/EscClient.hpp"
#include "Publishers/DS-015/Readiness.hpp"
#include "Publishers/DS-015/Gnss.hpp"
#include "Publishers/udral/Readiness.hpp"
#include "Publishers/udral/Gnss.hpp"
#include "Publishers/uORB/uorb_publisher.hpp"
typedef struct {
UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
UavcanPublisher *(*create_pub)(CanardHandle &handle, UavcanParamManager &pmgr) {};
const char *subject_name;
const uint8_t instance;
} UavcanDynPubBinder;
@ -94,7 +94,7 @@ typedef struct {
class PublicationManager
{
public:
PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {}
PublicationManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {}
~PublicationManager();
void update();
@ -104,57 +104,57 @@ public:
private:
void updateDynamicPublications();
CanardInstance &_canard_instance;
CanardHandle &_canard_handle;
UavcanParamManager &_param_manager;
List<UavcanPublisher *> _dynpublishers;
const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] {
#if CONFIG_UAVCAN_V1_GNSS_PUBLISHER
#if CONFIG_CYPHAL_GNSS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
{
return new UavcanGnssPublisher(ins, pmgr, 0);
return new UavcanGnssPublisher(handle, pmgr, 0);
},
"gps",
0
},
#endif
#if CONFIG_UAVCAN_V1_ESC_CONTROLLER
#if CONFIG_CYPHAL_ESC_CONTROLLER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
{
return new UavcanEscController(ins, pmgr);
return new UavcanEscController(handle, pmgr);
},
"esc",
0
},
#endif
#if CONFIG_UAVCAN_V1_READINESS_PUBLISHER
#if CONFIG_CYPHAL_READINESS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
{
return new UavcanReadinessPublisher(ins, pmgr, 0);
return new UavcanReadinessPublisher(handle, pmgr, 0);
},
"readiness",
0
},
#endif
#if CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER
#if CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
{
return new uORB_over_UAVCAN_Publisher<actuator_outputs_s>(ins, pmgr, ORB_ID(actuator_outputs));
return new uORB_over_UAVCAN_Publisher<actuator_outputs_s>(handle, pmgr, ORB_ID(actuator_outputs));
},
"uorb.actuator_outputs",
0
},
#endif
#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
#if CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
{
return new uORB_over_UAVCAN_Publisher<sensor_gps_s>(ins, pmgr, ORB_ID(sensor_gps));
return new uORB_over_UAVCAN_Publisher<sensor_gps_s>(handle, pmgr, ORB_ID(sensor_gps));
},
"uorb.sensor_gps",
0

9
src/drivers/uavcan_v1/Publishers/Publisher.hpp → src/drivers/cyphal/Publishers/Publisher.hpp
View File

@ -34,7 +34,7 @@
/**
* @file Publisher.hpp
*
* Defines basic functionality of UAVCAN v1 publisher class
* Defines basic functionality of Cyphal publisher class
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
@ -48,6 +48,7 @@
#include <uavcan/_register/Access_1_0.h>
#include "../CanardHandle.hpp"
#include "../CanardInterface.hpp"
#include "../ParamManager.hpp"
@ -60,9 +61,9 @@
class UavcanPublisher : public ListNode<UavcanPublisher *>
{
public:
UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name,
UavcanPublisher(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name,
uint8_t instance = 0) :
_canard_instance(ins), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name),
_canard_handle(handle), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name),
_instance(instance) { };
virtual ~UavcanPublisher() = default;
@ -132,7 +133,7 @@ public:
}
protected:
CanardInstance &_canard_instance;
CanardHandle &_canard_handle;
UavcanParamManager &_param_manager;
const char *_prefix_name;
const char *_subject_name;

0
src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.cpp → src/drivers/cyphal/Publishers/uORB/uorb_publisher.cpp
View File

16
src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp → src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp
View File

@ -50,9 +50,9 @@ template <class T>
class uORB_over_UAVCAN_Publisher : public UavcanPublisher
{
public:
uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
uORB_over_UAVCAN_Publisher(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta,
uint8_t instance = 0) :
UavcanPublisher(ins, pmgr, "uorb.", meta->o_name, instance),
UavcanPublisher(handle, pmgr, "uorb.", meta->o_name, instance),
_uorb_meta{meta},
_uorb_sub(meta)
{};
@ -67,20 +67,20 @@ public:
T data {};
_uorb_sub.update(&data);
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = _port_id, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _transfer_id,
.payload_size = get_payload_size(&data),
.payload = &data,
.transfer_id = _transfer_id
};
// set the data ready in the buffer and chop if needed
++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
canardTxPush(&_canard_instance, &transfer);
_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
get_payload_size(&data),
&data);
}
};

64
src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp → src/drivers/cyphal/Publishers/udral/Gnss.hpp
View File

@ -34,24 +34,23 @@
/**
* @file Gnss.hpp
*
* Defines basic functionality of UAVCAN v1 GNSS publisher
* Defines basic functionality of Cyphal GNSS publisher
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
#include <reg/drone/service/gnss/DilutionOfPrecision_0_1.h>
// UDRAL Specification Messages
#include <reg/udral/physics/kinematics/geodetic/Point_0_1.h>
#include "../Publisher.hpp"
class UavcanGnssPublisher : public UavcanPublisher
{
public:
UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanPublisher(ins, pmgr, "ds_015", "gps", instance)
UavcanGnssPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanPublisher(handle, pmgr, "udral", "gps", instance)
{
};
@ -64,66 +63,33 @@ public:
if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
sensor_gps_s gps {};
_gps_sub.update(&gps);
size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
reg_udral_physics_kinematics_geodetic_Point_0_1 geo {};
geo.latitude = gps.lat;
geo.longitude = gps.lon;
geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast<double>(gps.alt) };
uint8_t geo_payload_buffer[reg_drone_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = _port_id, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _transfer_id,
.payload_size = reg_drone_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &geo_payload_buffer,
};
int32_t result = reg_drone_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer,
&transfer.payload_size);
int32_t result = reg_udral_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer,
&payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
/// TODO: Also publish DilutionOfPrecision, ...?
reg_drone_service_gnss_DilutionOfPrecision_0_1 dop {
.geometric = NAN,
.position = NAN,
.horizontal = gps.hdop,
.vertical = gps.vdop,
.time = NAN,
.northing = NAN,
.easting = NAN,
};
uint8_t dop_payload_buffer[reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardPortID _port_id_2 = static_cast<CanardPortID>((uint16_t)_port_id + 1U);
CanardTransfer transfer2 = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = _port_id_2, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _transfer_id_2,
.payload_size = reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &dop_payload_buffer,
};
result = reg_drone_service_gnss_DilutionOfPrecision_0_1_serialize_(&dop, dop_payload_buffer, &transfer2.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_transfer_id_2; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer2);
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&geo_payload_buffer);
}
}
};

33
src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp → src/drivers/cyphal/Publishers/udral/Readiness.hpp
View File

@ -34,7 +34,7 @@
/**
* @file Readiness.hpp
*
* Defines the UAVCAN v1 readiness publisher
* Defines the Cyphal readiness publisher
* readiness state is used to command or report the availability status
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
@ -42,16 +42,16 @@
#pragma once
// DS-15 Specification Messages
#include <reg/drone/service/common/Readiness_0_1.h>
// UDRAL Specification Messages
#include <reg/udral/service/common/Readiness_0_1.h>
#include "../Publisher.hpp"
class UavcanReadinessPublisher : public UavcanPublisher
{
public:
UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance)
UavcanReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanPublisher(handle, pmgr, "udral", "readiness", instance)
{
};
@ -65,36 +65,37 @@ public:
if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
actuator_armed_s armed {};
_actuator_armed_sub.update(&armed);
size_t payload_size;
reg_drone_service_common_Readiness_0_1 readiness {};
reg_udral_service_common_Readiness_0_1 readiness {};
if (armed.armed) {
readiness.value = reg_drone_service_common_Readiness_0_1_ENGAGED;
readiness.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
} else {
readiness.value = reg_drone_service_common_Readiness_0_1_STANDBY;
readiness.value = reg_udral_service_common_Readiness_0_1_STANDBY;
}
uint8_t readiness_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
uint8_t readiness_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = _port_id, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _transfer_id,
.payload_size = reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &readiness_payload_buffer,
};
int32_t result = reg_drone_service_common_Readiness_0_1_serialize_(&readiness, readiness_payload_buffer,
&transfer.payload_size);
int32_t result = reg_udral_service_common_Readiness_0_1_serialize_(&readiness, readiness_payload_buffer,
&payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&readiness_payload_buffer);
}
}
};

35
src/drivers/uavcan_v1/ServiceClients/Access.hpp → src/drivers/cyphal/ServiceClients/Access.hpp
View File

@ -55,25 +55,26 @@
class UavcanAccessResponse : public UavcanBaseSubscriber
{
public:
UavcanAccessResponse(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanBaseSubscriber(ins, "", "Access", 0), _param_manager(pmgr) { };
UavcanAccessResponse(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanBaseSubscriber(handle, "", "Access", 0), _param_manager(pmgr) { };
void subscribe() override
{
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
canardRxSubscribe(&_canard_instance,
CanardTransferKindRequest,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindRequest,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
PX4_INFO("Access request");
size_t payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
uavcan_register_Access_Request_1_0 msg;
uavcan_register_Access_Request_1_0_initialize_(&msg);
@ -102,22 +103,22 @@ public:
uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.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 = receive.transfer_id,
.payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
.remote_node_id = receive.metadata.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = receive.metadata.transfer_id,
};
result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size);
result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
result = canardTxPush(&_canard_instance, &transfer);
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&response_payload_buffer);
}
//return result;

35
src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp → src/drivers/cyphal/ServiceClients/GetInfo.hpp
View File

@ -54,28 +54,29 @@
class UavcanGetInfoResponse : public UavcanBaseSubscriber
{
public:
UavcanGetInfoResponse(CanardInstance &ins) :
UavcanBaseSubscriber(ins, "", "GetInfo", 0) { };
UavcanGetInfoResponse(CanardHandle &handle) :
UavcanBaseSubscriber(handle, "", "GetInfo", 0) { };
void subscribe() override
{
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
canardRxSubscribe(&_canard_instance,
CanardTransferKindRequest,
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(
CanardTransferKindRequest,
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
PX4_INFO("GetInfo request");
// Setup node.GetInfo response
uavcan_node_GetInfo_Response_1_0 node_info;
size_t payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
uavcan_node_GetInfo_Response_1_0_initialize_(&node_info);
@ -106,23 +107,23 @@ public:
uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer response = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.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 = receive.transfer_id,
.payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
.remote_node_id = receive.metadata.remote_node_id, // Send back to request Node
.transfer_id = receive.metadata.transfer_id
};
int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(&node_info, (uint8_t *)&response_payload_buffer,
&response.payload_size);
&payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
result = canardTxPush(&_canard_instance, &response);
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&response_payload_buffer);
}
//TODO proper error handling

36
src/drivers/uavcan_v1/ServiceClients/List.hpp → src/drivers/cyphal/ServiceClients/List.hpp
View File

@ -54,25 +54,27 @@
class UavcanListResponse : public UavcanBaseSubscriber
{
public:
UavcanListResponse(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanBaseSubscriber(ins, "", "List", 0), _param_manager(pmgr) { };
UavcanListResponse(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanBaseSubscriber(handle, "", "List", 0), _param_manager(pmgr) { };
void subscribe() override
{
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
canardRxSubscribe(&_canard_instance,
CanardTransferKindRequest,
uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindRequest,
uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
PX4_INFO("List request");
size_t payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
uavcan_register_List_Request_1_0 msg;
uavcan_register_List_Response_1_0 response;
@ -90,22 +92,22 @@ public:
uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindResponse,
.port_id = uavcan_register_List_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 = receive.transfer_id,
.payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
.remote_node_id = receive.metadata.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = receive.metadata.transfer_id
};
result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size);
result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
result = canardTxPush(&_canard_instance, &transfer);
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&response_payload_buffer);
}
};

20
src/drivers/uavcan_v1/Services/AccessRequest.hpp → src/drivers/cyphal/Services/AccessRequest.hpp
View File

@ -54,8 +54,8 @@
class UavcanAccessServiceRequest : public UavcanServiceRequest
{
public:
UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanServiceRequest(ins, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_,
UavcanAccessServiceRequest(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanServiceRequest(handle, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_), _param_manager(pmgr) { };
bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler)
@ -70,26 +70,28 @@ public:
name.name.elements[7] = 's'; //HACK Change pub into sub
if (_param_manager.GetParamByName(name, request_msg.value)) {
size_t payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
name.name.elements[7] = 'p'; //HACK Change sub into pub
memcpy(&request_msg.name, &name, sizeof(request_msg.name));
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = _portID, // This is the subject-ID.
.remote_node_id = node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
.transfer_id = request_transfer_id
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &payload_size);
if (result == 0) {
return request(&transfer, handler);
return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&request_payload_buffer,
handler);
} else {
return false;

20
src/drivers/uavcan_v1/Services/ListRequest.hpp → src/drivers/cyphal/Services/ListRequest.hpp
View File

@ -54,31 +54,33 @@
class UavcanListServiceRequest : public UavcanServiceRequest
{
public:
UavcanListServiceRequest(CanardInstance &ins) :
UavcanServiceRequest(ins, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
UavcanListServiceRequest(CanardHandle &handle) :
UavcanServiceRequest(handle, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_) { };
bool getIndex(CanardNodeID node_id, uint16_t index, UavcanServiceRequestInterface *handler)
{
uavcan_register_List_Request_1_0 msg;
size_t payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
msg.index = index;
uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = node_id,
.transfer_id = request_transfer_id,
.payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
.transfer_id = request_transfer_id
};
if (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &transfer.payload_size) == 0) {
return request(&transfer, handler);
if (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &payload_size) == 0) {
return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&request_payload_buffer,
handler);
} else {
return false;

36
src/drivers/uavcan_v1/Services/ServiceRequest.hpp → src/drivers/cyphal/Services/ServiceRequest.hpp
View File

@ -53,43 +53,49 @@
class UavcanServiceRequestInterface
{
public:
virtual void response_callback(const CanardTransfer &receive) = 0;
virtual void response_callback(const CanardRxTransfer &receive) = 0;
};
class UavcanServiceRequest : public UavcanBaseSubscriber
{
public:
UavcanServiceRequest(CanardInstance &ins, const char *prefix_name, const char *subject_name, CanardPortID portID,
UavcanServiceRequest(CanardHandle &handle, const char *prefix_name, const char *subject_name, CanardPortID portID,
size_t extent) :
UavcanBaseSubscriber(ins, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { };
UavcanBaseSubscriber(handle, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { };
void subscribe() override
{
// Subscribe to requests response
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
_portID,
_extent,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindResponse,
_portID,
_extent,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
};
bool request(CanardTransfer *transfer, UavcanServiceRequestInterface *handler)
bool request(const CanardMicrosecond tx_deadline_usec,
const CanardTransferMetadata *transfer_metadata,
const size_t payload_size,
const void *const payload,
UavcanServiceRequestInterface *handler)
{
_response_callback = handler;
remote_node_id = transfer->remote_node_id;
remote_node_id = transfer_metadata->remote_node_id;
++request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
return canardTxPush(&_canard_instance, transfer) > 0;
return _canard_handle.TxPush(tx_deadline_usec,
transfer_metadata,
payload_size,
payload) > 0;
}
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
PX4_INFO("Response");
if (_response_callback != nullptr &&
receive.transfer_id == (request_transfer_id - 1) &&
receive.remote_node_id == remote_node_id) {
receive.metadata.transfer_id == (request_transfer_id - 1) &&
receive.metadata.remote_node_id == remote_node_id) {
_response_callback->response_callback(receive);
}
};

13
src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp → src/drivers/cyphal/Subscribers/BaseSubscriber.hpp
View File

@ -34,7 +34,7 @@
/**
* @file BaseSubscriber.hpp
*
* Defines basic functionality of UAVCAN v1 subscriber class
* Defines basic functionality of Cyphal subscriber class
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
@ -45,14 +45,15 @@
#include <lib/parameters/param.h>
#include "../CanardHandle.hpp"
#include "../CanardInterface.hpp"
#include "../ParamManager.hpp"
class UavcanBaseSubscriber
{
public:
UavcanBaseSubscriber(CanardInstance &ins, const char *prefix_name, const char *subject_name, uint8_t instance = 0) :
_canard_instance(ins), _prefix_name(prefix_name), _instance(instance)
UavcanBaseSubscriber(CanardHandle &handle, const char *prefix_name, const char *subject_name, uint8_t instance = 0) :
_canard_handle(handle), _prefix_name(prefix_name), _instance(instance)
{
_subj_sub._subject_name = subject_name;
_subj_sub._canard_sub.user_reference = this;
@ -72,12 +73,12 @@ public:
SubjectSubscription *curSubj = &_subj_sub;
while (curSubj != nullptr) {
canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub.port_id);
_canard_handle.RxUnsubscribe(CanardTransferKindMessage, curSubj->_canard_sub.port_id);
curSubj = curSubj->next;
}
};
virtual void callback(const CanardTransfer &msg) = 0;
virtual void callback(const CanardRxTransfer &msg) = 0;
CanardPortID id(uint32_t instance = 0)
{
@ -145,7 +146,7 @@ protected:
struct SubjectSubscription *next {nullptr};
};
CanardInstance &_canard_instance;
CanardHandle &_canard_handle;
const char *_prefix_name;
SubjectSubscription _subj_sub;
uint8_t _instance {0};

6
src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp → src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp
View File

@ -34,7 +34,7 @@
/**
* @file DynamicPortSubscriber.hpp
*
* Defines basic functionality of UAVCAN v1 subscriber class with Non-fixed unregulated port identifier
* Defines basic functionality of Cyphal subscriber class with Non-fixed unregulated port identifier
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
@ -54,9 +54,9 @@
class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
{
public:
UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name,
UavcanDynamicPortSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name,
const char *subject_name, uint8_t instance = 0) :
UavcanBaseSubscriber(ins, prefix_name, subject_name, instance), _param_manager(pmgr) { };
UavcanBaseSubscriber(handle, prefix_name, subject_name, instance), _param_manager(pmgr) { };
void updateParam()
{

17
src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp → src/drivers/cyphal/Subscribers/Heartbeat.hpp
View File

@ -50,22 +50,21 @@
class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
{
public:
UavcanHeartbeatSubscriber(CanardInstance &ins) :
UavcanBaseSubscriber(ins, "", "Heartbeat", 0) { };
UavcanHeartbeatSubscriber(CanardHandle &handle) :
UavcanBaseSubscriber(handle, "", "Heartbeat", 0) { };
void subscribe() override
{
// Subscribe to heartbeat messages
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, // The fixed Subject-ID
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, // The fixed Subject-ID
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
//TODO heartbeat management
};

17
src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp → src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp
View File

@ -52,21 +52,20 @@
class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanLegacyBatteryInfoSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "legacy.", "legacy_bms", instance) { };
UavcanLegacyBatteryInfoSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(handle, pmgr, "legacy.", "legacy_bms", instance) { };
void subscribe() override
{
// Subscribe to messages reg.drone.service.battery.Status.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
PX4_INFO("Legacy BmsCallback");

0
src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp → src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp
View File

17
src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp → src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp
View File

@ -52,9 +52,9 @@ template <class T>
class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber
{
public:
uORB_over_UAVCAN_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
uORB_over_UAVCAN_Subscriber(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta,
uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "uorb.", meta->o_name, instance),
UavcanDynamicPortSubscriber(handle, pmgr, "uorb.", meta->o_name, instance),
_uorb_meta{meta},
_uorb_pub(meta)
{};
@ -66,15 +66,14 @@ public:
T *data = NULL;
// Subscribe to messages uORB sensor_gps payload over UAVCAN
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
get_payload_size(data),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
get_payload_size(data),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
T *data = (T *)receive.payload;

67
src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp → src/drivers/cyphal/Subscribers/udral/Battery.hpp
View File

@ -34,7 +34,7 @@
/**
* @file Battery.hpp
*
* Defines basic functionality of UAVCAN v1 Battery subscription
* Defines basic functionality of Cyphal Battery subscription
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
@ -44,10 +44,10 @@
#include <uORB/topics/battery_status.h>
#include <uORB/PublicationMulti.hpp>
// DS-15 Specification Messages
#include <reg/drone/physics/electricity/SourceTs_0_1.h>
#include <reg/drone/service/battery/Parameters_0_3.h>
#include <reg/drone/service/battery/Status_0_2.h>
// UDRAL Specification Messages
#include <reg/udral/physics/electricity/SourceTs_0_1.h>
#include <reg/udral/service/battery/Parameters_0_3.h>
#include <reg/udral/service/battery/Status_0_2.h>
#include "../DynamicPortSubscriber.hpp"
@ -57,8 +57,8 @@
class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "energy_source", instance)
UavcanBmsSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "energy_source", instance)
{
_subj_sub.next = &_status_sub;
@ -74,36 +74,33 @@ public:
void subscribe() override
{
// Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
reg_drone_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
reg_udral_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
// Subscribe to messages reg.drone.service.battery.Status.0.2
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_status_sub._canard_sub.port_id,
reg_drone_service_battery_Status_0_2_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_status_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
_status_sub._canard_sub.port_id,
reg_udral_service_battery_Status_0_2_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_status_sub._canard_sub);
// Subscribe to messages reg.drone.service.battery.Parameters.0.3
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_parameters_sub._canard_sub.port_id,
reg_drone_service_battery_Parameters_0_3_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_parameters_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
_parameters_sub._canard_sub.port_id,
reg_udral_service_battery_Parameters_0_3_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_parameters_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
if (receive.port_id == _subj_sub._canard_sub.port_id) {
reg_drone_physics_electricity_SourceTs_0_1 source_ts {};
if (receive.metadata.port_id == _subj_sub._canard_sub.port_id) {
reg_udral_physics_electricity_SourceTs_0_1 source_ts {};
size_t source_ts_size_in_bytes = receive.payload_size;
reg_drone_physics_electricity_SourceTs_0_1_deserialize_(&source_ts,
reg_udral_physics_electricity_SourceTs_0_1_deserialize_(&source_ts,
(const uint8_t *)receive.payload,
&source_ts_size_in_bytes);
bat_status.timestamp = hrt_absolute_time(); //FIXME timesyncronization source_ts.timestamp.microsecond;
@ -123,10 +120,10 @@ public:
// TODO uORB publication rate limiting
_battery_status_pub.publish(bat_status);
} else if (receive.port_id == _status_sub._canard_sub.port_id) {
reg_drone_service_battery_Status_0_2 bat {};
} else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) {
reg_udral_service_battery_Status_0_2 bat {};
size_t bat_size_in_bytes = receive.payload_size;
reg_drone_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
reg_udral_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
bat_status.scale = -1; // What does the mean?
@ -157,10 +154,10 @@ public:
bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time?
} else if (receive.port_id == _parameters_sub._canard_sub.port_id) {
reg_drone_service_battery_Parameters_0_3 parameters {};
} else if (receive.metadata.port_id == _parameters_sub._canard_sub.port_id) {
reg_udral_service_battery_Parameters_0_3 parameters {};
size_t parameters_size_in_bytes = receive.payload_size;
reg_drone_service_battery_Parameters_0_3_deserialize_(&parameters,
reg_udral_service_battery_Parameters_0_3_deserialize_(&parameters,
(const uint8_t *)receive.payload,
&parameters_size_in_bytes);

40
src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp → src/drivers/cyphal/Subscribers/udral/Esc.hpp
View File

@ -34,7 +34,7 @@
/**
* @file Esc.hpp
*
* Defines basic functionality of UAVCAN v1 ESC setpoint subscription
* Defines basic functionality of Cyphal ESC setpoint subscription
* (For use on a CAN-->PWM node)
*
* @author Jacob Crabill <jacob@flyvoly.com>
@ -45,47 +45,45 @@
/// For use with PR-16808 once merged
// #include <uORB/topics/output_control.h>
// DS-15 Specification Messages
#include <reg/drone/service/actuator/common/sp/Vector8_0_1.h>
#include <reg/drone/service/common/Readiness_0_1.h>
// UDRAL Specification Messages
#include <reg/udral/service/actuator/common/sp/Vector8_0_1.h>
#include <reg/udral/service/common/Readiness_0_1.h>
#include "../DynamicPortSubscriber.hpp"
class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanEscSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "esc", instance) { };
UavcanEscSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "esc", instance) { };
void subscribe() override
{
// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
reg_udral_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
// Subscribe to messages reg.drone.service.common.Readiness.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub.port_id) + 1),
reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_canard_sub_readiness);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub.port_id) + 1),
reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_canard_sub_readiness);
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
// Test with Yakut:
// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
// yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: [1000, 2000, 3000, 4000, 0, 0, 0, 0]}'
PX4_INFO("EscCallback");
reg_drone_service_actuator_common_sp_Vector8_0_1 esc {};
reg_udral_service_actuator_common_sp_Vector8_0_1 esc {};
size_t esc_size_in_bits = receive.payload_size;
reg_drone_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload,
reg_udral_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload,
&esc_size_in_bits);
double val1 = static_cast<double>(esc.value[0]);

27
src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp → src/drivers/cyphal/Subscribers/udral/Gnss.hpp
View File

@ -34,33 +34,32 @@
/**
* @file Gnss.hpp
*
* Defines basic functionality of UAVCAN v1 GNSS subscription
* Defines basic functionality of Cyphal GNSS subscription
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
// UDRAL Specification Messages
#include <reg/udral/physics/kinematics/geodetic/Point_0_1.h>
#include "../DynamicPortSubscriber.hpp"
class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanGnssSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "gps", instance) { };
UavcanGnssSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "gps", instance) { };
void subscribe() override
{
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
_canard_handle.RxSubscribe(CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
reg_udral_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
/** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan):
* # A compliant implementation of this service should publish the following subjects:
@ -78,16 +77,16 @@ public:
*/
};
void callback(const CanardTransfer &receive) override
void callback(const CanardRxTransfer &receive) override
{
// Test with Yakut:
// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
// yakut pub 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}'
PX4_INFO("GpsCallback");
reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
reg_udral_physics_kinematics_geodetic_Point_0_1 geo {};
size_t geo_size_in_bits = receive.payload_size;
reg_drone_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits);
reg_udral_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits);
double lat = geo.latitude;
double lon = geo.longitude;

4
src/drivers/uavcan_v1/SubscriptionManager.cpp → src/drivers/cyphal/SubscriptionManager.cpp
View File

@ -59,7 +59,7 @@ void SubscriptionManager::subscribe()
{
_heartbeat_sub.subscribe();
#if CONFIG_UAVCAN_V1_GETINFO_RESPONDER
#if CONFIG_CYPHAL_GETINFO_RESPONDER
_getinfo_rsp.subscribe();
#endif
@ -101,7 +101,7 @@ void SubscriptionManager::updateDynamicSubscriptions()
uint16_t port_id = value.natural16.value.elements[0];
if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber
dynsub = sub.create_sub(_canard_instance, _param_manager);
dynsub = sub.create_sub(_canard_handle, _param_manager);
if (dynsub == nullptr) {
PX4_ERR("Out of memory");

86
src/drivers/uavcan_v1/SubscriptionManager.hpp → src/drivers/cyphal/SubscriptionManager.hpp
View File

@ -41,33 +41,33 @@
#pragma once
#ifndef CONFIG_UAVCAN_V1_ESC_SUBSCRIBER
#define CONFIG_UAVCAN_V1_ESC_SUBSCRIBER 0
#ifndef CONFIG_CYPHAL_ESC_SUBSCRIBER
#define CONFIG_CYPHAL_ESC_SUBSCRIBER 0
#endif
#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0
#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 0
#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0
#endif
#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1
#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 0
#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_1
#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 0
#endif
#ifndef CONFIG_UAVCAN_V1_BMS_SUBSCRIBER
#define CONFIG_UAVCAN_V1_BMS_SUBSCRIBER 0
#ifndef CONFIG_CYPHAL_BMS_SUBSCRIBER
#define CONFIG_CYPHAL_BMS_SUBSCRIBER 0
#endif
#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER
#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER 0
#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
#define CONFIG_CYPHAL_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
#define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
CONFIG_CYPHAL_BMS_SUBSCRIBER + \
CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
@ -79,14 +79,14 @@
#include "ServiceClients/List.hpp"
#include "Subscribers/BaseSubscriber.hpp"
#include "Subscribers/Heartbeat.hpp"
#include "Subscribers/DS-015/Battery.hpp"
#include "Subscribers/DS-015/Esc.hpp"
#include "Subscribers/DS-015/Gnss.hpp"
#include "Subscribers/udral/Battery.hpp"
#include "Subscribers/udral/Esc.hpp"
#include "Subscribers/udral/Gnss.hpp"
#include "Subscribers/legacy/LegacyBatteryInfo.hpp"
#include "Subscribers/uORB/uorb_subscriber.hpp"
typedef struct {
UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
UavcanDynamicPortSubscriber *(*create_sub)(CanardHandle &handle, UavcanParamManager &pmgr) {};
const char *subject_name;
const uint8_t instance;
} UavcanDynSubBinder;
@ -94,7 +94,7 @@ typedef struct {
class SubscriptionManager
{
public:
SubscriptionManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {}
SubscriptionManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {}
~SubscriptionManager();
void subscribe();
@ -104,57 +104,57 @@ public:
private:
void updateDynamicSubscriptions();
CanardInstance &_canard_instance;
CanardHandle &_canard_handle;
UavcanParamManager &_param_manager;
UavcanDynamicPortSubscriber *_dynsubscribers {nullptr};
UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance};
UavcanHeartbeatSubscriber _heartbeat_sub {_canard_handle};
#if CONFIG_UAVCAN_V1_GETINFO_RESPONDER
#if CONFIG_CYPHAL_GETINFO_RESPONDER
// GetInfo response
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
UavcanGetInfoResponse _getinfo_rsp {_canard_handle};
#endif
// Process register requests
UavcanAccessResponse _access_rsp {_canard_instance, _param_manager};
UavcanListResponse _list_rsp {_canard_instance, _param_manager};
UavcanAccessResponse _access_rsp {_canard_handle, _param_manager};
UavcanListResponse _list_rsp {_canard_handle, _param_manager};
const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] {
#if CONFIG_UAVCAN_V1_ESC_SUBSCRIBER
#if CONFIG_CYPHAL_ESC_SUBSCRIBER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanEscSubscriber(ins, pmgr, 0);
return new UavcanEscSubscriber(handle, pmgr, 0);
},
"esc",
0
},
#endif
#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0
#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanGnssSubscriber(ins, pmgr, 0);
return new UavcanGnssSubscriber(handle, pmgr, 0);
},
"gps",
0
},
#endif
#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 //FIXME decouple instanceing
#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 //FIXME decouple handletanceing
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanGnssSubscriber(ins, pmgr, 1);
return new UavcanGnssSubscriber(handle, pmgr, 1);
},
"gps",
1
},
#endif
#if CONFIG_UAVCAN_V1_BMS_SUBSCRIBER
#if CONFIG_CYPHAL_BMS_SUBSCRIBER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanBmsSubscriber(ins, pmgr, 0);
return new UavcanBmsSubscriber(handle, pmgr, 0);
},
"energy_source",
0
@ -162,19 +162,19 @@ private:
#endif
#if 0 //Obsolete to be removed
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0);
return new UavcanLegacyBatteryInfoSubscriber(handle, pmgr, 0);
},
"legacy_bms",
0
},
#endif
#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER
#if CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new uORB_over_UAVCAN_Subscriber<sensor_gps_s>(ins, pmgr, ORB_ID(sensor_gps));
return new uORB_over_UAVCAN_Subscriber<sensor_gps_s>(handle, pmgr, ORB_ID(sensor_gps));
},
"uorb.sensor_gps",
0

0
src/drivers/uavcan_v1/legacy_data_types → src/drivers/cyphal/legacy_data_types
View File

1
src/drivers/cyphal/libcanard Submodule

@ -0,0 +1 @@
Subproject commit db87ea32aa092c48ea103963138b6346dd3e9008

1
src/drivers/uavcan_v1/module.yaml → src/drivers/cyphal/module.yaml
View File

@ -8,4 +8,3 @@ actuator_output:
max: { min: 0, max: 8191, default: 8191 }
failsafe: { min: 0, max: 8191 }
num_channels: 16

0
src/drivers/uavcan_v1/o1heap/o1heap.c → src/drivers/cyphal/o1heap/o1heap.c
View File

0
src/drivers/uavcan_v1/o1heap/o1heap.h → src/drivers/cyphal/o1heap/o1heap.h
View File

66
src/drivers/uavcan_v1/parameters.c → src/drivers/cyphal/parameters.c
View File

@ -32,28 +32,28 @@
****************************************************************************/
/**
* UAVCAN v1
* Cyphal
*
* 0 - UAVCAN disabled.
* 1 - Enables UAVCANv1
* 0 - Cyphal disabled.
* 1 - Enables Cyphal
*
* @boolean
* @reboot_required true
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UAVCAN_V1_ENABLE, 0);
PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0);
/**
* UAVCAN v1 Node ID.
* Cyphal Node ID.
*
* Read the specs at http://uavcan.org to learn more about Node ID.
*
* @min -1
* @max 125
* @reboot_required true
* @group UAVCANv1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1);
PARAM_DEFINE_INT32(CYPHAL_ID, 1);
/**
* UAVCAN/CAN v1 bus bitrate.
@ -62,9 +62,9 @@ PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1);
* @min 20000
* @max 1000000
* @reboot_required true
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000);
PARAM_DEFINE_INT32(CYPHAL_BAUD, 1000000);
/* Subscription port ID, -1 will be treated as unset */
@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000);
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_ESC0_SUB, -1);
@ -82,7 +82,7 @@ PARAM_DEFINE_INT32(UCAN1_ESC0_SUB, -1);
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_GPS0_SUB, -1);
@ -91,100 +91,100 @@ PARAM_DEFINE_INT32(UCAN1_GPS0_SUB, -1);
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_GPS1_SUB, -1);
/**
* DS-015 battery energy source subscription port ID.
* UDRAL battery energy source subscription port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_BMS_ES_SUB, -1);
/**
* DS-015 battery status subscription port ID.
* UDRAL battery status subscription port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_BMS_BS_SUB, -1);
/**
* DS-015 battery parameters subscription port ID.
* UDRAL battery parameters subscription port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_BMS_BP_SUB, -1);
/**
* UAVCAN v1 leagcy battery port ID.
* Cyphal leagcy battery port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1);
/**
* sensor_gps uORB over UAVCAN v1 subscription port ID.
* sensor_gps uORB over Cyphal subscription port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
/**
* sensor_gps uORB over UAVCAN v1 publication port ID.
* sensor_gps uORB over Cyphal publication port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
// Publication Port IDs
/**
* UAVCAN v1 ESC publication port ID.
* Cyphal ESC publication port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
/**
* UAVCAN v1 GPS publication port ID.
* Cyphal GPS publication port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1);
/**
* UAVCAN v1 Servo publication port ID.
* Cyphal Servo publication port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1);
/**
* actuator_outputs uORB over UAVCAN v1 publication port ID.
* actuator_outputs uORB over Cyphal publication port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_ACTR_PUB, -1);

1
src/drivers/cyphal/public_regulated_data_types Submodule

@ -0,0 +1 @@
Subproject commit d0bd6516dac8ff61287fe49a9f2c75e7d4dc1b8e

1
src/drivers/uavcan_v1/libcanard

@ -1 +0,0 @@
Subproject commit 2d449453fc8c4060f276c6dc585d4e1e5bf4fd52

1
src/drivers/uavcan_v1/public_regulated_data_types

@ -1 +0,0 @@
Subproject commit 0a773b93ce5c94e1d2791b180058cb9897fab7e1