Skip to content

Commit

Permalink
Refs PX4#1960. Added some serial flow control
Browse files Browse the repository at this point in the history
  • Loading branch information
Vicente Monge committed Mar 13, 2017
1 parent 2f1b660 commit e6ebd88
Show file tree
Hide file tree
Showing 5 changed files with 110 additions and 162 deletions.
9 changes: 5 additions & 4 deletions Tools/generate_microRTPS_support.py
View file
Expand Up @@ -19,7 +19,8 @@
uorb_templates_dir = root_path + "/msg/templates/uorb"
urtps_templates_dir = root_path + "/msg/templates/urtps"

px_generate_uorb_topic_files.generate_uRTPS_application_file(msg_file, out_dir, uorb_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_idl_file( msg_file, out_dir, urtps_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_uRTPS_receiver_header( msg_file, out_dir, urtps_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_uRTPS_receiver_source( msg_file, out_dir, urtps_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_uRTPS_application_file(msg_file, out_dir, uorb_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_uRTPS_app_CMList_file (msg_file, out_dir, uorb_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_idl_file (msg_file, out_dir, urtps_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_uRTPS_receiver_header (msg_file, out_dir, urtps_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
px_generate_uorb_topic_files.generate_uRTPS_receiver_source (msg_file, out_dir, urtps_templates_dir, px_generate_uorb_topic_files.INCL_DEFAULT)
119 changes: 46 additions & 73 deletions Tools/px_generate_uorb_topic_files.py
View file
Expand Up @@ -82,12 +82,14 @@
INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
PACKAGE = 'px4'
TOPICS_TOKEN = '# TOPICS '
uRTPS_TRANS_TEMPL_FILE = 'uRTPS_UART_transmitter.cpp.template'
uRTPS_TRANS_APP_TEMPL_FILE = 'uRTPS_UART_transmitter.cpp.template'
uRTPS_TRANS_CML_TEMPL_FILE = 'transmitter_CMakeLists.txt.template'
IDL_TEMPLATE_FILE = 'msg.idl.template'
uRTPS_RECEV_SRC_TEMPL_FILE = 'uRTPS_UART_receiver.cxx.template'
uRTPS_RECEV_H_TEMPL_FILE = 'uRTPS_UART_receiver.h.template'



def get_multi_topics(filename):
"""
Get TOPICS names from a "# TOPICS" line
Expand Down Expand Up @@ -149,108 +151,86 @@ def generate_uRTPS_application_file(filename_msg, outputdir, templatedir, includ
"""
Generates an application to send by UART msg content
"""
msg_context = genmsg.msg_loader.MsgContext.create_default()
full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename_msg))
spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename_msg, full_type_name)
topics = get_multi_topics(filename_msg)
if includepath:
search_path = genmsg.command_line.includepath_to_dict(includepath)
else:
search_path = {}
genmsg.msg_loader.load_depends(msg_context, spec, search_path)
md5sum = genmsg.gentools.compute_md5(msg_context, spec)
if len(topics) == 0:
topics.append(spec.short_name)
em_globals = {
"file_name_in": filename_msg,
"md5sum": md5sum,
"search_path": search_path,
"msg_context": msg_context,
"spec": spec,
"topics": topics
}
em_globals = get_em_globals(filename_msg, includepath)
spec_short_name = em_globals["spec"].short_name

# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)

template_file = os.path.join(templatedir, uRTPS_TRANS_TEMPL_FILE)
output_file = os.path.join(outputdir, spec.short_name + "_" + uRTPS_TRANS_TEMPL_FILE.replace(".cpp.template", ".cpp"))
template_file = os.path.join(templatedir, uRTPS_TRANS_APP_TEMPL_FILE)
output_file = os.path.join(outputdir, spec_short_name + "_" + uRTPS_TRANS_APP_TEMPL_FILE.replace(".cpp.template", ".cpp"))

return generate_by_template(output_file, template_file, em_globals)

def generate_uRTPS_app_CMList_file(filename_msg, outputdir, templatedir, includepath):
"""
Generates an application to send by UART msg content
"""
em_globals = get_em_globals(filename_msg, includepath)
spec_short_name = em_globals["spec"].short_name

# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)

template_file = os.path.join(templatedir, uRTPS_TRANS_CML_TEMPL_FILE)
output_file = os.path.join(outputdir, spec_short_name + "_CMakeLists.txt")

return generate_by_template(output_file, template_file, em_globals)

def generate_idl_file(filename_msg, outputdir, templatedir, includepath):
"""
Generates an .idl from .msg file
"""
msg_context = genmsg.msg_loader.MsgContext.create_default()
full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename_msg))
spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename_msg, full_type_name)
topics = get_multi_topics(filename_msg)
if includepath:
search_path = genmsg.command_line.includepath_to_dict(includepath)
else:
search_path = {}
genmsg.msg_loader.load_depends(msg_context, spec, search_path)
md5sum = genmsg.gentools.compute_md5(msg_context, spec)
if len(topics) == 0:
topics.append(spec.short_name)
em_globals = {
"file_name_in": filename_msg,
"md5sum": md5sum,
"search_path": search_path,
"msg_context": msg_context,
"spec": spec,
"topics": topics
}
em_globals = get_em_globals(filename_msg, includepath)
spec_short_name = em_globals["spec"].short_name

# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)

template_file = os.path.join(templatedir, IDL_TEMPLATE_FILE)
output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace("msg.idl.template", str(spec.short_name + "_.idl")))
output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace("msg.idl.template", str(spec_short_name + "_.idl")))

return generate_by_template(output_file, template_file, em_globals)

def generate_uRTPS_receiver_source(filename_msg, outputdir, templatedir, includepath):
"""
Generates receiver .cpp by UART msg content
"""
msg_context = genmsg.msg_loader.MsgContext.create_default()
full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename_msg))
spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename_msg, full_type_name)
topics = get_multi_topics(filename_msg)
if includepath:
search_path = genmsg.command_line.includepath_to_dict(includepath)
else:
search_path = {}
genmsg.msg_loader.load_depends(msg_context, spec, search_path)
md5sum = genmsg.gentools.compute_md5(msg_context, spec)
if len(topics) == 0:
topics.append(spec.short_name)
em_globals = {
"file_name_in": filename_msg,
"md5sum": md5sum,
"search_path": search_path,
"msg_context": msg_context,
"spec": spec,
"topics": topics
}
em_globals = get_em_globals(filename_msg, includepath)
spec_short_name = em_globals["spec"].short_name

# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)

template_file = os.path.join(templatedir, uRTPS_RECEV_SRC_TEMPL_FILE)
output_file = os.path.join(outputdir, spec.short_name + "_" + uRTPS_RECEV_SRC_TEMPL_FILE.replace(".cxx.template", ".cxx"))
output_file = os.path.join(outputdir, spec_short_name + "_" + uRTPS_RECEV_SRC_TEMPL_FILE.replace(".cxx.template", ".cxx"))

return generate_by_template(output_file, template_file, em_globals)

def generate_uRTPS_receiver_header(filename_msg, outputdir, templatedir, includepath):
"""
Generates receiver .h by UART msg content
"""
em_globals = get_em_globals(filename_msg, includepath)
spec_short_name = em_globals["spec"].short_name

# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)

template_file = os.path.join(templatedir, uRTPS_RECEV_H_TEMPL_FILE)
output_file = os.path.join(outputdir, spec_short_name + "_" + uRTPS_RECEV_H_TEMPL_FILE.replace(".h.template", ".h"))

return generate_by_template(output_file, template_file, em_globals)

def get_em_globals(filename_msg, includepath):
"""
Generates em globals dictionary
"""
msg_context = genmsg.msg_loader.MsgContext.create_default()
full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename_msg))
spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename_msg, full_type_name)
Expand All @@ -272,14 +252,7 @@ def generate_uRTPS_receiver_header(filename_msg, outputdir, templatedir, include
"topics": topics
}

# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)

template_file = os.path.join(templatedir, uRTPS_RECEV_H_TEMPL_FILE)
output_file = os.path.join(outputdir, spec.short_name + "_" + uRTPS_RECEV_H_TEMPL_FILE.replace(".h.template", ".h"))

return generate_by_template(output_file, template_file, em_globals)
return em_globals

def generate_by_template(output_file, template_file, em_globals):
"""
Expand Down
10 changes: 6 additions & 4 deletions msg/templates/uorb/uRTPS_UART_transmitter.cpp.template
View file
Expand Up @@ -44,7 +44,7 @@ int @(topic_name)_trans_main(int argc, char *argv[])
int serial_fd = uart_init(argv[1], 115200);

int sub_fd = orb_subscribe(ORB_ID(@(topic_name)));
orb_set_interval(sub_fd, 200);
orb_set_interval(sub_fd, 1000);
px4_pollfd_struct_t poll_fd;
poll_fd.fd = sub_fd;
poll_fd.events = POLLIN;
Expand Down Expand Up @@ -72,19 +72,21 @@ int @(topic_name)_trans_main(int argc, char *argv[])
if (poll_fd.revents & POLLIN)
{
/* obtained data for the first file descriptor */
struct @(uorb_struct) raw;
struct @(uorb_struct) data;
/* copy raw data into local buffer */
orb_copy(ORB_ID(@(topic_name)), sub_fd, &raw);
orb_copy(ORB_ID(@(topic_name)), sub_fd, &data);

char buffer[MICROCDR_INIT_BUF_LENGTH];
uint32_t length = 0;
serialize_@(topic_name)(&raw, buffer, &length);
serialize_@(topic_name)(&data, buffer, &length);

// Write serialized data trough UART
dprintf(serial_fd, ">>>");
for(int j=0; j < length; j++)
{
dprintf(serial_fd, "%c", buffer[j]);
}
dprintf(serial_fd, "<<<");
}
}
}
Expand Down
91 changes: 53 additions & 38 deletions msg/templates/urtps/uRTPS_UART_receiver.cxx.template
View file
Expand Up @@ -104,13 +104,13 @@ uint8_t @(topic_name)_UART_receiver::init_uart(std::string uart)
def add_deserialize_statement(field):
if (not field.is_header):
if (not field.is_array):
print '\t\t\tcdr_des >> data.' + field.name + '();'
print '\tcdr_des >> data.' + field.name + '();'
else:
for i in xrange(field.array_len):
print '\t\t\tcdr_des >> data.' + field.name + '()' + ('[%d];' %i)
print '\tcdr_des >> data.' + field.name + '()' + ('[%d];' %i)

def add_code_to_deserialize():
print '\t\t\tcdr_des >> data.timestamp();'
print '\tcdr_des >> data.timestamp();'
# sort fields (using a stable sort) as in the declaration of the type
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
for field in sorted_fields:
Expand All @@ -120,47 +120,62 @@ def add_code_to_deserialize():

uint8_t @(topic_name)_UART_receiver::readFromUART(@(topic_class_name) &data)
{
//----- CHECK FOR ANY RX BYTES -----
if (m_uart_filestream != -1)
if (m_uart_filestream == -1) return 2;

const size_t max_size = 1024;
// Read up to max_size characters from the port if they are there
static char rx_buffer[max_size];
static uint32_t pos_to_write = 0;
char aux = 0;
int rx_length = 0;

if (0 == pos_to_write) memset(rx_buffer, 0, max_size);

while (pos_to_write <= max_size) // Not enough
{
// Read up to 255 characters from the port if they are there
char rx_buffer[1024] = {};
int rx_length = read(m_uart_filestream, (void*)rx_buffer, sizeof(rx_buffer)); //Filestream, buffer to store in, number of bytes to read (max)
if (rx_length != 72)
rx_length = read(m_uart_filestream, (void*)&aux, 1);
if (1 != rx_length) return 1;

rx_buffer[pos_to_write++] = aux;
if (pos_to_write < 3) continue;

if (aux == '>')
{
printf(".%d", rx_length);
return 1;
// Beginning
if (0 == strncmp(rx_buffer + (pos_to_write - 3), ">>>", 3))
{
memset(rx_buffer, 0, 1024);
rx_buffer[0] = rx_buffer[1] = rx_buffer[2] = '>';
pos_to_write = 3;
}
}
else
else if (aux == '<')
{
//Bytes received
eprosima::fastcdr::FastBuffer cdrbuffer(rx_buffer, sizeof(rx_buffer));
eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
@add_code_to_deserialize()

printf("read %d bytes ", rx_length);
printf("accelerometer: %04f %04f %04f\n",
data.accelerometer_m_s2()[0],
data.accelerometer_m_s2()[1],
data.accelerometer_m_s2()[2]);

/*SerializedPayload_t payload;
payload.data = (octet*)rx_buffer;
payload.length = sizeof(rx_buffer);

sensor_combined_ data;
myType.deserialize(&payload, &data);
// Ending
if (0 == strncmp(rx_buffer + (pos_to_write - 3), "<<<", 3) &&
0 == strncmp(rx_buffer, ">>>", 3))
{
rx_length = pos_to_write;
break;
}
}
if (pos_to_write > max_size)
{
pos_to_write = 0;
return 1;
}
}

printf("read %d bytes ", rx_length);
printf("accelerometer: %04f %04f %04f\n",
st.accelerometer_m_s2()[0],
st.accelerometer_m_s2()[1],
st.accelerometer_m_s2()[2]);*/
char ser_buffer[max_size];
memset(ser_buffer, 0, max_size);
memcpy(ser_buffer, rx_buffer + 3, pos_to_write - 6);


//Bytes received
memset(&data, 0, sizeof(data));
eprosima::fastcdr::FastBuffer cdrbuffer(ser_buffer, sizeof(ser_buffer));
eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
@add_code_to_deserialize()

return 0;
}
}
return -1;
return 0;
}
43 changes: 0 additions & 43 deletions src/examples/micrortps_transmitter/CMakeLists.txt
View file

This file was deleted.

0 comments on commit e6ebd88

Please sign in to comment.