fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCANv1 Move PNP from UAVCAN.cpp to NodeManager.cpp

Browse Source
This commit is contained in:
Peter van der Perk 2021-02-26 22:09:16 +01:00 committed by Lorenz Meier
parent d5e468a19e
commit 28c76663cd
6 changed files with 317 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -74,6 +74,8 @@ px4_add_module(
SRCS
ParamManager.hpp
ParamManager.cpp
NodeManager.hpp
NodeManager.cpp
Uavcan.cpp
Uavcan.hpp
${SRCS}

109
src/drivers/uavcan_v1/NodeManager.cpp Normal file
View File

@ -0,0 +1,109 @@
/****************************************************************************
*
* 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 NodeIDManager.cpp
*
* Defines basic implementation of UAVCAN PNP for dynamic Node ID
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#include "NodeManager.hpp"
bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
{
if (msg.allocated_node_id.count == 0) {
msg.allocated_node_id.count = 1;
msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET;
/* Search for an available NodeID to assign */
if (msg.allocated_node_id.elements[0].value == CANARD_NODE_ID_UNSET) {
for (uint32_t i = 1; i < 16; i++) { // Note we're node ID 0
if (nodeid_registry[i].node_id == 0) { // Unused
nodeid_registry[i].node_id = 1;
memcpy(&nodeid_registry[i].unique_id, &msg.unique_id_hash, 6);
break;
} else {
if (memcmp(&nodeid_registry[i].unique_id[0], &msg.unique_id_hash, 6) == 0) {
msg.allocated_node_id.elements[0].value = nodeid_registry[i].node_id; // Existing NodeID
break;
}
}
}
}
if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) {
_uavcan_pnp_nodeidallocation_last = hrt_absolute_time();
_node_register_request_index = 0;
_node_register_last_received_index = -1;
_node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured
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_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.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);
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);
}
return result >= 0;
}
}
return false;
}
bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg)
{
//TODO V2 CAN FD implementation
return false;
}

79
src/drivers/uavcan_v1/NodeManager.hpp Normal file
View File

@ -0,0 +1,79 @@
/****************************************************************************
*
* 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 NodeManager.hpp
*
* Defines basic implementation of UAVCAN PNP for dynamic Node ID
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <px4_platform_common/defines.h>
#include "CanardInterface.hpp"
#include <uavcan/node/ID_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
typedef struct {
uint8_t node_id;
uint8_t unique_id[16];
} UavcanNodeUniqueID;
class NodeManager
{
public:
NodeManager(CanardInstance &ins) : _canard_instance(ins) { };
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
/* TODO temporary store variables here to not break the existing code
* Ideally we implement service/request classes as well and put the logic
* to set registers in here as well */
uint8_t _node_register_setup = CANARD_NODE_ID_UNSET;
int32_t _node_register_request_index = 0;
int32_t _node_register_last_received_index = -1;
hrt_abstime _uavcan_pnp_nodeidallocation_last{0};
private:
CanardInstance &_canard_instance;
CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
UavcanNodeUniqueID nodeid_registry[16] {0}; //TODO configurable or just rewrite
};

104
src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp Normal file
View File

