yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

UAVCANv1 Iteration, dynamic subscribers and decoupling (#17624)

Browse Source 
* Update to use libcanard callback
Reworked param system with lambda

* Use callback instead of lambda saves 500 bytes of flash

* [UAVCANv1] Added ServiceRequest class, subscribers decoupled

* PX4 UAVCANv1 portID param default CANARD_PORT_ID_UNSET (65535U)

* [UAVCANv1] uORB sensor_gps modify timestamp for now untill we've got timesync working

* UAVCANv1 update parameter definition to use -1 as unset
This commit is contained in:
Peter van der Perk
2021-06-04 15:30:17 +02:00
committed by GitHub
parent 470c3cfa6e
commit 9756c70491
24 changed files with 591 additions and 410 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -76,6 +76,8 @@ px4_add_module(
${LIBCANARD_DIR}/libcanard/
${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
SRCS
SubscriptionManager.cpp
SubscriptionManager.hpp
ParamManager.hpp
ParamManager.cpp
NodeManager.hpp

38
src/drivers/uavcan_v1/NodeManager.cpp
View File

@@ -43,6 +43,26 @@
#include "NodeManager.hpp"
void NodeManager::callback(const CanardTransfer &receive)
{
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
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
HandleNodeIDRequest(node_id_alloc_msg);
}
}
bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
{
if (msg.allocated_node_id.count == 0) {
@@ -105,34 +125,38 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
return false;
}
bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg)
{
//TODO V2 CAN FD implementation
//TODO v2 node id
return false;
}
void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg)
void NodeManager::HandleListResponse(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, &register_in_size_bits);
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) {
if (nodeid_registry[i].node_id == receive.remote_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) {
if (nodeid_registry[i].node_id == receive.remote_node_id) {
nodeid_registry[i].register_index++; // Increment index counter for next update()
nodeid_registry[i].register_setup = false;
nodeid_registry[i].retry_count = 0;
}
}
if (_access_request.setPortId(node_id, msg.name)) {
if (_access_request.setPortId(receive.remote_node_id, msg.name, NULL)) { //FIXME confirm handler
PX4_INFO("Set portID succesfull");
} else {
@@ -147,7 +171,7 @@ void NodeManager::update()
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);
_list_request.getIndex(nodeid_registry[i].node_id, nodeid_registry[i].register_index, this);
nodeid_registry[i].retry_count++;
if (nodeid_registry[i].retry_count > RETRY_COUNT) {

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

@@ -51,13 +51,14 @@
#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?
#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_
typedef struct {
uint8_t node_id;
uint8_t unique_id[16];
@@ -66,17 +67,35 @@ typedef struct {
uint16_t retry_count;
} UavcanNodeEntry;
class NodeManager
class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface
{
public:
NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _access_request(ins, pmgr),
_list_request(ins) { };
NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
_canard_instance(ins), _access_request(ins, pmgr), _list_request(ins) { };
void subscribe() override
{
_access_request.subscribe();
_list_request.subscribe();
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,
&_subj_sub._canard_sub);
}
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
void response_callback(const CanardTransfer &receive) override
{
HandleListResponse(receive);
}
void callback(const CanardTransfer &receive); // NodeIDAllocation callback
void HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg);
void HandleListResponse(const CanardTransfer &receive);
void update();

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

@@ -52,34 +52,7 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
return false;
}
/// TODO: What will be our approach for handling other UAVCAN data-value types?
switch (param_type(param_handle)) {
case PARAM_TYPE_INT32: {
int32_t out_val {};
param_get(param_handle, &out_val);
if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
value.natural16.value.elements[0] = (uint16_t)out_val;
uavcan_register_Value_1_0_select_natural16_(&value);
} else {
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
}
break;
}
case PARAM_TYPE_FLOAT: {
float out_val {};
param_get(param_handle, &out_val);
value.real32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_real32_(&value);
break;
}
}
return true;
return param.px4_param_to_register_value(param_handle, value);
}
}
@@ -96,34 +69,7 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
return false;
}
/// TODO: What will be our approach for handling other UAVCAN data-value types?
switch (param_type(param_handle)) {
case PARAM_TYPE_INT32: {
int32_t out_val {};
param_get(param_handle, &out_val);
if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
value.natural16.value.elements[0] = (uint16_t)out_val;
uavcan_register_Value_1_0_select_natural16_(&value);
} else {
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
}
break;
}
case PARAM_TYPE_FLOAT: {
float out_val {};
param_get(param_handle, &out_val);
value.real32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_real32_(&value);
break;
}
}
return true;
return param.px4_param_to_register_value(param_handle, value);
}
}
@@ -140,21 +86,7 @@ bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, co
return false;
}
switch (param_type(param_handle)) {
case PARAM_TYPE_INT32: {
int32_t in_val = value.integer32.value.elements[0];
param_set(param_handle, &in_val);
break;
}
case PARAM_TYPE_FLOAT: {
float in_val = value.natural32.value.elements[0];
param_set(param_handle, &in_val);
break;
}
}
return true;
return param.register_value_to_px4_param(value, param_handle);
}
}

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

@@ -42,17 +42,52 @@
#pragma once
#include <lib/parameters/param.h>
#include <px4_platform_common/defines.h>
#include <uavcan/_register/Name_1_0.h>
#include <uavcan/_register/Value_1_0.h>
static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out)
{
if (param_type(in) == PARAM_TYPE_INT32) {
int32_t out_val {};
param_get(in, &out_val);
out.natural16.value.elements[0] = (uint16_t)out_val;
out.natural16.value.count = 1;
uavcan_register_Value_1_0_select_natural16_(&out);
return true;
}
return false;
};
static bool uavcan_port_id_to_px4_param(const uavcan_register_Value_1_0 &in, param_t &out)
{
if (uavcan_register_Value_1_0_is_natural16_(&in) && in.natural16.value.count == 1) {
if (param_type(out) == PARAM_TYPE_INT32) {
int32_t in_val = in.natural16.value.elements[0];
param_set(out, &in_val);
return true;
}
}
return false;
};
using param_2_reg_t = bool(*)(param_t &in, uavcan_register_Value_1_0 &out);
using reg_2_param_t = bool(*)(const uavcan_register_Value_1_0 &in, param_t &out);
typedef struct {
const char *uavcan_name;
const char *px4_name;
param_2_reg_t px4_param_to_register_value;
reg_2_param_t register_value_to_px4_param;
bool is_mutable {true};
bool is_persistent {true};
} UavcanParamBinder;
class UavcanParamManager
{
public:
@@ -63,18 +98,19 @@ public:
private:
const UavcanParamBinder _uavcan_params[11] {
{"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"},
{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"},
{"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"},
{"uavcan.sub.esc.0.id", "UCAN1_ESC0_PID"},
{"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"},
{"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"},
{"uavcan.sub.energy_source.0.id", "UCAN1_BMS_ES_PID"},
{"uavcan.sub.battery_status.0.id", "UCAN1_BMS_BS_PID"},
{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID"},
{"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_PID"},
{"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS"},
{"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.esc.0.id", "UCAN1_ESC0_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.energy_source.0.id", "UCAN1_BMS_ES_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.battery_status.0.id", "UCAN1_BMS_BS_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
//{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"}, //FIXME instancing
//{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"},
};

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

@@ -45,12 +45,11 @@
#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"
#include "../Publishers/Publisher.hpp"
class UavcanGetInfoResponse : public UavcanBaseSubscriber
{

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

@@ -47,15 +47,18 @@
#include <uavcan/_register/Access_1_0.h>
#include "ServiceRequest.hpp"
#include "../ParamManager.hpp"
#include "../Publishers/Publisher.hpp"
class UavcanAccessServiceRequest
class UavcanAccessServiceRequest : public UavcanServiceRequest
{
public:
UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
_canard_instance(ins), _param_manager(pmgr) { };
UavcanServiceRequest(ins, "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_), _param_manager(pmgr) { };
bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name)
bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler)
{
int result {0};
@@ -76,9 +79,9 @@ public:
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.port_id = _portID, // This is the subject-ID.
.remote_node_id = node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = access_request_transfer_id,
.transfer_id = request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
@@ -86,12 +89,11 @@ public:
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);
}
return request(&transfer, handler);
return result > 0;
} else {
return false;
}
} else {
return false;
@@ -99,8 +101,6 @@ public:
};
private:
CanardInstance &_canard_instance;
CanardTransferID access_request_transfer_id = 0;
UavcanParamManager &_param_manager;
};

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

@@ -47,44 +47,43 @@
#include <uavcan/_register/List_1_0.h>
#include "ServiceRequest.hpp"
#include "../ParamManager.hpp"
#include "../Publishers/Publisher.hpp"
class UavcanListServiceRequest
class UavcanListServiceRequest : public UavcanServiceRequest
{
public:
UavcanListServiceRequest(CanardInstance &ins) :
_canard_instance(ins) { };
UavcanServiceRequest(ins, "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_) { };
void request(CanardNodeID node_id, uint16_t index)
bool getIndex(CanardNodeID node_id, uint16_t index, UavcanServiceRequestInterface *handler)
{
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 = {
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
.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,
.transfer_id = 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 (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &transfer.payload_size) == 0) {
return request(&transfer, handler);
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);
} else {
return false;
}
};
private:
CanardInstance &_canard_instance;
CanardTransferID list_request_transfer_id = 0;
};

50
src/drivers/uavcan_v1/Services/ListReply.hpp → src/drivers/uavcan_v1/Services/ServiceRequest.hpp
View File

@@ -51,38 +51,54 @@
#include "../Subscribers/BaseSubscriber.hpp"
class UavcanListServiceReply : public UavcanBaseSubscriber
class UavcanServiceRequestInterface
{
public:
UavcanListServiceReply(CanardInstance &ins, NodeManager &nmgr) :
UavcanBaseSubscriber(ins, "List", 0), _nmgr(nmgr) { };
virtual void response_callback(const CanardTransfer &receive) = 0;
};
class UavcanServiceRequest : public UavcanBaseSubscriber
{
public:
UavcanServiceRequest(CanardInstance &ins, const char *subject_name, CanardPortID portID, size_t extent) :
UavcanBaseSubscriber(ins, subject_name, 0), _portID(portID), _extent(extent) { };
void subscribe() override
{
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
// Subscribe to requests response
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
_portID,
_extent,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
};
bool request(CanardTransfer *transfer, UavcanServiceRequestInterface *handler)
{
_response_callback = handler;
++request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
return canardTxPush(&_canard_instance, transfer) > 0;
}
void callback(const CanardTransfer &receive) override
{
PX4_INFO("List response");
PX4_INFO("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, &register_in_size_bits);
// Pass msg.name.name.elements to NodeManager
_nmgr.HandleListResponse(receive.remote_node_id, msg);
if (_response_callback != NULL) {
_response_callback->response_callback(receive);
}
};
private:
NodeManager &_nmgr;
protected:
CanardTransferID request_transfer_id = 0;
const CanardPortID _portID;
const size_t _extent;
UavcanServiceRequestInterface *_response_callback = NULL;
};

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

@@ -56,6 +56,12 @@ public:
_canard_instance(ins), _instance(instance)
{
_subj_sub._subject_name = subject_name;
_subj_sub._canard_sub.user_reference = this;
}
~UavcanBaseSubscriber()
{
unsubscribe();
}
virtual void subscribe() = 0;
@@ -64,7 +70,7 @@ public:
SubjectSubscription *curSubj = &_subj_sub;
while (curSubj != NULL) {
canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub._port_id);
canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub.port_id);
curSubj = curSubj->next;
}
};
@@ -78,7 +84,7 @@ public:
while (curSubj != NULL) {
if (instance == i) {
return curSubj->_canard_sub._port_id;
return curSubj->_canard_sub.port_id;
}
curSubj = curSubj->next;
@@ -93,7 +99,7 @@ public:
SubjectSubscription *curSubj = &_subj_sub;
while (curSubj != NULL) {
if (port_id == curSubj->_canard_sub._port_id) {
if (port_id == curSubj->_canard_sub.port_id) {
return true;
}
@@ -108,8 +114,8 @@ public:
SubjectSubscription *curSubj = &_subj_sub;
while (curSubj != NULL) {
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);
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);
}
curSubj = curSubj->next;

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

@@ -49,7 +49,7 @@
#include <reg/drone/service/battery/Parameters_0_3.h>
#include <reg/drone/service/battery/Status_0_2.h>
#include "DynamicPortSubscriber.hpp"
#include "../DynamicPortSubscriber.hpp"
#define KELVIN_OFFSET 273.15f
#define WH_TO_JOULE 3600
@@ -61,9 +61,13 @@ public:
UavcanDynamicPortSubscriber(ins, pmgr, "energy_source", instance)
{
_subj_sub.next = &_status_sub;
_status_sub._subject_name = _status_name;
_status_sub._canard_sub.user_reference = this;
_status_sub.next = &_parameters_sub;
_parameters_sub._subject_name = _parameters_name;
_parameters_sub._canard_sub.user_reference = this;
_parameters_sub.next = NULL;
}
@@ -72,7 +76,7 @@ public:
// Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub._port_id,
_subj_sub._canard_sub.port_id,
reg_drone_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
@@ -80,7 +84,7 @@ public:
// Subscribe to messages reg.drone.service.battery.Status.0.2
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_status_sub._canard_sub._port_id,
_status_sub._canard_sub.port_id,
reg_drone_service_battery_Status_0_2_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_status_sub._canard_sub);
@@ -88,7 +92,7 @@ public:
// Subscribe to messages reg.drone.service.battery.Parameters.0.3
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_parameters_sub._canard_sub._port_id,
_parameters_sub._canard_sub.port_id,
reg_drone_service_battery_Parameters_0_3_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_parameters_sub._canard_sub);
@@ -98,7 +102,7 @@ public:
{
PX4_INFO("BmsCallback");
if (receive.port_id == _subj_sub._canard_sub._port_id) {
if (receive.port_id == _subj_sub._canard_sub.port_id) {
reg_drone_physics_electricity_SourceTs_0_1 source_ts {};
size_t source_ts_size_in_bytes = receive.payload_size;
reg_drone_physics_electricity_SourceTs_0_1_deserialize_(&source_ts,
@@ -123,7 +127,7 @@ public:
print_message(bat_status);
} else if (receive.port_id == _status_sub._canard_sub._port_id) {
} else if (receive.port_id == _status_sub._canard_sub.port_id) {
reg_drone_service_battery_Status_0_2 bat {};
size_t bat_size_in_bytes = receive.payload_size;
reg_drone_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
@@ -157,7 +161,7 @@ public:
bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time?
} else if (receive.port_id == _parameters_sub._canard_sub._port_id) {
} else if (receive.port_id == _parameters_sub._canard_sub.port_id) {
reg_drone_service_battery_Parameters_0_3 parameters {};
size_t parameters_size_in_bytes = receive.payload_size;
reg_drone_service_battery_Parameters_0_3_deserialize_(&parameters,

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

@@ -49,7 +49,7 @@
#include <reg/drone/service/actuator/common/sp/Vector8_0_1.h>
#include <reg/drone/service/common/Readiness_0_1.h>
#include "DynamicPortSubscriber.hpp"
#include "../DynamicPortSubscriber.hpp"
class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
{
@@ -62,7 +62,7 @@ public:
// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub._port_id,
_subj_sub._canard_sub.port_id,
reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
@@ -70,7 +70,7 @@ public:
// Subscribe to messages reg.drone.service.common.Readiness.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub._port_id) + 1),
static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub.port_id) + 1),
reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_canard_sub_readiness);

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

@@ -44,7 +44,7 @@
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
#include "DynamicPortSubscriber.hpp"
#include "../DynamicPortSubscriber.hpp"
class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
{
@@ -57,7 +57,7 @@ public:
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub._port_id,
_subj_sub._canard_sub.port_id,
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);

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

@@ -47,9 +47,9 @@
#include <uavcan/_register/Access_1_0.h>
#include "DynamicPortSubscriber.hpp"
#include "../CanardInterface.hpp"
#include "../ParamManager.hpp"
#include "BaseSubscriber.hpp"
class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
{
@@ -73,20 +73,20 @@ public:
int32_t new_id = value.integer32.value.elements[0];
/* FIXME how about partial subscribing */
if (curSubj->_canard_sub._port_id != new_id) {
if (curSubj->_canard_sub.port_id != new_id) {
if (new_id == CANARD_PORT_ID_UNSET) {
// Cancel subscription
unsubscribe();
} else {
if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
// Already active; unsubscribe first
unsubscribe();
}
// Subscribe on the new port ID
curSubj->_canard_sub._port_id = (CanardPortID)new_id;
PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
curSubj->_canard_sub.port_id = (CanardPortID)new_id;
PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
subscribe();
}
}
@@ -96,6 +96,17 @@ public:
}
};
UavcanDynamicPortSubscriber *next()
{
return _next_sub;
}
void setNext(UavcanDynamicPortSubscriber *next)
{
_next_sub = next;
}
protected:
UavcanParamManager &_param_manager;
UavcanDynamicPortSubscriber *_next_sub {NULL};
};

31
src/drivers/uavcan_v1/Services/AccessReply.hpp → src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp
View File

@@ -32,38 +32,34 @@
****************************************************************************/
/**
* @file Access.hpp
* @file Heartbeat.hpp
*
* Defines a Access Service invoker and process Access responses
* Defines basic functionality of UAVCAN Heartbeat.1.0 subscription
*
* @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 <uavcan/node/Heartbeat_1_0.h>
#include "../Subscribers/BaseSubscriber.hpp"
#include "BaseSubscriber.hpp"
class UavcanAccessServiceReply : public UavcanBaseSubscriber
class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
{
public:
UavcanAccessServiceReply(CanardInstance &ins, NodeManager &nmgr) :
UavcanBaseSubscriber(ins, "Access", 0), _nmgr(nmgr) { };
UavcanHeartbeatSubscriber(CanardInstance &ins) :
UavcanBaseSubscriber(ins, "Heartbeat", 0) { };
void subscribe() override
{
// Subscribe to requests uavcan.pnp.NodeIDAllocationData
// Subscribe to heartbeat messages
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
CanardTransferKindMessage,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, // The fixed Subject-ID
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_subj_sub._canard_sub);
@@ -71,10 +67,7 @@ public:
void callback(const CanardTransfer &receive) override
{
PX4_INFO("Access response");
//TODO heartbeat management
};
private:
NodeManager &_nmgr;
};

102
src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp
View File

@@ -1,102 +0,0 @@
/****************************************************************************
*
* 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 "BaseSubscriber.hpp"
class UavcanNodeIDAllocationDataSubscriber : public UavcanBaseSubscriber
{
public:
UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, NodeManager &nmgr) :
UavcanBaseSubscriber(ins, "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,
&_subj_sub._canard_sub);
};
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;
};

4
src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp → src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp
View File

@@ -47,7 +47,7 @@
// Legacy message from UAVCANv0
#include <legacy/equipment/power/BatteryInfo_1_0.h>
#include "DynamicPortSubscriber.hpp"
#include "../DynamicPortSubscriber.hpp"
class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
{
@@ -60,7 +60,7 @@ public:
// Subscribe to messages reg.drone.service.battery.Status.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub._port_id,
_subj_sub._canard_sub.port_id,
legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler
&_subj_sub._canard_sub);

6
src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp
View File

@@ -57,7 +57,7 @@ public:
// Subscribe to messages uORB sensor_gps payload over UAVCAN
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub._port_id,
_subj_sub._canard_sub.port_id,
sizeof(struct sensor_gps_s),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
&_subj_sub._canard_sub);
@@ -69,6 +69,10 @@ public:
if (receive.payload_size == sizeof(struct sensor_gps_s)) {
sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
gps_msg->timestamp = hrt_absolute_time();
/* As long as we don't have timesync between nodes we set the timestamp to the current time */
_sensor_gps_pub.publish(*gps_msg);
} else {

87
src/drivers/uavcan_v1/SubscriptionManager.cpp Normal file
View File

@@ -0,0 +1,87 @@
/****************************************************************************
*
* 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 SubscriptionManager.cpp
*
* Manages the UAVCAN subscriptions
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#include "SubscriptionManager.hpp"
void SubscriptionManager::subscribe()
{
_heartbeat_sub.subscribe();
_getinfo_rsp.subscribe();
_access_rsp.subscribe();
for (auto &sub : _uavcan_subs) {
param_t param_handle = param_find(sub.px4_name);
if (param_handle == PARAM_INVALID) {
PX4_ERR("Param %s not found", sub.px4_name);
break;
}
if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
int32_t port_id {};
param_get(param_handle, &port_id);
if (port_id >= 0) { // PortID is set create a subscriber
UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager);
if (_dynsubscribers != NULL) {
_dynsubscribers->setNext(dynsub);
}
_dynsubscribers = dynsub;
dynsub->updateParam();
}
}
}
}
void SubscriptionManager::printInfo()
{
}
void SubscriptionManager::updateParams()
{
//TODO dynamically update params and unsubscribe
}

130
src/drivers/uavcan_v1/SubscriptionManager.hpp Normal file
View File

@@ -0,0 +1,130 @@
/****************************************************************************
*
* 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 SubscriptionManager.hpp
*
* Manages the UAVCAN subscriptions
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
#include "Subscribers/DynamicPortSubscriber.hpp"
#include "CanardInterface.hpp"
#include "ServiceClients/GetInfo.hpp"
#include "ServiceClients/Access.hpp"
#include "Subscribers/BaseSubscriber.hpp"
#include "Subscribers/Heartbeat.hpp"
#include "Subscribers/DS-015/Battery.hpp"
#include "Subscribers/DS-015/Esc.hpp"
#include "Subscribers/DS-015/Gnss.hpp"
#include "Subscribers/legacy/LegacyBatteryInfo.hpp"
#include "Subscribers/uORB/sensor_gps.hpp"
typedef struct {
const char *px4_name;
UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
} UavcanDynSubBinder;
class SubscriptionManager
{
public:
SubscriptionManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {}
void subscribe();
void printInfo();
void updateParams();
private:
CanardInstance &_canard_instance;
UavcanParamManager &_param_manager;
UavcanDynamicPortSubscriber *_dynsubscribers {NULL};
UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance};
// GetInfo response
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
// Process register requests
UavcanAccessResponse _access_rsp {_canard_instance, _param_manager};
const UavcanDynSubBinder _uavcan_subs[6] {
{
"UCAN1_ESC0_PID",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanEscSubscriber(ins, pmgr, 0);
}
},
{
"UCAN1_GPS0_PID",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanGnssSubscriber(ins, pmgr, 1);
}
},
{
"UCAN1_GPS1_PID",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanGnssSubscriber(ins, pmgr, 0);
}
},
{
"UCAN1_BMS_ES_PID",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanBmsSubscriber(ins, pmgr, 0);
}
},
{
"UCAN1_LG_BMS_PID",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0);
}
},
{
"UCAN1_UORB_GPS",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);
}
},
};
};

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

@@ -82,13 +82,13 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
PX4_INFO("main _canard_instance = %p", &_canard_instance);
_node_manager.subscribe();
for (auto &publisher : _publishers) {
publisher->updateParam();
}
for (auto &subscriber : _dynsubscribers) {
subscriber->updateParam();
}
_sub_manager.subscribe();
_mixing_output.mixingOutput().updateSubscriptions(false, false);
}
@@ -156,21 +156,6 @@ void UavcanNode::init()
// interface init
if (_can_interface) {
if (_can_interface->init() == PX4_OK) {
// We can't accept just any message; we must fist subscribe to specific IDs
// Subscribe to messages uavcan.node.Heartbeat.
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
for (auto &subscriber : _subscribers) {
subscriber->subscribe();
}
_initialized = true;
}
}
@@ -205,12 +190,7 @@ void UavcanNode::Run()
publisher->updateParam();
}
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
subscriber->updateParam();
}
_sub_manager.updateParams();
_mixing_output.updateParams();
@@ -240,7 +220,8 @@ void UavcanNode::Run()
while (_can_interface->receive(&received_frame) > 0) {
CanardTransfer receive{};
int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive);
CanardRxSubscription *subscription = NULL;
int32_t result = canardRxAccept2(&_canard_instance, &received_frame, 0, &receive, &subscription);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
@@ -253,19 +234,12 @@ void UavcanNode::Run()
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id > 0) {
// If not a fixed port ID, check any subscribers which may have registered it
for (auto &subscriber : _subscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
if (subscription != NULL) {
UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
sub_instance->callback(receive);
for (auto &subscriber : _dynsubscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
} else {
PX4_ERR("No matching sub for %d", receive.port_id);
}
// Deallocate the dynamic memory afterwards.
@@ -342,12 +316,43 @@ void UavcanNode::print_info()
publisher->printInfo();
}
for (auto &subscriber : _subscribers) {
subscriber->printInfo();
CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage];
while (rxs != NULL) {
if (rxs->user_reference == NULL) {
PX4_INFO("Message port id %d", rxs->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
}
rxs = rxs->next;
}
for (auto &subscriber : _dynsubscribers) {
subscriber->printInfo();
rxs = _canard_instance.rx_subscriptions[CanardTransferKindRequest];
while (rxs != NULL) {
if (rxs->user_reference == NULL) {
PX4_INFO("Service response port id %d", rxs->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
}
rxs = rxs->next;
}
rxs = _canard_instance.rx_subscriptions[CanardTransferKindResponse];
while (rxs != NULL) {
if (rxs->user_reference == NULL) {
PX4_INFO("Service request port id %d", rxs->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
}
rxs = rxs->next;
}
_mixing_output.printInfo();

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

@@ -55,40 +55,15 @@
#include <canard.h>
#include <canard_dsdl.h>
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h>
#include <uavcan/node/Heartbeat_1_0.h>
#include <uavcan/primitive/Empty_1_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>
#include "CanardInterface.hpp"
#include "Publishers/Publisher.hpp"
#include "Publishers/Gnss.hpp"
#include "Subscribers/BaseSubscriber.hpp"
#include "Subscribers/Battery.hpp"
#include "Subscribers/Esc.hpp"
#include "Subscribers/Gnss.hpp"
#include "Subscribers/NodeIDAllocationData.hpp"
#include "Subscribers/LegacyBatteryInfo.hpp"
// uORB over UAVCAN subscribers
#include "Subscribers/uORB/sensor_gps.hpp"
#include "ServiceClients/GetInfo.hpp"
#include "ServiceClients/Access.hpp"
#include "Services/AccessReply.hpp"
#include "Services/ListReply.hpp"
#include "NodeManager.hpp"
#include "SubscriptionManager.hpp"
#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
/**
@@ -184,8 +159,6 @@ private:
pthread_mutex_t _node_mutex;
CanardRxSubscription _heartbeat_subscription;
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
@@ -199,15 +172,15 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud,
(ParamInt<px4::params::UAVCAN_V1_BAT_MD>) _param_uavcan_v1_bat_md,
(ParamInt<px4::params::UAVCAN_V1_BAT_ID>) _param_uavcan_v1_bat_id
(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud
)
UavcanParamManager _param_manager;
NodeManager _node_manager {_canard_instance, _param_manager};
SubscriptionManager _sub_manager {_canard_instance, _param_manager};
UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};
UavcanEscController _esc_controller {_canard_instance, _param_manager};
@@ -216,26 +189,6 @@ private:
/// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers
UavcanPublisher *_publishers[2] {&_gps_pub, &_esc_controller};
UavcanGnssSubscriber _gps0_sub {_canard_instance, _param_manager, 0};
UavcanGnssSubscriber _gps1_sub {_canard_instance, _param_manager, 1};
UavcanBmsSubscriber _bms0_sub {_canard_instance, _param_manager, 0};
//UavcanBmsSubscriber _bms1_sub {_canard_instance, _param_manager, 1};
UavcanEscSubscriber _esc_sub {_canard_instance, _param_manager, 0};
UavcanLegacyBatteryInfoSubscriber _legacybms_sub {_canard_instance, _param_manager, 0};
UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};
UORB_over_UAVCAN_sensor_gps_Subscriber _sensor_gps_sub {_canard_instance, _param_manager, 0};
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
UavcanDynamicPortSubscriber *_dynsubscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub, &_sensor_gps_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};
};

2
src/drivers/uavcan_v1/libcanard

Submodule src/drivers/uavcan_v1/libcanard updated: 55938c51cf...2a11617028

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

@@ -66,43 +66,106 @@ PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1);
*/
PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000);
/**
* UAVCAN v1 battery mode.
*
* @value 0 Disable
* @value 1 Receive BMSStatus
* @value 2 Send BMSStatus
* @reboot_required true
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0);
/* Subscription port ID, -1 will be treated as unset */
/**
* UAVCAN v1 battery port ID.
* ESC0 port ID.
*
* @min 1
* @max 32767
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242);
PARAM_DEFINE_INT32(UCAN1_ESC0_PID, -1);
// Subscription Port IDs
PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0);
PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
//PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
//PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);
/**
* GPS 0 port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_GPS0_PID, -1);
PARAM_DEFINE_INT32(UCAN1_BMS_ES_PID, 0);
PARAM_DEFINE_INT32(UCAN1_BMS_BS_PID, 0);
PARAM_DEFINE_INT32(UCAN1_BMS_BP_PID, 0);
/**
* GPS 1 port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_GPS1_PID, -1);
PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, 0);
/**
* DS-015 battery energy source port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_BMS_ES_PID, -1);
/**
* DS-015 battery status port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_BMS_BS_PID, -1);
/**
* DS-015 battery parameters port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_BMS_BP_PID, -1);
/**
* UAVCAN v1 leagcy battery port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, -1);
/**
* sensor_gps uORB over UAVCAN v1 port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
// Publication Port IDs
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0);
PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0);
// uORB over UAVCAN
PARAM_DEFINE_INT32(UCAN1_UORB_GPS, 0);
/**
* UAVCAN v1 ESC port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
/**
* UAVCAN v1 GPS port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1);
/**
* UAVCAN v1 Servo port ID.
*
* @min -1
* @max 6143
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1);