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

UAVCANv1 Node implementation work, PNP, Registers and uORB publisher

This commit is contained in:
Peter van der Perk 2021-11-30 15:50:51 +01:00 committed by Daniel Agar
parent 79aa8ee587
commit ce909b23b1
31 changed files with 303 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,2 +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

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

@ -7,7 +7,10 @@ CONFIG_DRIVERS_BOOTLOADERS=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=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_MODULES_CONTROL_ALLOCATOR=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y

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

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

2
src/drivers/uavcan_v1/Actuators/EscClient.hpp
View File

@ -68,7 +68,7 @@ public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanPublisher(ins, pmgr, "esc") { };
UavcanPublisher(ins, pmgr, "ds_015", "esc") { };
~UavcanEscController() {};

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

@ -122,6 +122,7 @@ px4_add_module(
Uavcan.cpp
Uavcan.hpp
Publishers/uORB/uorb_publisher.cpp
Subscribers/uORB/uorb_subscriber.cpp
${SRCS}
o1heap/o1heap.c
o1heap/o1heap.h

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

@ -46,7 +46,7 @@
void NodeClient::callback(const CanardTransfer &receive)
{
if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id != CANARD_NODE_ID_UNSET) {
if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id == CANARD_NODE_ID_UNSET) {
int32_t allocated = CANARD_NODE_ID_UNSET;
px4_guid_t px4_guid;
@ -88,6 +88,9 @@ void NodeClient::callback(const CanardTransfer &receive)
}
_canard_instance.node_id = allocated;
PX4_INFO("Allocated Node ID %d", _canard_instance.node_id);
}
}

2
src/drivers/uavcan_v1/NodeManager.hpp
View File

@ -70,7 +70,7 @@ typedef struct {
class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface
{
public:
NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "", "NodeIDAllocationData", 0),
_canard_instance(ins), _access_request(ins, pmgr), _list_request(ins) { };
void subscribe() override

13
src/drivers/uavcan_v1/ParamManager.cpp
View File

@ -76,6 +76,19 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
return false;
}
bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name)
{
if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
return false;
}
strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
name.name.count = strlen(_uavcan_params[id].uavcan_name);
return true;
}
bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value)
{
for (auto &param : _uavcan_params) {

4
src/drivers/uavcan_v1/ParamManager.hpp
View File

@ -110,12 +110,13 @@ public:
bool GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value);
bool GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value);
bool GetParamName(uint32_t id, uavcan_register_Name_1_0 &name);
bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value);
private:
const UavcanParamBinder _uavcan_params[12] {
const UavcanParamBinder _uavcan_params[13] {
{"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@ -128,6 +129,7 @@ private:
{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS_P", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
//{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing
//{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"},
};

2
src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp
View File

@ -51,7 +51,7 @@ class UavcanGnssPublisher : public UavcanPublisher
{
public:
UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanPublisher(ins, pmgr, "gps", instance)
UavcanPublisher(ins, pmgr, "ds_015", "gps", instance)
{
};

2
src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp
View File

@ -51,7 +51,7 @@ class UavcanReadinessPublisher : public UavcanPublisher
{
public:
UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanPublisher(ins, pmgr, "readiness", instance)
UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance)
{
};

22
src/drivers/uavcan_v1/Publishers/Publisher.hpp
View File

@ -60,8 +60,10 @@
class UavcanPublisher : public ListNode<UavcanPublisher *>
{
public:
UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, uint8_t instance = 0) :
_canard_instance(ins), _param_manager(pmgr), _subject_name(subject_name), _instance(instance) { };
UavcanPublisher(CanardInstance &ins, 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),
_instance(instance) { };
virtual ~UavcanPublisher() = default;
@ -73,7 +75,7 @@ public:
void updateParam()
{
char uavcan_param[256];
snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s.%d.id", _subject_name, _instance);
snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s%s.%d.id", _prefix_name, _subject_name, _instance);
// Set _port_id from _uavcan_param
uavcan_register_Value_1_0 value;
@ -83,12 +85,12 @@ public:
if (_port_id != new_id) {
if (new_id == CANARD_PORT_ID_UNSET) {
PX4_INFO("Disabling publication of subject %s.%d", _subject_name, _instance);
PX4_INFO("Disabling publication of subject %s%s.%d", _prefix_name, _subject_name, _instance);
_port_id = CANARD_PORT_ID_UNSET;
} else {
_port_id = (CanardPortID)new_id;
PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id);
PX4_INFO("Enabling subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id);
}
}
}
@ -97,10 +99,10 @@ public:
void printInfo()
{
if (_port_id != CANARD_PORT_ID_UNSET) {
PX4_INFO("Enabled subject %s.%d on port %d", _subject_name, _instance, _port_id);
PX4_INFO("Enabled subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id);
} else {
PX4_INFO("Subject %s.%d disabled", _subject_name, _instance);
PX4_INFO("Subject %s%s.%d disabled", _prefix_name, _subject_name, _instance);
}
}
@ -109,6 +111,11 @@ public:
return _subject_name;
}
const char *getPrefixName()
{
return _prefix_name;
}
uint8_t getInstance()
{
return _instance;
@ -127,6 +134,7 @@ public:
protected:
CanardInstance &_canard_instance;
UavcanParamManager &_param_manager;
const char *_prefix_name;
const char *_subject_name;
uint8_t _instance {0};

2
src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp
View File

@ -52,7 +52,7 @@ class uORB_over_UAVCAN_Publisher : public UavcanPublisher
public:
uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
uint8_t instance = 0) :
UavcanPublisher(ins, pmgr, meta->o_name, instance),
UavcanPublisher(ins, pmgr, "uorb.", meta->o_name, instance),
_uorb_meta{meta},
_uorb_sub(meta)
{};

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

@ -56,7 +56,7 @@ class UavcanAccessResponse : public UavcanBaseSubscriber
{
public:
UavcanAccessResponse(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanBaseSubscriber(ins, "Access", 0), _param_manager(pmgr) { };
UavcanBaseSubscriber(ins, "", "Access", 0), _param_manager(pmgr) { };
void subscribe() override
{

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

@ -55,7 +55,7 @@ class UavcanGetInfoResponse : public UavcanBaseSubscriber
{
public:
UavcanGetInfoResponse(CanardInstance &ins) :
UavcanBaseSubscriber(ins, "GetInfo", 0) { };
UavcanBaseSubscriber(ins, "", "GetInfo", 0) { };
void subscribe() override
{

116
src/drivers/uavcan_v1/ServiceClients/List.hpp Normal file
View File

@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file List.hpp
*
* Defines response to a List request
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <version/version.h>
#include "../ParamManager.hpp"
#include <uavcan/_register/List_1_0.h>
#include "../Subscribers/BaseSubscriber.hpp"
class UavcanListResponse : public UavcanBaseSubscriber
{
public:
UavcanListResponse(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanBaseSubscriber(ins, "", "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);
};
void callback(const CanardTransfer &receive) override
{
PX4_INFO("List request");
uavcan_register_List_Request_1_0 msg;
uavcan_register_List_Response_1_0 response;
uavcan_register_List_Request_1_0_initialize_(&msg);
uavcan_register_List_Response_1_0_initialize_(&response);
size_t register_in_size_bits = receive.payload_size;
uavcan_register_List_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
int result {0};
if (_param_manager.GetParamName(msg.index, response.name) == 0) {
response.name.name.count = 0;
}
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,
.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,
};
result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
result = canardTxPush(&_canard_instance, &transfer);
}
};
private:
UavcanParamManager &_param_manager;
};

2
src/drivers/uavcan_v1/Services/AccessRequest.hpp
View File

@ -55,7 +55,7 @@ class UavcanAccessServiceRequest : public UavcanServiceRequest
{
public:
UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
UavcanServiceRequest(ins, "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_,
UavcanServiceRequest(ins, "", "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)

2
src/drivers/uavcan_v1/Services/ListRequest.hpp
View File

@ -55,7 +55,7 @@ class UavcanListServiceRequest : public UavcanServiceRequest
{
public:
UavcanListServiceRequest(CanardInstance &ins) :
UavcanServiceRequest(ins, "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
UavcanServiceRequest(ins, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_) { };

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

@ -59,8 +59,9 @@ public:
class UavcanServiceRequest : public UavcanBaseSubscriber
{
public:
UavcanServiceRequest(CanardInstance &ins, const char *subject_name, CanardPortID portID, size_t extent) :
UavcanBaseSubscriber(ins, subject_name, 0), _portID(portID), _extent(extent) { };
UavcanServiceRequest(CanardInstance &ins, const char *prefix_name, const char *subject_name, CanardPortID portID,
size_t extent) :
UavcanBaseSubscriber(ins, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { };
void subscribe() override

5
src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
View File

@ -51,8 +51,8 @@
class UavcanBaseSubscriber
{
public:
UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) :
_canard_instance(ins), _instance(instance)
UavcanBaseSubscriber(CanardInstance &ins, const char *prefix_name, const char *subject_name, uint8_t instance = 0) :
_canard_instance(ins), _prefix_name(prefix_name), _instance(instance)
{
_subj_sub._subject_name = subject_name;
_subj_sub._canard_sub.user_reference = this;
@ -146,6 +146,7 @@ protected:
};
CanardInstance &_canard_instance;
const char *_prefix_name;
SubjectSubscription _subj_sub;
uint8_t _instance {0};
/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)

2
src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp
View File

@ -58,7 +58,7 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "energy_source", instance)
UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "energy_source", instance)
{
_subj_sub.next = &_status_sub;

2
src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp
View File

@ -55,7 +55,7 @@ class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanEscSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "esc", instance) { };
UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "esc", instance) { };
void subscribe() override
{

2
src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp
View File

@ -50,7 +50,7 @@ class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanGnssSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "gps", instance) { };
UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "gps", instance) { };
void subscribe() override
{

11
src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp
View File

@ -54,9 +54,9 @@
class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
{
public:
UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name,
uint8_t instance = 0) :
UavcanBaseSubscriber(ins, subject_name, instance), _param_manager(pmgr) { };
UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name,
const char *subject_name, uint8_t instance = 0) :
UavcanBaseSubscriber(ins, prefix_name, subject_name, instance), _param_manager(pmgr) { };
void updateParam()
{
@ -64,7 +64,7 @@ public:
while (curSubj != nullptr) {
char uavcan_param[90];
snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance);
snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s%s.%d.id", _prefix_name, curSubj->_subject_name, _instance);
// Set _port_id from _uavcan_param
uavcan_register_Value_1_0 value;
@ -86,7 +86,8 @@ public:
// Subscribe on the new port ID
curSubj->_canard_sub.port_id = (CanardPortID)new_id;
PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance,
curSubj->_canard_sub.port_id);
subscribe();
}
}

2
src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp
View File

@ -51,7 +51,7 @@ class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
{
public:
UavcanHeartbeatSubscriber(CanardInstance &ins) :
UavcanBaseSubscriber(ins, "Heartbeat", 0) { };
UavcanBaseSubscriber(ins, "", "Heartbeat", 0) { };
void subscribe() override
{

2
src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp
View File

@ -53,7 +53,7 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
{
public:
UavcanLegacyBatteryInfoSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "legacy_bms", instance) { };
UavcanDynamicPortSubscriber(ins, pmgr, "legacy.", "legacy_bms", instance) { };
void subscribe() override
{

54
src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp Normal file
View File

@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file uorb_template.cpp
*
* Defines generic, templatized uORB over UAVCANv1 subscriber
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#include "uorb_subscriber.hpp"
/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */
template<>
void uORB_over_UAVCAN_Subscriber<sensor_gps_s>::convert(sensor_gps_s *data)
{
/* HOTFIX as long as we don't have UAVCAN timesyncronization we update the timestamp on arrival */
data->timestamp = hrt_absolute_time();
}

68
src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp → src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp
View File

@ -32,56 +32,84 @@
****************************************************************************/
/**
* @file sensor_gps.hpp
* @file uorb_template.hpp
*
* Defines uORB over UAVCANv1 sensor_gps subscriber
* Defines generic, templatized uORB over UAVCANv1 publisher
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <uORB/topics/sensor_gps.h>
#include <uORB/PublicationMulti.hpp>
#include "../DynamicPortSubscriber.hpp"
class UORB_over_UAVCAN_sensor_gps_Subscriber : public UavcanDynamicPortSubscriber
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/sensor_gps.h>
template <class T>
class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber
{
public:
UORB_over_UAVCAN_sensor_gps_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "uorb.sensor_gps", instance) { };
uORB_over_UAVCAN_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "uorb.", meta->o_name, instance),
_uorb_meta{meta},
_uorb_pub(meta)
{};
~uORB_over_UAVCAN_Subscriber() override = default;
void subscribe() override
{
T *data = NULL;
// Subscribe to messages uORB sensor_gps payload over UAVCAN
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub.port_id,
sizeof(struct sensor_gps_s),
get_payload_size(data),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
{
//PX4_INFO("uORB sensor_gps Callback");
T *data = (T *)receive.payload;
if (receive.payload_size == sizeof(struct sensor_gps_s)) {
sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
gps_msg->timestamp = hrt_absolute_time();
if (receive.payload_size == get_payload_size(data)) {
/* As long as we don't have timesync between nodes we set the timestamp to the current time */
/* Data type specific conversion if necceary */
convert(data);
_sensor_gps_pub.publish(*gps_msg);
_uorb_pub.publish(*data);
} else {
PX4_ERR("uORB over UAVCAN %s playload size mismatch got %d expected %d",
_subj_sub._subject_name, receive.payload_size, sizeof(struct sensor_gps_s));
PX4_ERR("uORB over UAVCAN %s payload size mismatch got %d expected %d",
_subj_sub._subject_name, receive.payload_size, get_payload_size(data));
}
};
private:
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
protected:
// Default payload-size function -- can specialize in derived class
size_t get_payload_size(const T *msg)
{
(void)msg;
return sizeof(T);
};
void convert(T *data) {};
private:
const orb_metadata *_uorb_meta;
uORB::PublicationMulti<T> _uorb_pub;
};
/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */
template<>
void uORB_over_UAVCAN_Subscriber<sensor_gps_s>::convert(sensor_gps_s *data);

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

@ -64,6 +64,7 @@ void SubscriptionManager::subscribe()
#endif
_access_rsp.subscribe();
_list_rsp.subscribe();
updateDynamicSubscriptions();
}

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

@ -76,13 +76,14 @@
#include "ServiceClients/GetInfo.hpp"
#include "ServiceClients/Access.hpp"
#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/legacy/LegacyBatteryInfo.hpp"
#include "Subscribers/uORB/sensor_gps.hpp"
#include "Subscribers/uORB/uorb_subscriber.hpp"
typedef struct {
UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
@ -116,6 +117,7 @@ private:
// Process register requests
UavcanAccessResponse _access_rsp {_canard_instance, _param_manager};
UavcanListResponse _list_rsp {_canard_instance, _param_manager};
const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] {
#if CONFIG_UAVCAN_V1_ESC_SUBSCRIBER
@ -172,7 +174,7 @@ private:
{
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);
return new uORB_over_UAVCAN_Subscriber<sensor_gps_s>(ins, pmgr, ORB_ID(sensor_gps));
},
"uorb.sensor_gps",
0

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

@ -141,6 +141,16 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1);
*/
PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
/**
* sensor_gps uORB over UAVCAN v1 publication port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
// Publication Port IDs
/**