forked from Archive/PX4-Autopilot
uavcan_v1: Add PublicationManager and uORB Publisher (#17863)
This commit is contained in:
parent
d8214f5e00
commit
1ee5f75aa5
|
@ -76,6 +76,8 @@ px4_add_module(
|
|||
${LIBCANARD_DIR}/libcanard/
|
||||
${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
|
||||
SRCS
|
||||
PublicationManager.cpp
|
||||
PublicationManager.hpp
|
||||
SubscriptionManager.cpp
|
||||
SubscriptionManager.hpp
|
||||
ParamManager.hpp
|
||||
|
@ -84,6 +86,7 @@ px4_add_module(
|
|||
NodeManager.cpp
|
||||
Uavcan.cpp
|
||||
Uavcan.hpp
|
||||
Publishers/uORB/uorb_publisher.cpp
|
||||
${SRCS}
|
||||
o1heap/o1heap.c
|
||||
o1heap/o1heap.h
|
||||
|
|
|
@ -0,0 +1,121 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 PublicationManager.cpp
|
||||
*
|
||||
* Manages the dynamic (run-time configurable) UAVCAN publications
|
||||
*
|
||||
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||
* @author Jacob Crabill <jacob@flyvoly.com>
|
||||
*/
|
||||
|
||||
|
||||
#include "PublicationManager.hpp"
|
||||
|
||||
PublicationManager::~PublicationManager()
|
||||
{
|
||||
_dynpublishers.clear();
|
||||
}
|
||||
|
||||
void PublicationManager::updateDynamicPublications()
|
||||
{
|
||||
for (auto &sub : _uavcan_pubs) {
|
||||
|
||||
bool found_publisher = false;
|
||||
|
||||
for (auto &dynpub : _dynpublishers) {
|
||||
// Check if subscriber has already been created
|
||||
const char *subj_name = dynpub->getSubjectName();
|
||||
const uint8_t instance = dynpub->getInstance();
|
||||
|
||||
if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) {
|
||||
found_publisher = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (found_publisher) {
|
||||
continue;
|
||||
}
|
||||
|
||||
char uavcan_param[90];
|
||||
snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s.%d.id", sub.subject_name, sub.instance);
|
||||
uavcan_register_Value_1_0 value;
|
||||
|
||||
if (_param_manager.GetParamByName(uavcan_param, value)) {
|
||||
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);
|
||||
|
||||
if (dynpub == nullptr) {
|
||||
PX4_ERR("Out of memory");
|
||||
return;
|
||||
}
|
||||
|
||||
_dynpublishers.add(dynpub);
|
||||
|
||||
dynpub->updateParam();
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Port ID param for publisher %s.%u not found", sub.subject_name, sub.instance);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PublicationManager::printInfo()
|
||||
{
|
||||
for (auto &dynpub : _dynpublishers) {
|
||||
dynpub->printInfo();
|
||||
}
|
||||
}
|
||||
|
||||
void PublicationManager::updateParams()
|
||||
{
|
||||
for (auto &dynpub : _dynpublishers) {
|
||||
dynpub->updateParam();
|
||||
}
|
||||
|
||||
// Check for any newly-enabled publication
|
||||
updateDynamicPublications();
|
||||
}
|
||||
|
||||
void PublicationManager::update()
|
||||
{
|
||||
for (auto &dynpub : _dynpublishers) {
|
||||
dynpub->update();
|
||||
}
|
||||
}
|
|
@ -0,0 +1,125 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 PublicationManager.hpp
|
||||
*
|
||||
* Manages the dynamic (run-time configurable) UAVCAN publications
|
||||
*
|
||||
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||
* @author Jacob Crabill <jacob@flyvoly.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include "Publishers/Publisher.hpp"
|
||||
#include "CanardInterface.hpp"
|
||||
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
#include "Actuators/EscClient.hpp"
|
||||
#include "Publishers/DS-015/Readiness.hpp"
|
||||
#include "Publishers/DS-015/Gnss.hpp"
|
||||
#include "Publishers/uORB/uorb_publisher.hpp"
|
||||
|
||||
typedef struct {
|
||||
UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
|
||||
const char *subject_name;
|
||||
const uint8_t instance;
|
||||
} UavcanDynPubBinder;
|
||||
|
||||
class PublicationManager
|
||||
{
|
||||
public:
|
||||
PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {}
|
||||
~PublicationManager();
|
||||
|
||||
void update();
|
||||
void printInfo();
|
||||
void updateParams();
|
||||
|
||||
private:
|
||||
void updateDynamicPublications();
|
||||
|
||||
CanardInstance &_canard_instance;
|
||||
UavcanParamManager &_param_manager;
|
||||
List<UavcanPublisher *> _dynpublishers;
|
||||
|
||||
const UavcanDynPubBinder _uavcan_pubs[5] {
|
||||
{
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
|
||||
{
|
||||
return new UavcanGnssPublisher(ins, pmgr, 0);
|
||||
},
|
||||
"gps",
|
||||
0
|
||||
},
|
||||
{
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
|
||||
{
|
||||
return new UavcanEscController(ins, pmgr);
|
||||
},
|
||||
"esc",
|
||||
0
|
||||
},
|
||||
{
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
|
||||
{
|
||||
return new UavcanReadinessPublisher(ins, pmgr, 0);
|
||||
},
|
||||
"readiness",
|
||||
0
|
||||
},
|
||||
{
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
|
||||
{
|
||||
return new uORB_over_UAVCAN_Publisher<actuator_outputs_s>(ins, pmgr, ORB_ID(actuator_outputs));
|
||||
},
|
||||
"uorb.actuator_outputs",
|
||||
0
|
||||
},
|
||||
|
||||
{
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
|
||||
{
|
||||
return new uORB_over_UAVCAN_Publisher<sensor_gps_s>(ins, pmgr, ORB_ID(sensor_gps));
|
||||
},
|
||||
"uorb.sensor_gps",
|
||||
0
|
||||
},
|
||||
};
|
||||
};
|
|
@ -45,7 +45,7 @@
|
|||
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
|
||||
#include <reg/drone/service/gnss/DilutionOfPrecision_0_1.h>
|
||||
|
||||
#include "Publisher.hpp"
|
||||
#include "../Publisher.hpp"
|
||||
|
||||
class UavcanGnssPublisher : public UavcanPublisher
|
||||
{
|
||||
|
@ -56,6 +56,8 @@ public:
|
|||
|
||||
};
|
||||
|
||||
~UavcanGnssPublisher() override = default;
|
||||
|
||||
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||
virtual void update() override
|
||||
{
|
|
@ -45,7 +45,7 @@
|
|||
// DS-15 Specification Messages
|
||||
#include <reg/drone/service/common/Readiness_0_1.h>
|
||||
|
||||
#include "Publisher.hpp"
|
||||
#include "../Publisher.hpp"
|
||||
|
||||
class UavcanReadinessPublisher : public UavcanPublisher
|
||||
{
|
||||
|
@ -56,6 +56,8 @@ public:
|
|||
|
||||
};
|
||||
|
||||
~UavcanReadinessPublisher() override = default;
|
||||
|
||||
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||
virtual void update() override
|
||||
{
|
|
@ -44,6 +44,7 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <lib/parameters/param.h>
|
||||
#include <containers/List.hpp>
|
||||
|
||||
#include <uavcan/_register/Access_1_0.h>
|
||||
|
||||
|
@ -56,12 +57,14 @@
|
|||
*/
|
||||
#define PUBLISHER_DEFAULT_TIMEOUT_USEC 100000UL
|
||||
|
||||
class UavcanPublisher
|
||||
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) { };
|
||||
|
||||
virtual ~UavcanPublisher() = default;
|
||||
|
||||
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||
virtual void update() = 0;
|
||||
|
||||
|
@ -101,6 +104,26 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
const char *getSubjectName()
|
||||
{
|
||||
return _subject_name;
|
||||
}
|
||||
|
||||
uint8_t getInstance()
|
||||
{
|
||||
return _instance;
|
||||
}
|
||||
|
||||
UavcanPublisher *next()
|
||||
{
|
||||
return _next_pub;
|
||||
}
|
||||
|
||||
void setNext(UavcanPublisher *next)
|
||||
{
|
||||
_next_pub = next;
|
||||
}
|
||||
|
||||
protected:
|
||||
CanardInstance &_canard_instance;
|
||||
UavcanParamManager &_param_manager;
|
||||
|
@ -109,4 +132,6 @@ protected:
|
|||
|
||||
CanardPortID _port_id {CANARD_PORT_ID_UNSET};
|
||||
CanardTransferID _transfer_id {0};
|
||||
|
||||
UavcanPublisher *_next_pub {nullptr};
|
||||
};
|
||||
|
|
|
@ -32,53 +32,22 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file sensor_gps.hpp
|
||||
* @file uorb_template.cpp
|
||||
*
|
||||
* Defines uORB over UAVCANv1 sensor_gps publisher
|
||||
* Defines generic, templatized uORB over UAVCANv1 publisher
|
||||
*
|
||||
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||
* @author Jacob Crabill <jacob@flyvoly.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "uorb_publisher.hpp"
|
||||
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
|
||||
|
||||
#include "../Publisher.hpp"
|
||||
|
||||
class UORB_over_UAVCAN_sensor_gps_Publisher : public UavcanPublisher
|
||||
template<>
|
||||
size_t uORB_over_UAVCAN_Publisher<actuator_outputs_s>::get_payload_size(actuator_outputs_s *msg)
|
||||
{
|
||||
public:
|
||||
UORB_over_UAVCAN_sensor_gps_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||
UavcanPublisher(ins, pmgr, "sensor_gps", instance)
|
||||
{};
|
||||
|
||||
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||
virtual void update() override
|
||||
{
|
||||
// Not sure if actuator_armed is a good indication of readiness but seems close to it
|
||||
if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
|
||||
sensor_gps_s gps_msg {};
|
||||
_sensor_gps_sub.update(&gps_msg);
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
|
||||
.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 = sizeof(struct sensor_gps_s),
|
||||
.payload = &gps_msg,
|
||||
};
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
|
||||
};
|
||||
// Remove unvalid output & padding from payload_size to save bandwidth
|
||||
return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) -
|
||||
((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0]));
|
||||
}
|
|
@ -32,34 +32,40 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file actuator_outputs.hpp
|
||||
* @file uorb_template.hpp
|
||||
*
|
||||
* Defines uORB over UAVCANv1 actuator_outputs publisher
|
||||
* Defines generic, templatized uORB over UAVCANv1 publisher
|
||||
*
|
||||
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
|
||||
#include "../Publisher.hpp"
|
||||
|
||||
class UORB_over_UAVCAN_actuator_outputs_Publisher : public UavcanPublisher
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
|
||||
template <class T>
|
||||
class uORB_over_UAVCAN_Publisher : public UavcanPublisher
|
||||
{
|
||||
public:
|
||||
UORB_over_UAVCAN_actuator_outputs_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||
UavcanPublisher(ins, pmgr, "actuator_outputs", instance)
|
||||
uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
|
||||
uint8_t instance = 0) :
|
||||
UavcanPublisher(ins, pmgr, meta->o_name, instance),
|
||||
_uorb_meta{meta},
|
||||
_uorb_sub(meta)
|
||||
{};
|
||||
|
||||
~uORB_over_UAVCAN_Publisher() override = default;
|
||||
|
||||
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||
// FIXME think about update and limiting
|
||||
virtual void update() override
|
||||
{
|
||||
// Not sure if actuator_armed is a good indication of readiness but seems close to it
|
||||
if (_actuator_outputs_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
|
||||
actuator_outputs_s actuator_msg {};
|
||||
_actuator_outputs_sub.update(&actuator_msg);
|
||||
if (_uorb_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
|
||||
T data {};
|
||||
_uorb_sub.update(&data);
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
|
||||
|
@ -68,8 +74,8 @@ public:
|
|||
.port_id = _port_id, // This is the subject-ID.
|
||||
.remote_node_id = CANARD_NODE_ID_UNSET,
|
||||
.transfer_id = _transfer_id,
|
||||
.payload_size = actuator_payload_size(&actuator_msg),
|
||||
.payload = &actuator_msg,
|
||||
.payload_size = get_payload_size(&data),
|
||||
.payload = &data,
|
||||
};
|
||||
|
||||
// set the data ready in the buffer and chop if needed
|
||||
|
@ -78,14 +84,20 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
// Remove unvalid output & padding from payload_size to save bandwidth
|
||||
size_t actuator_payload_size(actuator_outputs_s *msg)
|
||||
protected:
|
||||
// Default payload-size function -- can specialize in derived class
|
||||
size_t get_payload_size(T *msg)
|
||||
{
|
||||
return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) -
|
||||
((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0]));
|
||||
(void)msg;
|
||||
return sizeof(T);
|
||||
}
|
||||
|
||||
uORB::Subscription _actuator_outputs_sub{ORB_ID(actuator_outputs)};
|
||||
private:
|
||||
const orb_metadata *_uorb_meta;
|
||||
uORB::Subscription _uorb_sub;
|
||||
};
|
||||
|
||||
/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
|
||||
|
||||
template<>
|
||||
size_t uORB_over_UAVCAN_Publisher<actuator_outputs_s>::get_payload_size(actuator_outputs_s *msg);
|
|
@ -87,7 +87,7 @@ public:
|
|||
{
|
||||
PX4_INFO("Response");
|
||||
|
||||
if (_response_callback != NULL) {
|
||||
if (_response_callback != nullptr) {
|
||||
_response_callback->response_callback(receive);
|
||||
}
|
||||
};
|
||||
|
@ -99,6 +99,6 @@ protected:
|
|||
|
||||
const CanardPortID _portID;
|
||||
const size_t _extent;
|
||||
UavcanServiceRequestInterface *_response_callback = NULL;
|
||||
UavcanServiceRequestInterface *_response_callback = nullptr;
|
||||
|
||||
};
|
||||
|
|
|
@ -71,7 +71,7 @@ public:
|
|||
{
|
||||
SubjectSubscription *curSubj = &_subj_sub;
|
||||
|
||||
while (curSubj != NULL) {
|
||||
while (curSubj != nullptr) {
|
||||
canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub.port_id);
|
||||
curSubj = curSubj->next;
|
||||
}
|
||||
|
@ -84,7 +84,7 @@ public:
|
|||
uint32_t i = 0;
|
||||
SubjectSubscription *curSubj = &_subj_sub;
|
||||
|
||||
while (curSubj != NULL) {
|
||||
while (curSubj != nullptr) {
|
||||
if (instance == i) {
|
||||
return curSubj->_canard_sub.port_id;
|
||||
}
|
||||
|
@ -104,7 +104,7 @@ public:
|
|||
|
||||
SubjectSubscription *curSubj = &_subj_sub;
|
||||
|
||||
while (curSubj != NULL) {
|
||||
while (curSubj != nullptr) {
|
||||
if (port_id == curSubj->_canard_sub.port_id) {
|
||||
return true;
|
||||
}
|
||||
|
@ -129,7 +129,7 @@ public:
|
|||
{
|
||||
SubjectSubscription *curSubj = &_subj_sub;
|
||||
|
||||
while (curSubj != NULL) {
|
||||
while (curSubj != nullptr) {
|
||||
if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
|
||||
PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
|
||||
}
|
||||
|
@ -142,7 +142,7 @@ protected:
|
|||
struct SubjectSubscription {
|
||||
CanardRxSubscription _canard_sub;
|
||||
const char *_subject_name;
|
||||
struct SubjectSubscription *next {NULL};
|
||||
struct SubjectSubscription *next {nullptr};
|
||||
};
|
||||
|
||||
CanardInstance &_canard_instance;
|
||||
|
|
|
@ -68,7 +68,7 @@ public:
|
|||
|
||||
_parameters_sub._subject_name = _parameters_name;
|
||||
_parameters_sub._canard_sub.user_reference = this;
|
||||
_parameters_sub.next = NULL;
|
||||
_parameters_sub.next = nullptr;
|
||||
}
|
||||
|
||||
void subscribe() override
|
||||
|
|
|
@ -62,7 +62,7 @@ public:
|
|||
{
|
||||
SubjectSubscription *curSubj = &_subj_sub;
|
||||
|
||||
while (curSubj != NULL) {
|
||||
while (curSubj != nullptr) {
|
||||
char uavcan_param[90];
|
||||
snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance);
|
||||
|
||||
|
@ -112,5 +112,5 @@ public:
|
|||
|
||||
protected:
|
||||
UavcanParamManager &_param_manager;
|
||||
UavcanDynamicPortSubscriber *_next_sub {NULL};
|
||||
UavcanDynamicPortSubscriber *_next_sub {nullptr};
|
||||
};
|
||||
|
|
|
@ -48,7 +48,7 @@ SubscriptionManager::~SubscriptionManager()
|
|||
{
|
||||
UavcanDynamicPortSubscriber *dynsub;
|
||||
|
||||
while (_dynsubscribers != NULL) {
|
||||
while (_dynsubscribers != nullptr) {
|
||||
dynsub = _dynsubscribers;
|
||||
_dynsubscribers = dynsub->next();
|
||||
delete dynsub;
|
||||
|
@ -72,7 +72,7 @@ void SubscriptionManager::updateDynamicSubscriptions()
|
|||
bool found_subscriber = false;
|
||||
UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
|
||||
|
||||
while (dynsub != NULL) {
|
||||
while (dynsub != nullptr) {
|
||||
// Check if subscriber has already been created
|
||||
const char *subj_name = dynsub->getSubjectName();
|
||||
const uint8_t instance = dynsub->getInstance();
|
||||
|
@ -99,12 +99,12 @@ void SubscriptionManager::updateDynamicSubscriptions()
|
|||
if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber
|
||||
dynsub = sub.create_sub(_canard_instance, _param_manager);
|
||||
|
||||
if (dynsub == NULL) {
|
||||
if (dynsub == nullptr) {
|
||||
PX4_ERR("Out of memory");
|
||||
return;
|
||||
}
|
||||
|
||||
if (_dynsubscribers == NULL) {
|
||||
if (_dynsubscribers == nullptr) {
|
||||
// Set the head of our linked list
|
||||
_dynsubscribers = dynsub;
|
||||
|
||||
|
@ -112,7 +112,7 @@ void SubscriptionManager::updateDynamicSubscriptions()
|
|||
// Append the new subscriber to our linked list
|
||||
UavcanDynamicPortSubscriber *tmp = _dynsubscribers;
|
||||
|
||||
while (tmp->next() != NULL) {
|
||||
while (tmp->next() != nullptr) {
|
||||
tmp = tmp->next();
|
||||
}
|
||||
|
||||
|
@ -133,7 +133,7 @@ void SubscriptionManager::printInfo()
|
|||
{
|
||||
UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
|
||||
|
||||
while (dynsub != NULL) {
|
||||
while (dynsub != nullptr) {
|
||||
dynsub->printInfo();
|
||||
dynsub = dynsub->next();
|
||||
}
|
||||
|
@ -143,7 +143,7 @@ void SubscriptionManager::updateParams()
|
|||
{
|
||||
UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
|
||||
|
||||
while (dynsub != NULL) {
|
||||
while (dynsub != nullptr) {
|
||||
dynsub->updateParam();
|
||||
dynsub = dynsub->next();
|
||||
}
|
||||
|
|
|
@ -78,7 +78,7 @@ private:
|
|||
|
||||
CanardInstance &_canard_instance;
|
||||
UavcanParamManager &_param_manager;
|
||||
UavcanDynamicPortSubscriber *_dynsubscribers {NULL};
|
||||
UavcanDynamicPortSubscriber *_dynsubscribers {nullptr};
|
||||
|
||||
UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance};
|
||||
|
||||
|
|
|
@ -82,9 +82,7 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
|
|||
|
||||
_node_manager.subscribe();
|
||||
|
||||
for (auto &publisher : _publishers) {
|
||||
publisher->updateParam();
|
||||
}
|
||||
_pub_manager.updateParams();
|
||||
|
||||
_sub_manager.subscribe();
|
||||
|
||||
|
@ -185,12 +183,8 @@ void UavcanNode::Run()
|
|||
// update parameters from storage
|
||||
updateParams();
|
||||
|
||||
for (auto &publisher : _publishers) {
|
||||
// Have the publisher update its associated port-id parameter
|
||||
// Setting to 0 disable publication
|
||||
publisher->updateParam();
|
||||
}
|
||||
|
||||
// Update dynamic pub/sub objects based on Port ID params
|
||||
_pub_manager.updateParams();
|
||||
_sub_manager.updateParams();
|
||||
|
||||
_mixing_output.updateParams();
|
||||
|
@ -205,9 +199,7 @@ void UavcanNode::Run()
|
|||
sendHeartbeat();
|
||||
|
||||
// Check all publishers
|
||||
for (auto &publisher : _publishers) {
|
||||
publisher->update();
|
||||
}
|
||||
_pub_manager.update();
|
||||
|
||||
_node_manager.update();
|
||||
|
||||
|
@ -221,7 +213,7 @@ void UavcanNode::Run()
|
|||
|
||||
while (!_task_should_exit.load() && _can_interface->receive(&received_frame) > 0) {
|
||||
CanardTransfer receive{};
|
||||
CanardRxSubscription *subscription = NULL;
|
||||
CanardRxSubscription *subscription = nullptr;
|
||||
int32_t result = canardRxAccept2(&_canard_instance, &received_frame, 0, &receive, &subscription);
|
||||
|
||||
if (result < 0) {
|
||||
|
@ -235,7 +227,7 @@ void UavcanNode::Run()
|
|||
// A transfer has been received, process it.
|
||||
// PX4_INFO("received Port ID: %d", receive.port_id);
|
||||
|
||||
if (subscription != NULL) {
|
||||
if (subscription != nullptr) {
|
||||
UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
|
||||
sub_instance->callback(receive);
|
||||
|
||||
|
@ -325,14 +317,12 @@ void UavcanNode::print_info()
|
|||
heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
|
||||
heap_diagnostics.oom_count);
|
||||
|
||||
for (auto &publisher : _publishers) {
|
||||
publisher->printInfo();
|
||||
}
|
||||
_pub_manager.printInfo();
|
||||
|
||||
CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage];
|
||||
|
||||
while (rxs != NULL) {
|
||||
if (rxs->user_reference == NULL) {
|
||||
while (rxs != nullptr) {
|
||||
if (rxs->user_reference == nullptr) {
|
||||
PX4_INFO("Message port id %d", rxs->port_id);
|
||||
|
||||
} else {
|
||||
|
@ -344,8 +334,8 @@ void UavcanNode::print_info()
|
|||
|
||||
rxs = _canard_instance.rx_subscriptions[CanardTransferKindRequest];
|
||||
|
||||
while (rxs != NULL) {
|
||||
if (rxs->user_reference == NULL) {
|
||||
while (rxs != nullptr) {
|
||||
if (rxs->user_reference == nullptr) {
|
||||
PX4_INFO("Service response port id %d", rxs->port_id);
|
||||
|
||||
} else {
|
||||
|
@ -357,8 +347,8 @@ void UavcanNode::print_info()
|
|||
|
||||
rxs = _canard_instance.rx_subscriptions[CanardTransferKindResponse];
|
||||
|
||||
while (rxs != NULL) {
|
||||
if (rxs->user_reference == NULL) {
|
||||
while (rxs != nullptr) {
|
||||
if (rxs->user_reference == nullptr) {
|
||||
PX4_INFO("Service request port id %d", rxs->port_id);
|
||||
|
||||
} else {
|
||||
|
|
|
@ -46,6 +46,7 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
@ -58,11 +59,11 @@
|
|||
#include "CanardInterface.hpp"
|
||||
|
||||
#include "Publishers/Publisher.hpp"
|
||||
#include "Publishers/Gnss.hpp"
|
||||
#include "Publishers/uORB/actuator_outputs.hpp"
|
||||
#include "Publishers/uORB/uorb_publisher.hpp"
|
||||
|
||||
#include "NodeManager.hpp"
|
||||
|
||||
#include "PublicationManager.hpp"
|
||||
#include "SubscriptionManager.hpp"
|
||||
|
||||
#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
|
||||
|
@ -180,18 +181,12 @@ private:
|
|||
|
||||
NodeManager _node_manager {_canard_instance, _param_manager};
|
||||
|
||||
PublicationManager _pub_manager {_canard_instance, _param_manager};
|
||||
SubscriptionManager _sub_manager {_canard_instance, _param_manager};
|
||||
|
||||
UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};
|
||||
|
||||
UORB_over_UAVCAN_actuator_outputs_Publisher _actuator_pub {_canard_instance, _param_manager}; // uORB
|
||||
|
||||
/// TODO: Integrate with PublicationManager
|
||||
UavcanEscController _esc_controller {_canard_instance, _param_manager};
|
||||
|
||||
// Publication objects: Any object used to bridge a uORB message to a UAVCAN message
|
||||
/// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers
|
||||
UavcanPublisher *_publishers[3] {&_gps_pub, &_esc_controller, &_actuator_pub};
|
||||
|
||||
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
|
||||
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue