Skip to content

Commit

Permalink
"MAVCAN" PNP & Register example implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
@PetervdPerk-NXP @LorenzMeier
PetervdPerk-NXP authored and LorenzMeier committed Feb 7, 2021
1 parent e5d29d4 commit 8b2d20d
Show file tree
Hide file tree
Showing 8 changed files with 213 additions and 79 deletions.
4 changes: 2 additions & 2 deletions .gitmodules
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,10 @@
branch = master
[submodule "src/drivers/uavcan_v1/libcanard"]
path = src/drivers/uavcan_v1/libcanard
url = https://github.com/PX4/libcanard.git
url = https://github.com/UAVCAN/libcanard.git
[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
path = src/drivers/uavcan_v1/public_regulated_data_types
url = https://github.com/PX4/public_regulated_data_types.git
url = https://github.com/UAVCAN/public_regulated_data_types.git
[submodule "src/modules/micrortps_bridge/micro-CDR"]
path = src/modules/micrortps_bridge/micro-CDR
url = https://github.com/PX4/Micro-CDR.git
Expand Down
Binary file added src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp
View file
Binary file not shown.
4 changes: 2 additions & 2 deletions src/drivers/uavcan_v1/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,8 @@ px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR})

find_program(NNVG_PATH nnvg)
if(NNVG_PATH)
execute_process(COMMAND ${NNVG_PATH} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/regulated)
execute_process(COMMAND ${NNVG_PATH} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp ${DSDL_DIR}/uavcan)
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg)
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan)
else()
message(FATAL_ERROR "UAVCAN Nunavut nnvg not found")
endif()
Expand Down
4 changes: 2 additions & 2 deletions src/drivers/uavcan_v1/CanardSocketCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,12 +162,12 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
_send_tv->tv_usec = txf.timestamp_usec % 1000000ULL;
_send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL;

return sendmsg(_fd, &_send_msg, 0);
return sendmsg(_fd, &_send_msg, 1000);
}

int16_t CanardSocketCAN::receive(CanardFrame *rxf)
{
int32_t result = recvmsg(_fd, &_recv_msg, 0);
int32_t result = recvmsg(_fd, &_recv_msg, 1000);

if (result < 0) {
return result;
Expand Down
232 changes: 168 additions & 64 deletions src/drivers/uavcan_v1/Uavcan.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,6 @@
#include <lib/ecl/geo/geo.h>
#include <lib/version/version.h>

#define REGULATED_DRONE_SENSOR_BMSSTATUS_ID 32080

using namespace time_literals;

UavcanNode *UavcanNode::_instance;
Expand Down Expand Up @@ -153,19 +151,41 @@ void UavcanNode::Run()
// Subscribe to messages uavcan.node.Heartbeat.
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan::node::Heartbeat_1_0::PORT_ID,
uavcan::node::Heartbeat_1_0::SIZE,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);

if (_param_uavcan_v1_bat_md.get() == 1) {
canardRxSubscribe(&_canard_instance,

// Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_pnp_v1_subscription);

canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
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,
static_cast<CanardPortID>(_param_uavcan_v1_bat_id.get()),
regulated::drone::sensor::BMSStatus_1_0::SIZE,
test_port_id,
reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_drone_sensor_BMSStatus_subscription);
}
&_drone_srv_battery_subscription);

_initialized = true;
}
Expand Down Expand Up @@ -193,27 +213,61 @@ void UavcanNode::Run()



if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s &&
_node_register_setup != CANARD_NODE_ID_UNSET &&
_node_register_request_index == _node_register_last_received_index+1){

PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index);

uavcan_register_List_Request_1_0 msg;
msg.index = _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_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_register_request_index;
}
}

// send uavcan::node::Heartbeat_1_0 @ 1 Hz
if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) {

uavcan::node::Heartbeat_1_0 heartbeat{};
uavcan_node_Heartbeat_1_0 heartbeat{};
heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime
heartbeat.health = uavcan::node::Heartbeat_1_0::HEALTH_NOMINAL;
heartbeat.mode = uavcan::node::Heartbeat_1_0::MODE_OPERATIONAL;
heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;

heartbeat.serializeToBuffer(_uavcan_node_heartbeat_buffer);

const CanardTransfer transfer = {
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(),
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = uavcan::node::Heartbeat_1_0::PORT_ID,
.port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _uavcan_node_heartbeat_transfer_id++,
.payload_size = uavcan::node::Heartbeat_1_0::SIZE,
.payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &_uavcan_node_heartbeat_buffer,
};

uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size);

int32_t result = canardTxPush(&_canard_instance, &transfer);

if (result < 0) {
Expand All @@ -226,49 +280,11 @@ void UavcanNode::Run()
_uavcan_node_heartbeat_last = transfer.timestamp_usec;
}

// send regulated::drone::sensor::BMSStatus_1_0 @ 1 Hz
if (_param_uavcan_v1_bat_md.get() == 2) {
if (hrt_elapsed_time(&_regulated_drone_sensor_bmsstatus_last) >= 1_s) {

battery_status_s battery_status;

if (_battery_status_sub.update(&battery_status)) {
regulated::drone::sensor::BMSStatus_1_0 bmsstatus{};
//bmsstatus.timestamp = battery_status.timestamp;
//bmsstatus.remaining_capacity = battery_status.remaining;

bmsstatus.serializeToBuffer(_regulated_drone_sensor_bmsstatus_buffer);

const CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(),
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = static_cast<CanardPortID>(_param_uavcan_v1_bat_id.get()),
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _regulated_drone_sensor_bmsstatus_transfer_id++,
.payload_size = regulated::drone::sensor::BMSStatus_1_0::SIZE,
.payload = &_regulated_drone_sensor_bmsstatus_buffer,
};

int32_t result = canardTxPush(&_canard_instance, &transfer);

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.
PX4_ERR("Battery transmit error %d", result);
}

_regulated_drone_sensor_bmsstatus_last = transfer.timestamp_usec;
}
}
}

// Transmitting
// Look at the top of the TX queue.
for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
// Check if the frame has timed out.
if (hrt_absolute_time() > txf->timestamp_usec) {
if (hrt_absolute_time() > txf->timestamp_usec) { //FIXME wrong I think
// Send the frame. Redundant interfaces may be used here.
const int tx_res = _can_interface->transmit(*txf);

Expand Down Expand Up @@ -312,23 +328,111 @@ void UavcanNode::Run()

} else if (result == 1) {
// A transfer has been received, process it.
PX4_DEBUG("received Port ID: %d", receive.port_id);

if (receive.port_id == static_cast<CanardPortID>(_param_uavcan_v1_bat_id.get())) {
auto bms_status = regulated::drone::sensor::BMSStatus_1_0::deserializeFromBuffer((const uint8_t *)receive.payload,
receive.payload_size);
PX4_INFO("received Port ID: %d", receive.port_id);

if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
uavcan_pnp_NodeIDAllocationData_1_0 msg;

size_t pnp_in_size_bits = receive.payload_size;
uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t*)receive.payload, &pnp_in_size_bits);

//TODO internal database with unique id to node ip mappings now we give an hardcoded ID back

msg.allocated_node_id.count = 1;
msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID

_uavcan_pnp_nodeidallocation_last = hrt_absolute_time();
_node_register_request_index = 0;
_node_register_last_received_index = -1;
_node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured

PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);

uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];

CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id,
.payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &node_id_alloc_payload_buffer,
};

result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);

if(result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
} if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
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(strncmp((char*)msg.name.name.elements, "uavcan.pub.battery_status.id", msg.name.name.count) == 0) { //Battery status publisher
_node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, test_port_id);
_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] = test_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 (receive.port_id == test_port_id) {
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
//TODO deserialize

/*
battery_status_s battery_status{};
battery_status.id = bms_status.battery_id;
battery_status.voltage_v = bms_status.voltage;
//battery_status.remaining = bms_status.remaining_capacity;
battery_status.timestamp = hrt_absolute_time();
_battery_status_pub.publish(battery_status);
_battery_status_pub.publish(battery_status);*/
}

// Deallocate the dynamic memory afterwards.
_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
}
} else {
PX4_INFO("RX canard %d\r\n", result);
}
}

perf_end(_cycle_perf);
Expand Down
Loading

0 comments on commit 8b2d20d

Please sign in to comment.