forked from Archive/PX4-Autopilot
Added the notion of BaseSubscriber which allows to
subsscription for services responses and request and helps the usage of fixed port subscribers Furthermore move register autconfigure logic from Uavcan.cpp to NodeManager
This commit is contained in:
Peter van der Perk
Lorenz Meier
parent
9f9b01504d
commit
c4ab2797eb
|
|
@ -48,7 +48,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
|
|||
msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET;
|
||||
|
||||
/* Search for an available NodeID to assign */
|
||||
for (uint32_t i = 1; i < 16; i++) {
|
||||
for (uint32_t i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
|
||||
if (i == _canard_instance.node_id) {
|
||||
continue; // Don't give our NodeID to a node
|
||||
|
||||
|
|
@ -65,11 +65,6 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
|
|||
|
||||
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_];
|
||||
|
|
@ -93,6 +88,8 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
|
|||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
|
||||
_register_request_last = hrt_absolute_time();
|
||||
|
||||
return result >= 0;
|
||||
}
|
||||
}
|
||||
|
|
@ -106,3 +103,49 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg)
|
|||
//TODO V2 CAN FD implementation
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg)
|
||||
{
|
||||
if (msg.name.name.count == 0) {
|
||||
// Index doesn't exist, we've parsed through all registers
|
||||
for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
|
||||
if (nodeid_registry[i].node_id == node_id) {
|
||||
nodeid_registry[i].register_setup = true; // Don't update anymore
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
|
||||
if (nodeid_registry[i].node_id == node_id) {
|
||||
nodeid_registry[i].register_index++; // Increment index counter for next update()
|
||||
nodeid_registry[i].register_setup = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (strncmp((char *)msg.name.name.elements, gps_uorb_register_name,
|
||||
msg.name.name.count) == 0) {
|
||||
_access_request.setPortId(node_id, gps_uorb_register_name, 1235); //TODO configurable and combine with ParamManager.
|
||||
|
||||
} else if (strncmp((char *)msg.name.name.elements, bms_status_register_name,
|
||||
msg.name.name.count) == 0) { //Battery status publisher
|
||||
_access_request.setPortId(node_id, bms_status_register_name, 1234); //TODO configurable and combine with ParamManager.
|
||||
}
|
||||
}
|
||||
}
|
||||
void NodeManager::update()
|
||||
{
|
||||
if (hrt_elapsed_time(&_register_request_last) >= hrt_abstime(2 *
|
||||
1000000ULL)) { // Compiler hates me here, some 1_s doesn't work
|
||||
for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
|
||||
if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) {
|
||||
//Setting up registers
|
||||
_list_request.request(nodeid_registry[i].node_id, nodeid_registry[i].register_index);
|
||||
nodeid_registry[i].register_setup = true;
|
||||
}
|
||||
}
|
||||
|
||||
_register_request_last = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@
|
|||
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "CanardInterface.hpp"
|
||||
|
||||
|
|
@ -50,30 +51,46 @@
|
|||
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
|
||||
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
|
||||
|
||||
|
||||
class NodeManager;
|
||||
|
||||
#include "Services/AccessRequest.hpp"
|
||||
#include "Services/ListRequest.hpp"
|
||||
|
||||
//TODO make this an object instead?
|
||||
typedef struct {
|
||||
uint8_t node_id;
|
||||
uint8_t unique_id[16];
|
||||
} UavcanNodeUniqueID;
|
||||
uint8_t node_id;
|
||||
uint8_t unique_id[16];
|
||||
bool register_setup;
|
||||
uint16_t register_index;
|
||||
} UavcanNodeEntry;
|
||||
|
||||
class NodeManager
|
||||
{
|
||||
public:
|
||||
NodeManager(CanardInstance &ins) : _canard_instance(ins) { };
|
||||
NodeManager(CanardInstance &ins) : _canard_instance(ins), _access_request(ins), _list_request(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};
|
||||
void HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg);
|
||||
|
||||
void update();
|
||||
|
||||
private:
|
||||
CanardInstance &_canard_instance;
|
||||
CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
|
||||
UavcanNodeUniqueID nodeid_registry[16] {0}; //TODO configurable or just rewrite
|
||||
UavcanNodeEntry nodeid_registry[16] {0}; //TODO configurable or just rewrite
|
||||
|
||||
UavcanAccessServiceRequest _access_request;
|
||||
UavcanListServiceRequest _list_request;
|
||||
|
||||
bool nodeRegisterSetup = 0;
|
||||
|
||||
hrt_abstime _register_request_last{0};
|
||||
|
||||
//TODO work this out
|
||||
const char *gps_uorb_register_name = "uavcan.pub.gnss_uorb.id";
|
||||
const char *bms_status_register_name = "uavcan.pub.battery_status.id";
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,134 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 Access.hpp
|
||||
*
|
||||
* Defines response to a Access 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/node/ID_1_0.h>
|
||||
#include <uavcan/node/GetInfo_1_0.h>
|
||||
|
||||
#include "../Subscribers/BaseSubscriber.hpp"
|
||||
|
||||
class UavcanAccessResponse : public UavcanBaseSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanAccessResponse(CanardInstance &ins, UavcanParamManager &pmgr) :
|
||||
UavcanBaseSubscriber(ins, "Access", 0), _param_manager(pmgr) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindRequest,
|
||||
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
||||
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_;
|
||||
|
||||
};
|
||||
|
||||
void callback(const CanardTransfer &receive) override
|
||||
{
|
||||
PX4_INFO("Access request");
|
||||
|
||||
uavcan_register_Access_Request_1_0 msg;
|
||||
uavcan_register_Access_Request_1_0_initialize_(&msg);
|
||||
|
||||
size_t register_in_size_bits = receive.payload_size;
|
||||
uavcan_register_Access_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits);
|
||||
|
||||
int result {0};
|
||||
|
||||
uavcan_register_Value_1_0 value = msg.value;
|
||||
uavcan_register_Name_1_0 name = msg.name;
|
||||
|
||||
/// TODO: get/set parameter based on whether empty or not
|
||||
if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0
|
||||
// Value is empty -- 'Get' only
|
||||
result = _param_manager.GetParamByName(name, value) ? 0 : -1;
|
||||
|
||||
} else {
|
||||
// Set value
|
||||
result = _param_manager.SetParamByName(name, value) ? 0 : -1;
|
||||
|
||||
}
|
||||
|
||||
/// TODO: Access_Response
|
||||
uavcan_register_Access_Response_1_0 response {};
|
||||
response.value = value;
|
||||
|
||||
uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindResponse,
|
||||
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
|
||||
.transfer_id = access_response_transfer_id, /// TODO: track register Access _response_ separately?
|
||||
.payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &response_payload_buffer,
|
||||
};
|
||||
|
||||
result = uavcan_register_Access_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
|
||||
++access_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
|
||||
//return result;
|
||||
|
||||
};
|
||||
|
||||
private:
|
||||
UavcanParamManager &_param_manager;
|
||||
CanardTransferID access_response_transfer_id = 0;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,147 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 GetInfo.hpp
|
||||
*
|
||||
* Defines response to a GetInfo 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 "../NodeManager.hpp"
|
||||
|
||||
#include <uavcan/node/ID_1_0.h>
|
||||
#include <uavcan/node/GetInfo_1_0.h>
|
||||
|
||||
#include "../Subscribers/BaseSubscriber.hpp"
|
||||
|
||||
class UavcanGetInfoResponse : public UavcanBaseSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanGetInfoResponse(CanardInstance &ins) :
|
||||
UavcanBaseSubscriber(ins, "GetInfo", 0) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindRequest,
|
||||
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
|
||||
uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_;
|
||||
|
||||
};
|
||||
|
||||
void callback(const CanardTransfer &receive) override
|
||||
{
|
||||
PX4_INFO("GetInfo request");
|
||||
|
||||
// Setup node.GetInfo response
|
||||
|
||||
uavcan_node_GetInfo_Response_1_0 node_info;
|
||||
|
||||
uavcan_node_GetInfo_Response_1_0_initialize_(&node_info);
|
||||
|
||||
node_info.protocol_version.major = 1;
|
||||
node_info.protocol_version.minor = 0;
|
||||
|
||||
#if defined(BOARD_HAS_VERSIONING)
|
||||
node_info.hardware_version.major = (uint8_t)px4_board_hw_version();
|
||||
node_info.hardware_version.minor = (uint8_t)px4_board_hw_revision();
|
||||
#endif
|
||||
|
||||
unsigned fwver = px4_firmware_version();
|
||||
node_info.software_version.major = (fwver >> (8 * 3)) & 0xFF;
|
||||
node_info.software_version.minor = (fwver >> (8 * 2)) & 0xFF;
|
||||
|
||||
node_info.software_vcs_revision_id = px4_firmware_version_binary();
|
||||
|
||||
px4_guid_t px4_guid;
|
||||
board_get_px4_guid(px4_guid);
|
||||
memcpy(node_info.unique_id, px4_guid, sizeof(node_info.unique_id));
|
||||
|
||||
//TODO proper name
|
||||
strncpy((char *)node_info.name.elements,
|
||||
px4_board_name(),
|
||||
uavcan_node_GetInfo_Response_1_0_name_ARRAY_CAPACITY_);
|
||||
|
||||
node_info.name.count = strlen(px4_board_name());
|
||||
|
||||
uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardMicrosecond transmission_deadline = hrt_absolute_time() + 1000 * 100;
|
||||
|
||||
CanardTransfer response = {
|
||||
.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindResponse,
|
||||
.port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = receive.remote_node_id, // Send back to request Node
|
||||
.transfer_id = getinfo_response_transfer_id,
|
||||
.payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &response_payload_buffer,
|
||||
};
|
||||
|
||||
int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(&node_info, (uint8_t *)&response_payload_buffer,
|
||||
&response.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++getinfo_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &response);
|
||||
}
|
||||
|
||||
//TODO proper error handling
|
||||
if (result < 0) {
|
||||
// An error has occurred: either an argument is invalid or we've ran out of memory.
|
||||
// It is possible to statically prove that an out-of-memory will never occur for a given application if the
|
||||
// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
|
||||
// return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
private:
|
||||
CanardTransferID getinfo_response_transfer_id = 0;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 Access.hpp
|
||||
*
|
||||
* Defines a Access Service invoker and process Access responses
|
||||
*
|
||||
* @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 "../NodeManager.hpp"
|
||||
|
||||
#include <uavcan/_register/Access_1_0.h>
|
||||
|
||||
#include "../Subscribers/BaseSubscriber.hpp"
|
||||
|
||||
class UavcanAccessServiceReply : public UavcanBaseSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanAccessServiceReply(CanardInstance &ins, NodeManager &nmgr) :
|
||||
UavcanBaseSubscriber(ins, "Access", 0), _nmgr(nmgr) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindResponse,
|
||||
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
||||
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_;
|
||||
|
||||
};
|
||||
|
||||
void callback(const CanardTransfer &receive) override
|
||||
{
|
||||
PX4_INFO("Access response");
|
||||
};
|
||||
|
||||
private:
|
||||
NodeManager &_nmgr;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,94 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 Access.hpp
|
||||
*
|
||||
* Defines a Access Service invoker and process Access responses
|
||||
*
|
||||
* @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 <uavcan/_register/Access_1_0.h>
|
||||
|
||||
class UavcanAccessServiceRequest
|
||||
{
|
||||
public:
|
||||
UavcanAccessServiceRequest(CanardInstance &ins) :
|
||||
_canard_instance(ins) { };
|
||||
|
||||
void setPortId(CanardNodeID node_id, const char *register_name, uint16_t port_id)
|
||||
{
|
||||
int result {0};
|
||||
|
||||
uavcan_register_Access_Request_1_0 request_msg;
|
||||
strncpy((char *)&request_msg.name.name.elements[0], register_name, sizeof(uavcan_register_Name_1_0));
|
||||
request_msg.name.name.count = strlen(register_name);
|
||||
|
||||
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
|
||||
request_msg.value.natural16.value.count = 1;
|
||||
request_msg.value.natural16.value.elements[0] = port_id;
|
||||
|
||||
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindRequest,
|
||||
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = node_id, // Messages cannot be unicast, so use UNSET.
|
||||
.transfer_id = access_request_transfer_id,
|
||||
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &request_payload_buffer,
|
||||
};
|
||||
|
||||
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
CanardInstance &_canard_instance;
|
||||
CanardTransferID access_request_transfer_id = 0;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 a List Service invoker and process List responses
|
||||
*
|
||||
* @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 "../NodeManager.hpp"
|
||||
|
||||
#include <uavcan/_register/List_1_0.h>
|
||||
|
||||
#include "../Subscribers/BaseSubscriber.hpp"
|
||||
|
||||
class UavcanListServiceReply : public UavcanBaseSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanListServiceReply(CanardInstance &ins, NodeManager &nmgr) :
|
||||
UavcanBaseSubscriber(ins, "List", 0), _nmgr(nmgr) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindResponse,
|
||||
uavcan_register_List_1_0_FIXED_PORT_ID_,
|
||||
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_register_List_1_0_FIXED_PORT_ID_;
|
||||
|
||||
};
|
||||
|
||||
void callback(const CanardTransfer &receive) override
|
||||
{
|
||||
PX4_INFO("List response");
|
||||
|
||||
uavcan_register_List_Response_1_0 msg;
|
||||
|
||||
size_t register_in_size_bits = receive.payload_size;
|
||||
uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits);
|
||||
|
||||
// Pass msg.name.name.elements to NodeManager
|
||||
_nmgr.HandleListResponse(receive.remote_node_id, msg);
|
||||
};
|
||||
|
||||
private:
|
||||
NodeManager &_nmgr;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 ListRequest.hpp
|
||||
*
|
||||
* Defines a List Service invoker and process List responses
|
||||
*
|
||||
* @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 <uavcan/_register/List_1_0.h>
|
||||
|
||||
class UavcanListServiceRequest
|
||||
{
|
||||
public:
|
||||
UavcanListServiceRequest(CanardInstance &ins) :
|
||||
_canard_instance(ins) { };
|
||||
|
||||
|
||||
void request(CanardNodeID node_id, uint16_t index)
|
||||
{
|
||||
uavcan_register_List_Request_1_0 msg;
|
||||
msg.index = index;
|
||||
|
||||
uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer request = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindRequest,
|
||||
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = node_id,
|
||||
.transfer_id = list_request_transfer_id,
|
||||
.payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &request_payload_buffer,
|
||||
};
|
||||
|
||||
int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &request);
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
CanardInstance &_canard_instance;
|
||||
CanardTransferID list_request_transfer_id = 0;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 BaseSubscriber.hpp
|
||||
*
|
||||
* Defines basic functionality of UAVCAN v1 subscriber class
|
||||
*
|
||||
* @author Jacob Crabill <jacob@flyvoly.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <lib/parameters/param.h>
|
||||
|
||||
#include "../CanardInterface.hpp"
|
||||
|
||||
class UavcanBaseSubscriber
|
||||
{
|
||||
public:
|
||||
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
|
||||
|
||||
UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) :
|
||||
_canard_instance(ins), _subject_name(subject_name), _instance(instance) { };
|
||||
|
||||
virtual void subscribe() = 0;
|
||||
virtual void unsubscribe() { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, _port_id); };
|
||||
|
||||
virtual void callback(const CanardTransfer &msg) = 0;
|
||||
|
||||
CanardPortID id() { return _port_id; };
|
||||
|
||||
void printInfo()
|
||||
{
|
||||
if (_port_id != CANARD_PORT_ID_UNSET) {
|
||||
PX4_INFO("Subscribed %s.%d on port %d", _subject_name, _instance, _port_id);
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
CanardInstance &_canard_instance;
|
||||
CanardRxSubscription _canard_sub;
|
||||
const char *_subject_name;
|
||||
uint8_t _instance {0};
|
||||
/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
|
||||
|
||||
CanardPortID _port_id {CANARD_PORT_ID_UNSET};
|
||||
};
|
||||
|
|
@ -45,13 +45,13 @@
|
|||
#include <reg/drone/service/battery/Parameters_0_1.h>
|
||||
#include <reg/drone/service/battery/Status_0_1.h>
|
||||
|
||||
#include "Subscriber.hpp"
|
||||
#include "DynamicPortSubscriber.hpp"
|
||||
|
||||
class UavcanBmsSubscriber : public UavcanSubscriber
|
||||
class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||
UavcanSubscriber(ins, pmgr, "bms", instance) { };
|
||||
UavcanDynamicPortSubscriber(ins, pmgr, "bms", instance) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
|
|
|
|||
|
|
@ -32,9 +32,9 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Subscriber.hpp
|
||||
* @file DynamicPortSubscriber.hpp
|
||||
*
|
||||
* Defines basic functionality of UAVCAN v1 subscriber class
|
||||
* Defines basic functionality of UAVCAN v1 subscriber class with Non-fixed unregulated port identifier
|
||||
*
|
||||
* @author Jacob Crabill <jacob@flyvoly.com>
|
||||
*/
|
||||
|
|
@ -47,23 +47,16 @@
|
|||
|
||||
#include <uavcan/_register/Access_1_0.h>
|
||||
|
||||
#include "DynamicPortSubscriber.hpp"
|
||||
#include "../CanardInterface.hpp"
|
||||
#include "../ParamManager.hpp"
|
||||
|
||||
class UavcanSubscriber
|
||||
class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
|
||||
{
|
||||
public:
|
||||
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
|
||||
|
||||
UavcanSubscriber(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 void subscribe() = 0;
|
||||
virtual void unsubscribe() { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, _port_id); };
|
||||
|
||||
virtual void callback(const CanardTransfer &msg) = 0;
|
||||
|
||||
CanardPortID id() { return _port_id; };
|
||||
UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name,
|
||||
uint8_t instance = 0) :
|
||||
UavcanBaseSubscriber(ins, subject_name, instance), _param_manager(pmgr) { };
|
||||
|
||||
void updateParam()
|
||||
{
|
||||
|
|
@ -94,20 +87,6 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
void printInfo()
|
||||
{
|
||||
if (_port_id != CANARD_PORT_ID_UNSET) {
|
||||
PX4_INFO("Subscribed %s.%d on port %d", _subject_name, _instance, _port_id);
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
CanardInstance &_canard_instance;
|
||||
UavcanParamManager &_param_manager;
|
||||
CanardRxSubscription _canard_sub;
|
||||
const char *_subject_name;
|
||||
uint8_t _instance {0};
|
||||
/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
|
||||
|
||||
CanardPortID _port_id {CANARD_PORT_ID_UNSET};
|
||||
};
|
||||
|
|
@ -49,13 +49,13 @@
|
|||
#include <reg/drone/service/actuator/common/sp/Vector8_0_1.h>
|
||||
#include <reg/drone/service/common/Readiness_0_1.h>
|
||||
|
||||
#include "Subscriber.hpp"
|
||||
#include "DynamicPortSubscriber.hpp"
|
||||
|
||||
class UavcanEscSubscriber : public UavcanSubscriber
|
||||
class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanEscSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||
UavcanSubscriber(ins, pmgr, "esc", instance) { };
|
||||
UavcanDynamicPortSubscriber(ins, pmgr, "esc", instance) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
|
|
|
|||
|
|
@ -44,13 +44,13 @@
|
|||
// DS-15 Specification Messages
|
||||
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
|
||||
|
||||
#include "Subscriber.hpp"
|
||||
#include "DynamicPortSubscriber.hpp"
|
||||
|
||||
class UavcanGnssSubscriber : public UavcanSubscriber
|
||||
class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanGnssSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||
UavcanSubscriber(ins, pmgr, "gps", instance) { };
|
||||
UavcanDynamicPortSubscriber(ins, pmgr, "gps", instance) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
|
|
|
|||
|
|
@ -53,13 +53,13 @@
|
|||
#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"
|
||||
#include "BaseSubscriber.hpp"
|
||||
|
||||
class UavcanNodeIDAllocationDataSubscriber : public UavcanSubscriber
|
||||
class UavcanNodeIDAllocationDataSubscriber : public UavcanBaseSubscriber
|
||||
{
|
||||
public:
|
||||
UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, NodeManager &nmgr) :
|
||||
UavcanSubscriber(ins, pmgr, "NodeIDAllocationData", 0), _nmgr(nmgr) { };
|
||||
UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, NodeManager &nmgr) :
|
||||
UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0), _nmgr(nmgr) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
|
|||
publisher->updateParam();
|
||||
}
|
||||
|
||||
for (auto &subscriber : _subscribers) {
|
||||
for (auto &subscriber : _dynsubscribers) {
|
||||
subscriber->updateParam();
|
||||
}
|
||||
|
||||
|
|
@ -168,20 +168,6 @@ void UavcanNode::init()
|
|||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_heartbeat_subscription);
|
||||
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindResponse,
|
||||
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
||||
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_register_access_subscription);
|
||||
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindResponse,
|
||||
uavcan_register_List_1_0_FIXED_PORT_ID_,
|
||||
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_register_list_subscription);
|
||||
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindMessage,
|
||||
bms_port_id,
|
||||
|
|
@ -196,6 +182,9 @@ void UavcanNode::init()
|
|||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_drone_srv_gps_subscription);
|
||||
|
||||
for (auto &subscriber : _subscribers) {
|
||||
subscriber->subscribe();
|
||||
}
|
||||
|
||||
_initialized = true;
|
||||
}
|
||||
|
|
@ -231,7 +220,7 @@ void UavcanNode::Run()
|
|||
publisher->updateParam();
|
||||
}
|
||||
|
||||
for (auto &subscriber : _subscribers) {
|
||||
for (auto &subscriber : _dynsubscribers) {
|
||||
// Have the subscriber update its associated port-id parameter
|
||||
// If the port-id changes, (re)start the subscription
|
||||
// Setting the port-id to 0 disables the subscription
|
||||
|
|
@ -246,40 +235,6 @@ void UavcanNode::Run()
|
|||
perf_begin(_cycle_perf);
|
||||
perf_count(_interval_perf);
|
||||
|
||||
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_manager._node_register_setup,
|
||||
_node_manager._node_register_request_index);
|
||||
|
||||
uavcan_register_List_Request_1_0 msg;
|
||||
msg.index = _node_manager._node_register_request_index;
|
||||
|
||||
uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer request = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindRequest,
|
||||
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.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,
|
||||
};
|
||||
|
||||
int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++_uavcan_register_list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &request);
|
||||
|
||||
++_node_manager._node_register_request_index;
|
||||
}
|
||||
}
|
||||
|
||||
// send uavcan::node::Heartbeat_1_0 @ 1 Hz
|
||||
sendHeartbeat();
|
||||
|
||||
|
|
@ -288,6 +243,8 @@ void UavcanNode::Run()
|
|||
publisher->update();
|
||||
}
|
||||
|
||||
_node_manager.update();
|
||||
|
||||
// Transmitting
|
||||
// Look at the top of the TX queue.
|
||||
for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
|
||||
|
|
@ -340,13 +297,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_register_List_1_0_FIXED_PORT_ID_) {
|
||||
result = handleRegisterList(receive);
|
||||
|
||||
} else if (receive.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
|
||||
result = handleRegisterAccess(receive);
|
||||
|
||||
} else if (receive.port_id == bms_port_id) {
|
||||
if (receive.port_id == bms_port_id) {
|
||||
result = handleBMSStatus(receive);
|
||||
|
||||
} else if (receive.port_id == gps_port_id) {
|
||||
|
|
@ -359,6 +310,12 @@ void UavcanNode::Run()
|
|||
subscriber->callback(receive);
|
||||
}
|
||||
}
|
||||
|
||||
for (auto &subscriber : _dynsubscribers) {
|
||||
if (receive.port_id == subscriber->id()) {
|
||||
subscriber->callback(receive);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Deallocate the dynamic memory afterwards.
|
||||
|
|
@ -492,176 +449,6 @@ void UavcanNode::sendHeartbeat()
|
|||
}
|
||||
}
|
||||
|
||||
int UavcanNode::handleRegisterList(const CanardTransfer &receive)
|
||||
{
|
||||
uavcan_register_List_Response_1_0 msg;
|
||||
|
||||
size_t register_in_size_bits = receive.payload_size;
|
||||
uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits);
|
||||
|
||||
int result;
|
||||
|
||||
if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id",
|
||||
msg.name.name.count) == 0) { //Demo GPS status publisher
|
||||
_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_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));
|
||||
|
||||
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
|
||||
request_msg.value.natural16.value.count = 1;
|
||||
request_msg.value.natural16.value.elements[0] = gps_port_id;
|
||||
|
||||
|
||||
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindRequest,
|
||||
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
|
||||
.transfer_id = _uavcan_register_access_request_transfer_id,
|
||||
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &request_payload_buffer,
|
||||
};
|
||||
|
||||
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
|
||||
} else if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id",
|
||||
msg.name.name.count) == 0) { //Battery status publisher
|
||||
_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_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));
|
||||
|
||||
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
|
||||
request_msg.value.natural16.value.count = 1;
|
||||
request_msg.value.natural16.value.elements[0] = bms_port_id;
|
||||
|
||||
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindRequest,
|
||||
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
|
||||
.transfer_id = _uavcan_register_access_request_transfer_id,
|
||||
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &request_payload_buffer,
|
||||
};
|
||||
|
||||
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
|
||||
} else {
|
||||
uavcan_register_Value_1_0 out_value;
|
||||
_node_manager._node_register_last_received_index++;
|
||||
_node_manager._uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); // Reset timer for next request
|
||||
|
||||
if (_param_manager.GetParamByName(msg.name, out_value)) {
|
||||
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
|
||||
|
||||
uavcan_register_Access_Request_1_0 request_msg;
|
||||
memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
|
||||
|
||||
uavcan_register_Value_1_0_select_natural16_(&request_msg.value); /// TODO don't select; see which it is
|
||||
request_msg.value.natural16.value.count = 1;
|
||||
request_msg.value.natural16.value.elements[0] = out_value.natural16.value.elements[0]; /// TODO
|
||||
|
||||
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindRequest,
|
||||
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
|
||||
.transfer_id = _uavcan_register_access_request_transfer_id,
|
||||
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &request_payload_buffer,
|
||||
};
|
||||
|
||||
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int UavcanNode::handleRegisterAccess(const CanardTransfer &receive)
|
||||
{
|
||||
uavcan_register_Access_Request_1_0 msg {};
|
||||
|
||||
size_t register_in_size_bits = receive.payload_size;
|
||||
uavcan_register_Access_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits);
|
||||
|
||||
int result {0};
|
||||
|
||||
uavcan_register_Value_1_0 value = msg.value;
|
||||
uavcan_register_Name_1_0 name = msg.name;
|
||||
|
||||
/// TODO: get/set parameter based on whether empty or not
|
||||
if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0
|
||||
// Value is empty -- 'Get' only
|
||||
result = _param_manager.GetParamByName(name, value) ? 0 : -1;
|
||||
|
||||
} else {
|
||||
// Set value
|
||||
result = _param_manager.SetParamByName(name, value) ? 0 : -1;
|
||||
|
||||
}
|
||||
|
||||
/// TODO: Access_Response
|
||||
uavcan_register_Access_Response_1_0 response {};
|
||||
response.value = value;
|
||||
|
||||
uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindResponse,
|
||||
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
|
||||
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
|
||||
.transfer_id = _uavcan_register_access_request_transfer_id, /// TODO: track register Access _response_ separately?
|
||||
.payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &response_payload_buffer,
|
||||
};
|
||||
|
||||
result = uavcan_register_Access_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
|
||||
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int UavcanNode::handleBMSStatus(const CanardTransfer &receive)
|
||||
{
|
||||
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
|
||||
|
|
|
|||
|
|
@ -71,12 +71,18 @@
|
|||
#include "Publishers/Publisher.hpp"
|
||||
#include "Publishers/Gnss.hpp"
|
||||
|
||||
#include "Subscribers/Subscriber.hpp"
|
||||
#include "Subscribers/BaseSubscriber.hpp"
|
||||
#include "Subscribers/Battery.hpp"
|
||||
#include "Subscribers/Esc.hpp"
|
||||
#include "Subscribers/Gnss.hpp"
|
||||
#include "Subscribers/NodeIDAllocationData.hpp"
|
||||
|
||||
#include "ServiceClients/GetInfo.hpp"
|
||||
#include "ServiceClients/Access.hpp"
|
||||
|
||||
#include "Services/AccessReply.hpp"
|
||||
#include "Services/ListReply.hpp"
|
||||
|
||||
#include "NodeManager.hpp"
|
||||
|
||||
#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
|
||||
|
|
@ -239,10 +245,17 @@ 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};
|
||||
UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};
|
||||
|
||||
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
|
||||
UavcanAccessResponse _access_rsp {_canard_instance, _param_manager};
|
||||
|
||||
UavcanAccessServiceReply _access_service {_canard_instance, _node_manager};
|
||||
UavcanListServiceReply _list_service {_canard_instance, _node_manager};
|
||||
|
||||
// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
|
||||
UavcanSubscriber *_subscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub, &_nodeid_sub}; /// TODO: turn into List<UavcanSubscription*>
|
||||
UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List<UavcanSubscription*>
|
||||
UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List<UavcanSubscription*>
|
||||
|
||||
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue