Skip to content
Snippets Groups Projects
Commit 76387b16 authored by Daniel Agar's avatar Daniel Agar Committed by Lorenz Meier
Browse files

uorb autogeneration

parent 3aa22974
No related branches found
No related tags found
No related merge requests found
Showing
with 496 additions and 67 deletions
......@@ -295,11 +295,11 @@ add_definitions(${definitions})
#=============================================================================
# source code generation
#
file(GLOB_RECURSE msg_files msg/*.msg)
file(GLOB msg_files msg/*.msg)
px4_generate_messages(TARGET msg_gen
MSG_FILES ${msg_files}
OS ${OS}
DEPENDS git_genmsg git_gencpp
DEPENDS git_genmsg git_gencpp prebuild_targets
)
px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD})
px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD})
......
......@@ -155,6 +155,9 @@ px4-stm32f4discovery_default:
px4fmu-v2_ekf2:
$(call cmake-build,nuttx_px4fmu-v2_ekf2)
px4fmu-v2_lpe:
$(call cmake-build,nuttx_px4fmu-v2_lpe)
mindpx-v2_default:
$(call cmake-build,nuttx_mindpx-v2_default)
......
......@@ -49,6 +49,7 @@ sys.path.append(px4_tools_dir + "/genmsg/src")
sys.path.append(px4_tools_dir + "/gencpp/src")
try:
import em
import genmsg.template_tools
except ImportError as e:
print("python import error: ", e)
......@@ -74,24 +75,75 @@ __license__ = "BSD"
__email__ = "thomasgubler@gmail.com"
msg_template_map = {'msg.h.template': '@NAME@.h'}
srv_template_map = {}
incl_default = ['std_msgs:./msg/std_msgs']
package = 'px4'
TEMPLATE_FILE = 'msg.h.template'
OUTPUT_FILE_EXT = '.h'
INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
PACKAGE = 'px4'
TOPICS_TOKEN = '# TOPICS '
def convert_file(filename, outputdir, templatedir, includepath):
def get_multi_topics(filename):
"""
Converts a single .msg file to a uorb header
Get TOPICS names from a "# TOPICS" line
"""
#print("Generating headers from {0}".format(filename))
genmsg.template_tools.generate_from_file(filename,
package,
outputdir,
templatedir,
includepath,
msg_template_map,
srv_template_map)
ofile = open(filename, 'r')
text = ofile.read()
result = []
for each_line in text.split('\n'):
if each_line.startswith (TOPICS_TOKEN):
topic_names_str = each_line.strip()
topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "")
result.extend(topic_names_str.split(" "))
ofile.close()
return result
def generate_header_from_file(filename, outputdir, templatedir, includepath):
"""
Converts a single .msg file to a uorb header file
"""
msg_context = genmsg.msg_loader.MsgContext.create_default()
full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename))
spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name)
topics = get_multi_topics(filename)
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,
"md5sum": md5sum,
"search_path": search_path,
"msg_context": msg_context,
"spec": spec,
"topics": topics
}
# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)
template_file = os.path.join(templatedir, TEMPLATE_FILE)
output_file = os.path.join(outputdir, spec.short_name + OUTPUT_FILE_EXT)
if os.path.isfile(output_file):
return False
ofile = open(output_file, 'w')
# todo, reuse interpreter
interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True})
if not os.path.isfile(template_file):
ofile.close()
os.remove(output_file)
raise RuntimeError("Template file %s not found in template dir %s" % (template_file, templatedir))
interpreter.file(open(template_file)) #todo try
interpreter.shutdown()
ofile.close()
return True
def convert_dir(inputdir, outputdir, templatedir):
......@@ -122,7 +174,7 @@ def convert_dir(inputdir, outputdir, templatedir):
if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime):
return False
includepath = incl_default + [':'.join([package, inputdir])]
includepath = INCL_DEFAULT + [':'.join([PACKAGE, inputdir])]
for f in os.listdir(inputdir):
# Ignore hidden files
if f.startswith("."):
......@@ -133,11 +185,7 @@ def convert_dir(inputdir, outputdir, templatedir):
if not os.path.isfile(fn):
continue
convert_file(fn,
outputdir,
templatedir,
includepath)
generate_header_from_file(fn, outputdir, templatedir, includepath)
return True
......@@ -151,15 +199,15 @@ def copy_changed(inputdir, outputdir, prefix='', quiet=False):
if not os.path.isdir(outputdir):
os.makedirs(outputdir)
for f in os.listdir(inputdir):
fni = os.path.join(inputdir, f)
for input_file in os.listdir(inputdir):
fni = os.path.join(inputdir, input_file)
if os.path.isfile(fni):
# Check if f exists in outpoutdir, copy the file if not
fno = os.path.join(outputdir, prefix + f)
# Check if input_file exists in outpoutdir, copy the file if not
fno = os.path.join(outputdir, prefix + input_file)
if not os.path.isfile(fno):
shutil.copy(fni, fno)
if not quiet:
print("{0}: new header file".format(f))
print("{0}: new header file".format(fno))
continue
if os.path.getmtime(fni) > os.path.getmtime(fno):
......@@ -168,11 +216,11 @@ def copy_changed(inputdir, outputdir, prefix='', quiet=False):
if not filecmp.cmp(fni, fno):
shutil.copy(fni, fno)
if not quiet:
print("{0}: updated".format(f))
print("{0}: updated".format(input_file))
continue
if not quiet:
print("{0}: unchanged".format(f))
print("{0}: unchanged".format(input_file))
def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False):
......@@ -208,11 +256,7 @@ if __name__ == "__main__":
if args.file is not None:
for f in args.file:
convert_file(
f,
args.outputdir,
args.templatedir,
incl_default)
generate_header_from_file(f, args.outputdir, args.templatedir, INCL_DEFAULT)
elif args.dir is not None:
convert_dir_save(
args.dir,
......
#!/usr/bin/env python
#############################################################################
#
# Copyright (C) 2013-2015 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.
#
#############################################################################
"""
px_generate_uorb_topic_sources.py
Generates cpp source files for uorb topics from .msg (ROS syntax)
message files
"""
from __future__ import print_function
import os
import shutil
import filecmp
import argparse
import sys
px4_tools_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.append(px4_tools_dir + "/genmsg/src")
sys.path.append(px4_tools_dir + "/gencpp/src")
try:
import em
import genmsg.template_tools
except ImportError as e:
print("python import error: ", e)
print('''
Required python packages not installed.
On a Debian/Ubuntu system please run:
sudo apt-get install python-empy
sudo pip install catkin_pkg
On MacOS please run:
sudo pip install empy catkin_pkg
On Windows please run:
easy_install empy catkin_pkg
''')
exit(1)
__author__ = "Sergey Belash, Thomas Gubler"
__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
__license__ = "BSD"
__email__ = "againagainst@gmail.com, thomasgubler@gmail.com"
TOPIC_TEMPLATE_FILE = 'msg.cpp.template'
TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template'
OUTPUT_FILE_EXT = '.cpp'
PACKAGE = 'px4'
TOPICS_TOKEN = '# TOPICS '
def get_multi_topics(filename):
"""
Get TOPICS names from a "# TOPICS" line
"""
ofile = open(filename, 'r')
text = ofile.read()
result = []
for each_line in text.split('\n'):
if each_line.startswith (TOPICS_TOKEN):
topic_names_str = each_line.strip()
topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "")
result.extend(topic_names_str.split(" "))
ofile.close()
return result
def get_msgs_list(msgdir):
"""
Makes list of msg files in the given directory
"""
return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")]
def generate_source_from_file(filename, outputdir, template_file, quiet=False):
"""
Converts a single .msg file to a uorb source file
"""
# print("Generating sources from {0}".format(filename))
msg_context = genmsg.msg_loader.MsgContext.create_default()
full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename))
spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name)
topics = get_multi_topics(filename)
if len(topics) == 0:
topics.append(spec.short_name)
em_globals = {
"file_name_in": filename,
"spec": spec,
"topics": topics
}
output_file = os.path.join(outputdir, spec.short_name + OUTPUT_FILE_EXT)
if os.path.isfile(output_file):
return False
generate_by_template(output_file, template_file, em_globals)
if not quiet:
print("{0}: new source file".format(output_file))
return True
def generate_by_template(output_file, template_file, em_globals):
"""
Invokes empy intepreter to geneate output_file by the
given template_file and predefined em_globals dict
"""
ofile = open(output_file, 'w')
# todo, reuse interpreter
interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True})
if not os.path.isfile(template_file):
ofile.close()
os.remove(output_file)
raise RuntimeError("Template file %s not found" % (template_file))
interpreter.file(open(template_file)) #todo try
interpreter.shutdown()
ofile.close()
def convert_dir(msgdir, outputdir, templatedir, quiet=False):
"""
Converts all .msg files in msgdir to uORB sources
"""
# Make sure output directory exists:
if not os.path.isdir(outputdir):
os.makedirs(outputdir)
template_file = os.path.join(templatedir, TOPIC_TEMPLATE_FILE)
for f in os.listdir(msgdir):
# Ignore hidden files
if f.startswith("."):
continue
fn = os.path.join(msgdir, f)
# Only look at actual files
if not os.path.isfile(fn):
continue
generate_source_from_file(fn, outputdir, template_file, quiet)
# generate cpp file with topics list
tl_globals = {"msgs" : get_msgs_list(msgdir)}
tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE)
tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", ""))
generate_by_template(tl_out_file, tl_template_file, tl_globals)
return True
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Convert msg files to uorb sources')
parser.add_argument('-d', dest='dir', help='directory with msg files')
parser.add_argument('-f', dest='file', help="files to convert (use only without -d)", nargs="+")
parser.add_argument('-e', dest='templatedir', help='directory with template files',)
parser.add_argument('-o', dest='outputdir', help='output directory for source files')
parser.add_argument('-q', dest='quiet', default=False, action='store_true',
help='string added as prefix to the output file '
' name when converting directories')
args = parser.parse_args()
if args.file is not None:
for f in args.file:
generate_source_from_file(f, args.outputdir, args.templatedir, args.quiet)
elif args.dir is not None:
convert_dir(args.dir, args.outputdir, args.templatedir, args.quiet)
......@@ -359,13 +359,15 @@ function(px4_generate_messages)
NAME px4_generate_messages
OPTIONS VERBOSE
ONE_VALUE OS TARGET
MULTI_VALUE MSG_FILES DEPENDS
MULTI_VALUE MSG_FILES DEPENDS INCLUDES
REQUIRED MSG_FILES OS TARGET
ARGN ${ARGN})
set(QUIET)
if(NOT VERBOSE)
set(QUIET "-q")
endif()
# headers
set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics)
set(msg_list)
foreach(msg_file ${MSG_FILES})
......@@ -390,6 +392,26 @@ function(px4_generate_messages)
VERBATIM
)
# !sources
set(msg_source_out_path ${CMAKE_BINARY_DIR}/topics_sources)
set(msg_source_files_out ${msg_source_out_path}/uORBTopics.cpp)
foreach(msg ${msg_list})
list(APPEND msg_source_files_out ${msg_source_out_path}/${msg}.cpp)
endforeach()
add_custom_command(OUTPUT ${msg_source_files_out}
COMMAND ${PYTHON_EXECUTABLE}
Tools/px_generate_uorb_topic_sources.py
${QUIET}
-d msg
-e msg/templates/uorb
-o ${msg_source_out_path}
DEPENDS ${DEPENDS} ${MSG_FILES}
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
COMMENT "Generating uORB topic sources"
VERBATIM
)
set_source_files_properties(${msg_source_files_out} PROPERTIES GENERATED TRUE)
# multi messages for target OS
set(msg_multi_out_path
${CMAKE_BINARY_DIR}/src/platforms/${OS}/px4_messages)
......@@ -411,8 +433,13 @@ function(px4_generate_messages)
COMMENT "Generating uORB topic multi headers for ${OS}"
VERBATIM
)
add_custom_target(${TARGET}
DEPENDS ${msg_multi_files_out} ${msg_files_out})
add_library(${TARGET}
${msg_source_files_out}
${msg_multi_files_out}
${msg_files_out}
)
endfunction()
#=============================================================================
......
set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "build type")
include(nuttx/px4_impl_nuttx)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
set(config_module_list
#
# Board support modules
#
drivers/device
drivers/stm32
drivers/stm32/adc
drivers/stm32/tone_alarm
drivers/led
drivers/px4fmu
drivers/px4io
drivers/boards/px4fmu-v2
drivers/rgbled
drivers/mpu6000
#drivers/mpu9250
drivers/lsm303d
drivers/l3gd20
#drivers/hmc5883
drivers/ms5611
#drivers/mb12xx
#drivers/srf02
#drivers/sf0x
#drivers/ll40ls
drivers/trone
drivers/gps
drivers/pwm_out_sim
#drivers/hott
#drivers/hott/hott_telemetry
#drivers/hott/hott_sensors
#drivers/blinkm
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
#drivers/frsky_telemetry
modules/sensors
#drivers/mkblctrl
#drivers/px4flow
#drivers/oreoled
drivers/gimbal
drivers/pwm_input
drivers/camera_trigger
drivers/bst
#drivers/snapdragon_rc_pwm
#drivers/lis3mdl
#
# System commands
#
systemcmds/bl_update
systemcmds/mixer
systemcmds/param
systemcmds/perf
systemcmds/pwm
systemcmds/esc_calib
systemcmds/reboot
#systemcmds/topic_listener
systemcmds/top
systemcmds/config
systemcmds/nshterm
systemcmds/mtd
systemcmds/dumpfile
systemcmds/ver
#
# General system control
#
modules/commander
modules/navigator
modules/mavlink
modules/gpio_led
#modules/uavcan
modules/land_detector
#
# Estimation modules (EKF/ SO3 / other filters)
#
# Too high RAM usage due to static allocations
# modules/attitude_estimator_ekf
modules/attitude_estimator_q
modules/ekf_att_pos_estimator
modules/position_estimator_inav
#
# Vehicle Control
#
# modules/segway # XXX Needs GCC 4.7 fix
modules/fw_pos_control_l1
modules/fw_att_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
#
# Logging
#
modules/sdlog2
#
# Library modules
#
modules/param
modules/systemlib
modules/systemlib/mixer
modules/uORB
modules/dataman
#
# Libraries
#
lib/controllib
lib/mathlib
lib/mathlib/math/filter
lib/ecl
lib/external_lgpl
lib/geo
lib/geo_lookup
lib/conversion
lib/launchdetection
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
platforms/nuttx
# had to add for cmake, not sure why wasn't in original config
platforms/common
platforms/nuttx/px4_layer
#
# OBC challenge
#
#modules/bottle_drop
#
# Rover apps
#
#examples/rover_steering_control
#
# Demo apps
#
#examples/math_demo
# Tutorial code from
# https://px4.io/dev/px4_simple_app
#examples/px4_simple_app
# Tutorial code from
# https://px4.io/dev/daemon
#examples/px4_daemon_app
# Tutorial code from
# https://px4.io/dev/debug_values
#examples/px4_mavlink_debug
# Tutorial code from
# https://px4.io/dev/example_fixedwing_control
#examples/fixedwing_control
# Hardware test
#examples/hwtest
)
set(config_extra_builtin_cmds
serdis
sercon
)
set(config_io_board
px4io-v2
)
set(config_io_extra_libs
)
add_custom_target(sercon)
set_target_properties(sercon PROPERTIES
PRIORITY "SCHED_PRIORITY_DEFAULT"
MAIN "sercon" STACK_MAIN "2048")
add_custom_target(serdis)
set_target_properties(serdis PROPERTIES
PRIORITY "SCHED_PRIORITY_DEFAULT"
MAIN "serdis" STACK_MAIN "2048")
uint64 timestamp # Microseconds since system boot
bool armed # Set to true if system is armed
bool prearmed # Set to true if the actuator safety is disabled but motors are not armed
bool ready_to_arm # Set to true if system is ready to be armed
......
......@@ -10,6 +10,9 @@ uint8 INDEX_AIRBRAKES = 6
uint8 INDEX_LANDING_GEAR = 7
uint8 GROUP_INDEX_ATTITUDE = 0
uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control
# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control
uint8 NUM_ACTUATORS_DIRECT = 16
uint64 timestamp # timestamp in us since system boot
uint32 nvalues # number of valid values
float32[16] values # actuator values, from -1 to 1
uint8 NUM_ACTUATOR_OUTPUTS = 16
uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking
uint64 timestamp # output timestamp in us since system boot
uint32 noutputs # valid outputs
float32[16] output # output data, in natural output units
uint64 timestamp # microseconds since system boot, needed to integrate
float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown
float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown
float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown
......
uint64 timestamp # microseconds since system boot, needed to integrate
float32 voltage_v # Battery voltage in volts, 0 if unknown
float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown
float32 current_a # Battery current in amperes, -1 if unknown
......
uint64 timestamp # Timestamp when camera was triggered
uint32 seq # Image sequence
......@@ -14,6 +14,5 @@ uint8 MAIN_STATE_AUTO_LAND = 11
uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
uint8 MAIN_STATE_MAX = 13
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
uint8 main_state # main state machine
# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
uint64 timestamp # in microseconds since system start
float32 x_acc # X acceleration in body frame
float32 y_acc # Y acceleration in body frame
float32 z_acc # Z acceleration in body frame
......
uint64 timestamp # Microseconds since system boot, needed to integrate
uint64 error_count # Number of errors detected by driver
float32 differential_pressure_raw_pa # Raw differential pressure reading (may be negative)
float32 differential_pressure_filtered_pa # Low pass filtered differential pressure reading
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment