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

orb structs: add padding bytes to align the structs where necessary

Browse Source 
This is required for the logger, we just manually add the padding bytes
what would otherwise be done by the compiler. Additionally we reorder
the fields by type, so that padding is only necessary for nested types.
This commit is contained in:
Beat Küng 2016-05-05 14:10:23 +02:00 committed by Lorenz Meier
parent 7d4c4c0401
commit eabc43d78c
5 changed files with 152 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
msg/templates/uorb/msg.cpp.template
View File

@ -13,10 +13,12 @@
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - md5sum (String) MD5Sum of the .msg specification
@# - search_path (dict) search paths for genmsg
@# - topics (List of String) multi-topic names
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2016 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
@ -54,7 +56,6 @@ import genmsg.msgs
import gencpp
uorb_struct = '%s_s'%spec.short_name
uorb_pack_func = 'pack_%s'%spec.short_name
topic_name = spec.short_name
type_map = {
@ -70,9 +71,6 @@ type_map = {
'float64': 'double',
'bool': 'bool',
'char': 'char',
'fence_vertex': 'fence_vertex',
'position_setpoint': 'position_setpoint',
'esc_report': 'esc_report'
}
msgtype_size_map = {
@ -88,13 +86,8 @@ msgtype_size_map = {
'float64': 8,
'bool': 1,
'char': 1,
'fence_vertex': 8,
'position_setpoint': 104,
'esc_report': 36
}
msgtypes_composite = ['position_setpoint', 'esc_report', 'fence_vertex']
def convert_type(spec_type):
bare_type = spec_type
if '/' in spec_type:
@ -102,7 +95,9 @@ def convert_type(spec_type):
bare_type = (spec_type.split('/'))[1]
msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
c_type = type_map[msg_type]
c_type = msg_type
if msg_type in type_map:
c_type = type_map[msg_type]
if is_array:
return c_type + "[" + str(array_length) + "]"
return c_type
@ -116,9 +111,65 @@ def bare_name(msg_type):
return bare.split('[')[0]
def sizeof_field_type(field):
return msgtype_size_map[bare_name(field.type)]
bare_name_str = bare_name(field.type)
if bare_name_str in msgtype_size_map:
return msgtype_size_map[bare_name_str]
return 0 # this is for non-builtin types: sort them at the end
def get_children_fields(base_type):
(package, name) = genmsg.names.package_resource_name(base_type)
tmp_msg_context = genmsg.msg_loader.MsgContext.create_default()
spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path)
sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True)
return sorted_fields
# Add padding fields before the embedded types, at the end and calculate the
# struct size
def add_padding_bytes(fields):
struct_size = 8 # account for the timestamp
align_to = 8 # this is always 8, because of the 64bit timestamp
i = 0
padding_idx = 0
while i < len(fields):
field = fields[i]
if not field.is_header:
a_pos = field.type.find('[')
array_size = 1
if field.is_array:
array_size = field.array_len
if field.is_builtin:
field.sizeof_field_type = sizeof_field_type(field)
else:
# embedded type: may need to add padding
num_padding_bytes = align_to - (struct_size % align_to)
if num_padding_bytes != align_to:
padding_field = genmsg.Field('_padding'+str(padding_idx),
'uint8['+str(num_padding_bytes)+']')
padding_idx += 1
padding_field.sizeof_field_type = 1
struct_size += num_padding_bytes
fields.insert(i, padding_field)
i += 1
children_fields = get_children_fields(field.base_type)
field.sizeof_field_type, unused = add_padding_bytes(children_fields)
struct_size += field.sizeof_field_type * array_size
i += 1
# add padding at the end (necessary for embedded types)
num_padding_bytes = align_to - (struct_size % align_to)
if num_padding_bytes == align_to:
num_padding_bytes = 0
else:
padding_field = genmsg.Field('_padding'+str(padding_idx),
'uint8['+str(num_padding_bytes)+']')
padding_idx += 1
padding_field.sizeof_field_type = 1
struct_size += num_padding_bytes
fields.append(padding_field)
return (struct_size, num_padding_bytes)
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
struct_size, padding_end_size = add_padding_bytes(sorted_fields)
topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
}@
@ -127,42 +178,10 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so
#include <uORB/topics/@(topic_name).h>
void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer)
{
struct @(uorb_struct) *topic = static_cast<struct @(uorb_struct) *>(in);
@{
serialize_expr = " serialize_{type}(&topic->{field}, buffer);"
serialize_array_expr = " serialize_{type}(&topic->{field}[{index}], buffer);"
buffer_memcpy_expr = " memcpy(((char *)buffer->data) + buffer->next, &topic->{field}, sizeof(topic->{field}));"
buffer_next_expr = " buffer->next += sizeof(topic->{field});"
for each_field in sorted_fields:
if not each_field.is_header:
# call appropriate serialize functions for fields that are orb messages
if bare_name(each_field.type) in msgtypes_composite:
if each_field.is_array:
for i in range(each_field.array_len):
print(serialize_array_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name, index=i))
else:
print(serialize_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name))
# copy primitive fields
else:
print(buffer_memcpy_expr.format(field=each_field.name))
print(buffer_next_expr.format(field=each_field.name))
}@
}
@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
const char *__orb_@(topic_name)_fields = "@( topic_name.upper() ):@( ";".join(topic_fields) );";
@[for multi_topic in topics]@
ORB_DEFINE(@multi_topic, struct @uorb_struct, &serialize_@(topic_name), __orb_@(topic_name)_fields);
ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size),
__orb_@(topic_name)_fields);
@[end for]

82
msg/templates/uorb/msg.h.template
View File

@ -13,10 +13,12 @@
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - md5sum (String) MD5Sum of the .msg specification
@# - search_path (dict) search paths for genmsg
@# - topics (List of String) multi-topic names
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2016 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
@ -107,9 +109,6 @@ type_map = {
'float64': 'double',
'bool': 'bool',
'char': 'char',
'fence_vertex': 'fence_vertex',
'position_setpoint': 'position_setpoint',
'esc_report': 'esc_report'
}
msgtype_size_map = {
@ -125,9 +124,6 @@ msgtype_size_map = {
'float64': 8,
'bool': 1,
'char': 1,
'fence_vertex': 8,
'position_setpoint': 104,
'esc_report': 36
}
def bare_name(msg_type):
@ -139,7 +135,62 @@ def bare_name(msg_type):
return bare.split('[')[0]
def sizeof_field_type(field):
return msgtype_size_map[bare_name(field.type)]
bare_name_str = bare_name(field.type)
if bare_name_str in msgtype_size_map:
return msgtype_size_map[bare_name_str]
return 0 # this is for non-builtin types: sort them at the end
def get_children_fields(base_type):
(package, name) = genmsg.names.package_resource_name(base_type)
tmp_msg_context = genmsg.msg_loader.MsgContext.create_default()
spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path)
sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True)
return sorted_fields
# Add padding fields before the embedded types, at the end and calculate the
# struct size
def add_padding_bytes(fields):
struct_size = 8 # account for the timestamp
align_to = 8 # this is always 8, because of the 64bit timestamp
i = 0
padding_idx = 0
while i < len(fields):
field = fields[i]
if not field.is_header:
a_pos = field.type.find('[')
array_size = 1
if field.is_array:
array_size = field.array_len
if field.is_builtin:
field.sizeof_field_type = sizeof_field_type(field)
else:
# embedded type: may need to add padding
num_padding_bytes = align_to - (struct_size % align_to)
if num_padding_bytes != align_to:
padding_field = genmsg.Field('_padding'+str(padding_idx),
'uint8['+str(num_padding_bytes)+']')
padding_idx += 1
padding_field.sizeof_field_type = 1
struct_size += num_padding_bytes
fields.insert(i, padding_field)
i += 1
children_fields = get_children_fields(field.base_type)
field.sizeof_field_type, unused = add_padding_bytes(children_fields)
struct_size += field.sizeof_field_type * array_size
i += 1
# add padding at the end (necessary for embedded types)
num_padding_bytes = align_to - (struct_size % align_to)
if num_padding_bytes == align_to:
num_padding_bytes = 0
else:
padding_field = genmsg.Field('_padding'+str(padding_idx),
'uint8['+str(num_padding_bytes)+']')
padding_idx += 1
padding_field.sizeof_field_type = 1
struct_size += num_padding_bytes
fields.append(padding_field)
return (struct_size, num_padding_bytes)
# Function to print a standard ros type
def print_field_def(field):
@ -165,18 +216,23 @@ def print_field_def(field):
# need to add _t: int8 --> int8_t
type_px4 = type_map[type_name]
else:
raise Exception("Type {0} not supported, add to to template file!".format(type_name))
type_px4 = type_name
print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
comment = ''
if field.name.startswith('_padding'):
comment = ' // required for logger'
print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name,
array_size, comment))
def print_parsed_fields():
# sort fields
# sort fields (using a stable sort)
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
struct_size, padding_end_size = add_padding_bytes(sorted_fields)
# loop over all fields and print the type and name
for field in sorted_fields:
if (not field.is_header):
print_field_def(field)
}@
#ifdef __cplusplus
@ -205,8 +261,6 @@ for constant in spec.constants:
#endif
};
void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer);
/* register this as object request broker structure */
@[for multi_topic in topics]@
ORB_DECLARE(@multi_topic);

34
src/modules/logger/logger.cpp
View File

@ -494,43 +494,21 @@ void Logger::run()
for (LoggerSubscription &sub : _subscriptions) {
/* each message consists of a header followed by an orb data object
* The size of the data object is given by orb_metadata.o_size
*/
size_t padded_msg_size = sub.metadata->o_size;
uint8_t orb_msg_padded[padded_msg_size];
size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size;
//TODO: use sub.metadata->o_size_no_padding
uint8_t buffer[msg_size];
/* if this topic has been updated, copy the new data into the message buffer
* and write a message to the log
*/
//orb_check(sub.fd, &updated); // check whether a non-multi topic has been updated
/* this works for both single and multi-instances */
for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], &orb_msg_padded, &sub.time_tried_subscribe)) {
if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s),
&sub.time_tried_subscribe)) {
struct orb_output_buffer output_buffer = {};
uint8_t buffer[sizeof(message_data_header_s) + padded_msg_size];
output_buffer.data = &buffer;
output_buffer.next = sizeof(message_data_header_s);
sub.metadata->serialize(&orb_msg_padded, &output_buffer);
size_t msg_size = output_buffer.next;
//uint64_t timestamp;
//memcpy(&timestamp, buffer + sizeof(message_data_header_s), sizeof(timestamp));
//warnx("topic: %s, instance: %d, timestamp: %llu",
// sub.metadata->o_name, instance, timestamp);
/* copy the current topic data into the buffer after the header */
//orb_copy(sub.metadata, sub.fd, buffer + sizeof(message_data_header_s));
/* fill the message header struct in-place at the front of the buffer,
* accessing the unaligned (packed) structure properly
*/
message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
header->msg_type = static_cast<uint8_t>(MessageType::DATA);
/* the ORB topic data object has 2 unused trailing bytes? */
//header->msg_size = static_cast<uint16_t>(msg_size - 3);
header->msg_size = msg_size;
header->msg_size = static_cast<uint16_t>(msg_size - 3);
header->msg_id = msg_id;
header->multi_id = 0x80 + instance; // Non multi, active

17
src/modules/uORB/uORB.h
View File

@ -46,21 +46,14 @@
// Hack until everything is using this header
#include <systemlib/visibility.h>
struct orb_output_buffer {
void *data;
size_t next;
};
typedef void (*func_ptr)(void *in, struct orb_output_buffer *out);
/**
* Object metadata.
*/
struct orb_metadata {
const char *o_name; /**< unique object name */
const size_t o_size; /**< object size */
func_ptr serialize; /**< serialization function for this orb topic */
const char *o_fields; /**< semicolon separated list of fields */
const size_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
const char *o_fields; /**< semicolon separated list of fields (with type) */
};
typedef const struct orb_metadata *orb_id_t;
@ -119,14 +112,14 @@ enum ORB_PRIO {
*
* @param _name The name of the topic.
* @param _struct The structure the topic provides.
* @param _func The pointer to a function that serializes this topic
* @param _size_no_padding Struct size w/o padding at the end
* @param _fields All fields in a semicolon separated list e.g: "float[3] position;bool armed"
*/
#define ORB_DEFINE(_name, _struct, _func, _fields) \
#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields) \
const struct orb_metadata __orb_##_name = { \
#_name, \
sizeof(_struct), \
_func, \
_size_no_padding, \
_fields \
}; struct hack

14
src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
View File

@ -41,8 +41,8 @@ struct orb_test {
int val;
hrt_abstime time;
};
ORB_DEFINE(orb_test, struct orb_test, nullptr, "ORB_TEST:int val;hrt_abstime time;");
ORB_DEFINE(orb_multitest, struct orb_test, nullptr, "ORB_MULTITEST:int val;hrt_abstime time;");
ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;");
ORB_DEFINE(orb_multitest, struct orb_test, sizeof(orb_test), "ORB_MULTITEST:int val;hrt_abstime time;");
struct orb_test_medium {
@ -50,9 +50,10 @@ struct orb_test_medium {
hrt_abstime time;
char junk[64];
};
ORB_DEFINE(orb_test_medium, struct orb_test_medium, nullptr, "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, nullptr,
"ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium),
"ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium),
"ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
struct orb_test_large {
@ -60,7 +61,8 @@ struct orb_test_large {
hrt_abstime time;
char junk[512];
};
ORB_DEFINE(orb_test_large, struct orb_test_large, nullptr, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
ORB_DEFINE(orb_test_large, struct orb_test_large, sizeof(orb_test_large),
"ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
namespace uORBTest