@ -0,0 +1,104 @@
/****************************************************************************
*
* 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 NodeIDAllocationData.hpp
*
* Defines basic functionality of UAVCAN NodeIDAllocationData.1.0 subscription
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include "../NodeManager.hpp"
//Quick and Dirty PNP imlementation only V1 for now as well
#include <uavcan/node/ID_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
#define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
#include "Subscriber.hpp"
class UavcanNodeIDAllocationDataSubscriber : public UavcanSubscriber
{
public:
UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, NodeManager &nmgr) :
UavcanSubscriber(ins, pmgr, "NodeIDAllocationData", 0), _nmgr(nmgr) { };
void subscribe() override
{
// Subscribe to messages uavcan.pnp.NodeIDAllocationData
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,
&_canard_sub);
_port_id = _canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID;
};
void callback(const CanardTransfer &receive) override
{
PX4_INFO("NodeIDAllocationData");
if (_canard_instance.mtu_bytes == 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,
&msg_size_in_bytes);
/// do something with the data
_nmgr.HandleNodeIDRequest(node_id_alloc_msg);
} else {
uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg {};
size_t msg_size_in_bytes = receive.payload_size;
uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
&msg_size_in_bytes);
/// do something with the data
_nmgr.HandleNodeIDRequest(node_id_alloc_msg);
}
};
private:
NodeManager &_nmgr;
};

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

@ -168,14 +168,6 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
// Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_pnp_v1_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
@ -190,8 +182,6 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_list_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
bms_port_id,
@ -256,14 +246,15 @@ void UavcanNode::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s &&
_node_register_setup != CANARD_NODE_ID_UNSET &&
_node_register_request_index == _node_register_last_received_index + 1) {
if (hrt_elapsed_time(&_node_manager._uavcan_pnp_nodeidallocation_last) >= 1_s &&
_node_manager._node_register_setup != CANARD_NODE_ID_UNSET &&
_node_manager._node_register_request_index == _node_manager._node_register_last_received_index + 1) {
PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index);
PX4_INFO("NodeID %i request register %i", _node_manager._node_register_setup,
_node_manager._node_register_request_index);
uavcan_register_List_Request_1_0 msg;
msg.index = _node_register_request_index;
msg.index = _node_manager._node_register_request_index;
uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
@ -272,7 +263,7 @@ void UavcanNode::Run()
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = _node_register_setup,
.remote_node_id = _node_manager._node_register_setup,
.transfer_id = _uavcan_register_list_request_transfer_id,
.payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
@ -285,7 +276,7 @@ void UavcanNode::Run()
++_uavcan_register_list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &request);
++_node_register_request_index;
++_node_manager._node_register_request_index;
}
}
@ -349,10 +340,7 @@ void UavcanNode::Run()
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
result = handlePnpNodeIDAllocationData(receive);
} else if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
result = handleRegisterList(receive);
} else if (receive.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
@ -504,48 +492,6 @@ void UavcanNode::sendHeartbeat()
}
}
int UavcanNode::handlePnpNodeIDAllocationData(const CanardTransfer &receive)
{
uavcan_pnp_NodeIDAllocationData_1_0 msg;
size_t pnp_in_size_bits = receive.payload_size;
uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &pnp_in_size_bits);
//TODO internal database with unique id to node ip mappings now we give an hardcoded ID back
msg.allocated_node_id.count = 1;
msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID
_uavcan_pnp_nodeidallocation_last = hrt_absolute_time();
_node_register_request_index = 0;
_node_register_last_received_index = -1;
_node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured
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_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.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);
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);
}
return result;
}
int UavcanNode::handleRegisterList(const CanardTransfer &receive)
{
uavcan_register_List_Response_1_0 msg;
@ -557,9 +503,9 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id",
msg.name.name.count) == 0) { //Demo GPS status publisher
_node_register_setup = CANARD_NODE_ID_UNSET;
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id);
_node_register_last_received_index++;
_node_manager._node_register_last_received_index++;
uavcan_register_Access_Request_1_0 request_msg;
memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
@ -592,9 +538,9 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
} else if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id",
msg.name.name.count) == 0) { //Battery status publisher
_node_register_setup = CANARD_NODE_ID_UNSET;
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
_node_register_last_received_index++;
_node_manager._node_register_last_received_index++;
uavcan_register_Access_Request_1_0 request_msg;
memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
@ -628,8 +574,8 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
uavcan_register_Value_1_0 out_value;
if (_param_manager.GetParamByName(msg.name, out_value)) {
_node_register_setup = CANARD_NODE_ID_UNSET;
_node_register_last_received_index++;
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
_node_manager._node_register_last_received_index++;
uavcan_register_Access_Request_1_0 request_msg;
memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));

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

@ -61,21 +61,11 @@
#include <uavcan/node/Heartbeat_1_0.h>
#include <uavcan/primitive/Empty_1_0.h>
//Quick and Dirty PNP imlementation only V1 for now as well
#include <uavcan/node/ID_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
#include <reg/drone/service/battery/Parameters_0_1.h>
#include <reg/drone/service/battery/Status_0_1.h>
#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_
#define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_
#include "CanardInterface.hpp"
#include "Publishers/Publisher.hpp"
@ -85,6 +75,9 @@
#include "Subscribers/Battery.hpp"
#include "Subscribers/Esc.hpp"
#include "Subscribers/Gnss.hpp"
#include "Subscribers/NodeIDAllocationData.hpp"
#include "NodeManager.hpp"
#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
@ -213,15 +206,8 @@ private:
const uint16_t bms_port_id = 1234;
const uint16_t gps_port_id = 1235;
CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
hrt_abstime _uavcan_pnp_nodeidallocation_last{0};
CanardTransferID _uavcan_register_list_request_transfer_id{0};
CanardTransferID _uavcan_register_access_request_transfer_id{0};
//Register interface NodeID TODO MVP right have to make a queue
uint8_t _node_register_setup = CANARD_NODE_ID_UNSET;
int32_t _node_register_request_index = 0;
int32_t _node_register_last_received_index = -1;
// regulated::drone::sensor::BMSStatus_1_0
uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_service_battery_Status_0_1_EXTENT_BYTES_];
@ -238,6 +224,8 @@ private:
UavcanParamManager _param_manager;
NodeManager _node_manager {_canard_instance};
UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};
UavcanEscController _esc_controller {_canard_instance, _param_manager};
@ -251,9 +239,11 @@ private:
UavcanBmsSubscriber _bms0_sub {_canard_instance, _param_manager, 0};
UavcanBmsSubscriber _bms1_sub {_canard_instance, _param_manager, 1};
UavcanEscSubscriber _esc_sub {_canard_instance, _param_manager, 0};
UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _param_manager, _node_manager};
// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
UavcanSubscriber *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List<UavcanSubscription*>
UavcanSubscriber *_subscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub, &_nodeid_sub}; /// TODO: turn into List<UavcanSubscription*>
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
};