forked from Archive/PX4-Autopilot
UAVCANv1 Node implementation work, PNP, Registers and uORB publisher
This commit is contained in:
parent
79aa8ee587
commit
ce909b23b1
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -4,3 +4,6 @@
|
|||
#------------------------------------------------------------------------------
|
||||
|
||||
pwm_out mode_pwm1 start
|
||||
|
||||
ifup can0
|
||||
uavcan_v1 start
|
||||
|
|
|
@ -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() {};
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 ¶m : _uavcan_params) {
|
||||
|
|
|
@ -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"},
|
||||
};
|
||||
|
|
|
@ -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)
|
||||
{
|
||||
|
||||
};
|
||||
|
|
|
@ -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)
|
||||
{
|
||||
|
||||
};
|
||||
|
|
|
@ -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};
|
||||
|
||||
|
|
|
@ -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)
|
||||
{};
|
||||
|
|
|
@ -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
|
||||
{
|
||||
|
|
|
@ -55,7 +55,7 @@ class UavcanGetInfoResponse : public UavcanBaseSubscriber
|
|||
{
|
||||
public:
|
||||
UavcanGetInfoResponse(CanardInstance &ins) :
|
||||
UavcanBaseSubscriber(ins, "GetInfo", 0) { };
|
||||
UavcanBaseSubscriber(ins, "", "GetInfo", 0) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
|
|
|
@ -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, ®ister_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;
|
||||
|
||||
};
|
|
@ -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)
|
||||
|
|
|
@ -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_) { };
|
||||
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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
|
||||
{
|
||||
|
|
|
@ -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
|
||||
{
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -51,7 +51,7 @@ class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
|
|||
{
|
||||
public:
|
||||
UavcanHeartbeatSubscriber(CanardInstance &ins) :
|
||||
UavcanBaseSubscriber(ins, "Heartbeat", 0) { };
|
||||
UavcanBaseSubscriber(ins, "", "Heartbeat", 0) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
|
|
|
@ -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
|
||||
{
|
||||
|
|
|
@ -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();
|
||||
}
|
|
@ -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);
|
|
@ -64,6 +64,7 @@ void SubscriptionManager::subscribe()
|
|||
#endif
|
||||
|
||||
_access_rsp.subscribe();
|
||||
_list_rsp.subscribe();
|
||||
|
||||
updateDynamicSubscriptions();
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue