mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-24 13:03:33 +08:00
UAVCAN v1 GPS demo with MAVCAN autoconfigure
This commit is contained in:
Peter van der Perk
committed by
Lorenz Meier
Lorenz Meier
parent
5f8ad65c40
commit
8242968b2b
+197
-146
@@ -155,37 +155,45 @@ void UavcanNode::Run()
|
||||
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_heartbeat_subscription);
|
||||
|
||||
// Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1
|
||||
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,
|
||||
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,
|
||||
test_port_id,
|
||||
reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_drone_srv_battery_subscription);
|
||||
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindMessage,
|
||||
bms_port_id,
|
||||
reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_drone_srv_battery_subscription);
|
||||
|
||||
canardRxSubscribe(&_canard_instance, //Temporory GPS message DSDL not defined yet
|
||||
CanardTransferKindMessage,
|
||||
gps_port_id,
|
||||
sizeof(struct sensor_gps_s),
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_drone_srv_gps_subscription);
|
||||
|
||||
|
||||
_initialized = true;
|
||||
}
|
||||
@@ -213,39 +221,39 @@ 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,
|
||||
};
|
||||
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) {
|
||||
|
||||
int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
|
||||
@@ -266,7 +274,7 @@ void UavcanNode::Run()
|
||||
.payload = &_uavcan_node_heartbeat_buffer,
|
||||
};
|
||||
|
||||
uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size);
|
||||
uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size);
|
||||
|
||||
int32_t result = canardTxPush(&_canard_instance, &transfer);
|
||||
|
||||
@@ -330,109 +338,152 @@ void UavcanNode::Run()
|
||||
// A transfer has been received, process it.
|
||||
PX4_INFO("received Port ID: %d", receive.port_id);
|
||||
|
||||
if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
|
||||
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,
|
||||
};
|
||||
if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
|
||||
uavcan_pnp_NodeIDAllocationData_1_0 msg;
|
||||
|
||||
result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);
|
||||
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(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, ®ister_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);
|
||||
uavcan_register_List_Response_1_0 msg;
|
||||
|
||||
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) {
|
||||
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);
|
||||
|
||||
|
||||
if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id",
|
||||
msg.name.name.count) == 0) { //Demo GPS status publisher
|
||||
_node_register_setup = CANARD_NODE_ID_UNSET;
|
||||
PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_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] = 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);
|
||||
}
|
||||
}
|
||||
|
||||
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, bms_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] = 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 if (receive.port_id == bms_port_id) {
|
||||
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
|
||||
//TODO deserialize
|
||||
|
||||
/*
|
||||
//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_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);*/
|
||||
|
||||
} else if (receive.port_id == gps_port_id) {
|
||||
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
|
||||
|
||||
sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
|
||||
|
||||
_sensor_gps_pub.publish(*gps_msg);
|
||||
}
|
||||
|
||||
// Deallocate the dynamic memory afterwards.
|
||||
_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
|
||||
|
||||
} else {
|
||||
PX4_INFO("RX canard %d\r\n", result);
|
||||
}
|
||||
//PX4_INFO("RX canard %d\r\n", result);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
|
||||
Reference in New Issue
Block a user