forked from Archive/PX4-Autopilot
uorb autogeneration
This commit is contained in:
parent
3aa2297497
commit
76387b1693
|
@ -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})
|
||||
|
|
3
Makefile
3
Makefile
|
@ -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,
|
||||
|
|
|
@ -0,0 +1,192 @@
|
|||
#!/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()
|
||||
|
||||
#=============================================================================
|
||||
|
|
|
@ -0,0 +1,189 @@
|
|||
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")
|
|
@ -1,5 +1,4 @@
|
|||
|
||||
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
|
||||
|
||||
|
|
|
@ -1,5 +0,0 @@
|
|||
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
|
|
@ -1,5 +0,0 @@
|
|||
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
|
|
@ -1,5 +0,0 @@
|
|||
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
|
|
@ -1,5 +0,0 @@
|
|||
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
|
|
@ -1,4 +1,3 @@
|
|||
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
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
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
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
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
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
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
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
|
||||
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
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
# 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
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
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
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
# DISTANCE_SENSOR message data
|
||||
|
||||
uint64 timestamp # Microseconds since system boot
|
||||
|
||||
float32 min_distance # Minimum distance the sensor can measure (in m)
|
||||
float32 max_distance # Maximum distance the sensor can measure (in m)
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # Timestamp in microseconds since boot
|
||||
float32[6] vel_pos_innov # velocity and position innovations
|
||||
float32[3] mag_innov # earth magnetic field innovations
|
||||
float32 heading_innov # heading innovation
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
uint8 NUM_ENCODERS = 4
|
||||
|
||||
uint64 timestamp
|
||||
int64[4] counts # counts of encoder
|
||||
float32[4] velocity # counts of encoder/ second
|
||||
|
|
|
@ -11,7 +11,6 @@ uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C
|
|||
uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus
|
||||
|
||||
uint16 counter # incremented by the writing thread everytime new data is stored
|
||||
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
|
||||
|
||||
uint8 esc_count # number of connected ESCs
|
||||
uint8 esc_connectiontype # how ESCs connected to the system
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # Timestamp in microseconds since boot
|
||||
float32[32] states # Internal filter states
|
||||
float32 n_states # Number of states effectively used
|
||||
float32[3] vibe # Vibration levels in X, Y and Z
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
# Filtered bottom flow in bodyframe.
|
||||
uint64 timestamp # time of this estimate, in microseconds since system start
|
||||
|
||||
float32 sumx # Integrated bodyframe x flow in meters
|
||||
float32 sumy # Integrated bodyframe y flow in meters
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # timestamp, UNIX epoch (GPS synced)
|
||||
float64 lat # target position (deg * 1e7)
|
||||
float64 lon # target position (deg * 1e7)
|
||||
float32 alt # target position
|
||||
|
|
|
@ -1,5 +1,3 @@
|
|||
uint64 timestamp # timestamp this topic was emitted
|
||||
|
||||
float32 nav_roll
|
||||
float32 nav_pitch
|
||||
float32 nav_bearing
|
||||
|
|
|
@ -8,7 +8,6 @@
|
|||
#
|
||||
###############################################################################################
|
||||
|
||||
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
|
||||
|
||||
float32 roll_body # body angle in NED frame
|
||||
float32 pitch_body # body angle in NED frame
|
||||
|
@ -35,3 +34,6 @@ bool fw_control_yaw # control heading with rudder (used for auto takeoff on
|
|||
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
|
||||
|
||||
bool apply_flaps
|
||||
|
||||
# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
|
||||
# TOPICS fw_virtual_attitude_setpoint
|
||||
|
|
|
@ -8,7 +8,6 @@
|
|||
#
|
||||
###############################################################################################
|
||||
|
||||
uint64 timestamp # in microseconds since system start
|
||||
|
||||
float32 roll # body angular rates in NED frame
|
||||
float32 pitch # body angular rates in NED frame
|
||||
|
|
|
@ -17,7 +17,6 @@ int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2
|
|||
#
|
||||
# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
|
||||
|
||||
uint64 timestamp # Timestamp in microseconds since boot, from gyro
|
||||
#
|
||||
int16[3] gyro_raw # Raw sensor values of angular velocity
|
||||
float32[3] gyro_rad_s # Angular velocity in radian per seconds
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
# GPS home position in WGS84 coordinates.
|
||||
uint64 timestamp # Timestamp (microseconds since system boot)
|
||||
|
||||
float64 lat # Latitude in degrees
|
||||
float64 lon # Longitude in degrees
|
||||
|
|
|
@ -12,8 +12,6 @@ int8 MODE_SLOT_5 = 4 # mode slot 5 selected
|
|||
int8 MODE_SLOT_6 = 5 # mode slot 6 selected
|
||||
int8 MODE_SLOT_MAX = 6 # number of slots plus one
|
||||
|
||||
uint64 timestamp
|
||||
|
||||
# Any of the channels may not be available and be set to NaN
|
||||
# to indicate that it does not contain valid data.
|
||||
# The variable names follow the definition of the
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # Timestamp in microseconds since boot
|
||||
|
||||
uint8[50] text
|
||||
uint8 severity
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # in microseconds since system start
|
||||
|
||||
float32 roll_rate_integ # roll rate inegrator
|
||||
float32 pitch_rate_integ # pitch rate integrator
|
||||
|
|
|
@ -8,7 +8,6 @@
|
|||
#
|
||||
###############################################################################################
|
||||
|
||||
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
|
||||
|
||||
float32 roll_body # body angle in NED frame
|
||||
float32 pitch_body # body angle in NED frame
|
||||
|
@ -35,3 +34,6 @@ bool fw_control_yaw # control heading with rudder (used for auto takeoff on
|
|||
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
|
||||
|
||||
bool apply_flaps
|
||||
|
||||
# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
|
||||
# TOPICS mc_virtual_attitude_setpoint
|
||||
|
|
|
@ -8,7 +8,6 @@
|
|||
#
|
||||
###############################################################################################
|
||||
|
||||
uint64 timestamp # in microseconds since system start
|
||||
|
||||
float32 roll # body angular rates in NED frame
|
||||
float32 pitch # body angular rates in NED frame
|
||||
|
|
|
@ -1,3 +1,7 @@
|
|||
int32 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1
|
||||
uint32 count # count of the missions stored in the dataman
|
||||
int32 current_seq # default -1, start at the one changed latest
|
||||
|
||||
# fixme: there is no mission definition in objects_common.cpp
|
||||
# but it's required for systemcmds/topic_listener/topic_listener
|
||||
# TOPICS mission offboard_mission onboard_mission
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
# Off-board control mode
|
||||
uint64 timestamp
|
||||
|
||||
bool ignore_thrust
|
||||
bool ignore_attitude
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
# Optical flow in NED body frame in SI units.
|
||||
# @see http://en.wikipedia.org/wiki/International_System_of_Units
|
||||
|
||||
uint64 timestamp # in microseconds since system start
|
||||
uint8 sensor_id # id of the sensor emitting the flow value
|
||||
float32 pixel_flow_x_integral # accumulated optical flow in radians around x axis
|
||||
float32 pixel_flow_y_integral # accumulated optical flow in radians around y axis
|
||||
|
|
|
@ -1,2 +1 @@
|
|||
uint64 timestamp # time at which the latest parameter was updated
|
||||
bool saved # wether the change has already been saved to disk
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
|
||||
uint64 timestamp # Microseconds since system boot
|
||||
uint64 error_count
|
||||
uint32 pulse_width # Pulse width, timer counts
|
||||
uint32 period # Period, timer counts
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # Microseconds since system boot
|
||||
int32[100] string
|
||||
uint64 MAX_STRLEN = 100
|
||||
uint64 strlen
|
||||
|
|
|
@ -20,7 +20,7 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
|
|||
uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
|
||||
uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
|
||||
uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
|
||||
uint64 timestamp # Timestamp in microseconds since boot time
|
||||
|
||||
uint64 timestamp_last_valid # Timestamp of last valid RC signal
|
||||
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
|
||||
uint8 channel_count # Number of valid channels
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
|
||||
uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
|
||||
|
||||
uint64 timestamp # time at which the map was updated
|
||||
bool[3] valid #true for RC-Param channels which are mapped to a param
|
||||
int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used
|
||||
char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
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
|
|
@ -1,5 +1,4 @@
|
|||
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
|
|
@ -1,3 +1,2 @@
|
|||
uint64 timestamp
|
||||
bool safety_switch_available # Set to true if a safety switch is connected
|
||||
bool safety_off # Set to true if safety is off
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
uint8 SAT_INFO_MAX_SATELLITES = 20
|
||||
|
||||
uint64 timestamp # Timestamp of satellite info
|
||||
uint8 count # Number of satellites in satellite info
|
||||
uint8[20] svid # Space vehicle ID [1..255], see scheme below
|
||||
uint8[20] used # 0: Satellite not used, 1: used for navigation
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp
|
||||
uint64 integral_dt # integration time
|
||||
uint64 error_count
|
||||
float32 x # acceleration in the NED X board axis in m/s^2
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
float32 pressure
|
||||
float32 altitude
|
||||
float32 temperature
|
||||
uint64 timestamp
|
||||
uint64 error_count
|
||||
|
|
|
@ -25,7 +25,6 @@ uint32 SENSOR_PRIO_MAX = 255
|
|||
#
|
||||
# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
|
||||
|
||||
uint64 timestamp # Timestamp in microseconds since boot, from gyro
|
||||
uint64[3] gyro_timestamp # Gyro timestamps
|
||||
int16[9] gyro_raw # Raw sensor values of angular velocity
|
||||
float32[9] gyro_rad_s # Angular velocity in radian per seconds
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp
|
||||
uint64 integral_dt # integration time
|
||||
uint64 error_count
|
||||
float32 x # angular velocity in the NED X board axis in rad/s
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp
|
||||
uint64 error_count
|
||||
float32 x
|
||||
float32 y
|
||||
|
|
|
@ -1,3 +1,2 @@
|
|||
uint64 timestamp # microseconds since system boot
|
||||
float32 voltage_v # Servo rail voltage in volts
|
||||
float32 rssi_v # RSSI pin voltage in volts
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # microseconds since system boot
|
||||
float32 voltage5V_v # peripheral 5V rail voltage
|
||||
uint8 usb_connected # USB is connected when 1
|
||||
uint8 brick_valid # brick power is good when 1
|
||||
|
|
|
@ -6,7 +6,6 @@ uint8 TECS_MODE_LAND_THROTTLELIM = 4
|
|||
uint8 TECS_MODE_BAD_DESCENT = 5
|
||||
uint8 TECS_MODE_CLIMBOUT = 6
|
||||
|
||||
uint64 timestamp # timestamp, in microseconds since system start */
|
||||
|
||||
float32 altitudeSp
|
||||
float32 altitude_filtered
|
||||
|
|
|
@ -4,7 +4,6 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
|
|||
uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
|
||||
uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
|
||||
|
||||
uint64 timestamp
|
||||
uint64 heartbeat_time # Time of last received heartbeat from remote system
|
||||
uint64 telem_time # Time of last received telemetry status packet, 0 for none
|
||||
uint8 type # type of the radio hardware
|
||||
|
|
|
@ -55,7 +55,6 @@
|
|||
import genmsg.msgs
|
||||
import gencpp
|
||||
|
||||
cpp_class = 'px4_%s'%spec.short_name
|
||||
native_type = '%s_s'%spec.short_name
|
||||
topic_name = spec.short_name
|
||||
}@
|
||||
|
@ -74,6 +73,11 @@ topic_name = spec.short_name
|
|||
|
||||
namespace px4
|
||||
{
|
||||
@[for multi_topic in topics]@
|
||||
@{
|
||||
cpp_class = 'px4_%s'%multi_topic
|
||||
}@
|
||||
|
||||
|
||||
class __EXPORT @(cpp_class) :
|
||||
public PX4Message<@(native_type)>
|
||||
|
@ -89,7 +93,8 @@ public:
|
|||
|
||||
~@(cpp_class)() {}
|
||||
|
||||
static PX4TopicHandle handle() {return ORB_ID(@(topic_name));}
|
||||
static PX4TopicHandle handle() {return ORB_ID(@(multi_topic));}
|
||||
};
|
||||
@[end for]
|
||||
|
||||
};
|
||||
|
|
|
@ -0,0 +1,132 @@
|
|||
@###############################################
|
||||
@#
|
||||
@# PX4 ROS compatible message source code
|
||||
@# generation for C++
|
||||
@#
|
||||
@# EmPy template for generating <msg>.h files
|
||||
@# Based on the original template for ROS
|
||||
@#
|
||||
@###############################################
|
||||
@# Start of Template
|
||||
@#
|
||||
@# Context:
|
||||
@# - file_name_in (String) Source file
|
||||
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
|
||||
@# - md5sum (String) MD5Sum of the .msg specification
|
||||
@###############################################
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* Auto-generated by genmsg_cpp from file @file_name_in */
|
||||
|
||||
@{
|
||||
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 = {
|
||||
'int8': 'int8_t',
|
||||
'int16': 'int16_t',
|
||||
'int32': 'int32_t',
|
||||
'int64': 'int64_t',
|
||||
'uint8': 'uint8_t',
|
||||
'uint16': 'uint16_t',
|
||||
'uint32': 'uint32_t',
|
||||
'uint64': 'uint64_t',
|
||||
'float32': 'float',
|
||||
'float64': 'double',
|
||||
'bool': 'bool',
|
||||
'char': 'char',
|
||||
'fence_vertex': 'fence_vertex',
|
||||
'position_setpoint': 'position_setpoint',
|
||||
'esc_report': 'esc_report'
|
||||
}
|
||||
|
||||
msgtype_size_map = {
|
||||
'int8': 8,
|
||||
'int16': 16,
|
||||
'int32': 32,
|
||||
'int64': 64,
|
||||
'uint8': 8,
|
||||
'uint16': 16,
|
||||
'uint32': 32,
|
||||
'uint64': 64,
|
||||
'float32': 32,
|
||||
'float64': 64,
|
||||
'bool': 1,
|
||||
'char': 1,
|
||||
'fence_vertex': 8,
|
||||
'position_setpoint': 104,
|
||||
'esc_report': 36
|
||||
}
|
||||
|
||||
def convert_type(spec_type):
|
||||
bare_type = spec_type
|
||||
if '/' in spec_type:
|
||||
# removing prefix
|
||||
bare_type = (spec_type.split('/'))[1]
|
||||
|
||||
msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
|
||||
c_type = type_map[msg_type]
|
||||
if is_array:
|
||||
return c_type + "[" + str(array_length) + "]"
|
||||
return c_type
|
||||
|
||||
def bare_name(msg_type):
|
||||
bare = msg_type
|
||||
if '/' in msg_type:
|
||||
# removing prefix
|
||||
bare = (msg_type.split('/'))[1]
|
||||
# removing suffix
|
||||
return bare.split('[')[0]
|
||||
|
||||
def sizeof_field_type(field):
|
||||
return msgtype_size_map[bare_name(field.type)]
|
||||
|
||||
sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
|
||||
topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
|
||||
}@
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <drivers/drv_orb_dev.h>
|
||||
#include <uORB/topics/@(topic_name).h>
|
||||
|
||||
@# 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, __orb_@(topic_name)_fields);
|
||||
@[end for]
|
|
@ -75,7 +75,6 @@ for field in spec.parsed_fields():
|
|||
(package, name) = genmsg.names.package_resource_name(field.base_type)
|
||||
package = package or spec.package # convert '' to package
|
||||
print('#include <uORB/topics/%s.h>'%(name))
|
||||
|
||||
}@
|
||||
|
||||
@# Constants c style
|
||||
|
@ -85,17 +84,12 @@ for field in spec.parsed_fields():
|
|||
@[end for]
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @@addtogroup topics
|
||||
* @@{
|
||||
*/
|
||||
|
||||
@##############################
|
||||
@# Main struct of message
|
||||
@##############################
|
||||
@{
|
||||
|
||||
type_map = {'int8': 'int8_t',
|
||||
type_map = {
|
||||
'int8': 'int8_t',
|
||||
'int16': 'int16_t',
|
||||
'int32': 'int32_t',
|
||||
'int64': 'int64_t',
|
||||
|
@ -109,37 +103,77 @@ type_map = {'int8': 'int8_t',
|
|||
'char': 'char',
|
||||
'fence_vertex': 'fence_vertex',
|
||||
'position_setpoint': 'position_setpoint',
|
||||
'esc_report': 'esc_report'}
|
||||
'esc_report': 'esc_report'
|
||||
}
|
||||
|
||||
msgtype_size_map = {
|
||||
'int8': 8,
|
||||
'int16': 16,
|
||||
'int32': 32,
|
||||
'int64': 64,
|
||||
'uint8': 8,
|
||||
'uint16': 16,
|
||||
'uint32': 32,
|
||||
'uint64': 64,
|
||||
'float32': 32,
|
||||
'float64': 64,
|
||||
'bool': 1,
|
||||
'char': 1,
|
||||
'fence_vertex': 8,
|
||||
'position_setpoint': 104,
|
||||
'esc_report': 36
|
||||
}
|
||||
|
||||
def bare_name(msg_type):
|
||||
bare = msg_type
|
||||
if '/' in msg_type:
|
||||
# removing prefix
|
||||
bare = (msg_type.split('/'))[1]
|
||||
# removing suffix
|
||||
return bare.split('[')[0]
|
||||
|
||||
def sizeof_field_type(field):
|
||||
return msgtype_size_map[bare_name(field.type)]
|
||||
|
||||
# Function to print a standard ros type
|
||||
def print_field_def(field):
|
||||
type = field.type
|
||||
type_name = field.type
|
||||
# detect embedded types
|
||||
sl_pos = type.find('/')
|
||||
sl_pos = type_name.find('/')
|
||||
type_appendix = ''
|
||||
type_prefix = ''
|
||||
if (sl_pos >= 0):
|
||||
type = type[sl_pos + 1:]
|
||||
type_name = type_name[sl_pos + 1:]
|
||||
type_prefix = 'struct '
|
||||
type_appendix = '_s'
|
||||
|
||||
# detect arrays
|
||||
a_pos = type.find('[')
|
||||
a_pos = type_name.find('[')
|
||||
array_size = ''
|
||||
if (a_pos >= 0):
|
||||
# field is array
|
||||
array_size = type[a_pos:]
|
||||
type = type[:a_pos]
|
||||
array_size = type_name[a_pos:]
|
||||
type_name = type_name[:a_pos]
|
||||
|
||||
if type in type_map:
|
||||
if type_name in type_map:
|
||||
# need to add _t: int8 --> int8_t
|
||||
type_px4 = type_map[type]
|
||||
type_px4 = type_map[type_name]
|
||||
else:
|
||||
raise Exception("Type {0} not supported, add to to template file!".format(type))
|
||||
raise Exception("Type {0} not supported, add to to template file!".format(type_name))
|
||||
|
||||
print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
|
||||
|
||||
}
|
||||
def print_parsed_fields():
|
||||
# sort fields
|
||||
sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
|
||||
# loop over all fields and print the type and name
|
||||
for field in sorted_fields:
|
||||
if (not field.is_header):
|
||||
print_field_def(field)
|
||||
|
||||
}@
|
||||
|
||||
// #pragma pack(push, 1) badly breaks alignment everywhere!
|
||||
#ifdef __cplusplus
|
||||
@#class @(uorb_struct) {
|
||||
struct __EXPORT @(uorb_struct) {
|
||||
|
@ -147,31 +181,27 @@ struct __EXPORT @(uorb_struct) {
|
|||
#else
|
||||
struct @(uorb_struct) {
|
||||
#endif
|
||||
@{
|
||||
# loop over all fields and print the type and name
|
||||
for field in spec.parsed_fields():
|
||||
if (not field.is_header):
|
||||
print_field_def(field)
|
||||
}@
|
||||
@# timestamp at the beginning of each topic is required for logger
|
||||
uint64_t timestamp; // required for logger
|
||||
@print_parsed_fields()
|
||||
#ifdef __cplusplus
|
||||
@# Constants again c++-ified
|
||||
@{
|
||||
for constant in spec.constants:
|
||||
type = constant.type
|
||||
if type in type_map:
|
||||
type_name = constant.type
|
||||
if type_name in type_map:
|
||||
# need to add _t: int8 --> int8_t
|
||||
type_px4 = type_map[type]
|
||||
type_px4 = type_map[type_name]
|
||||
else:
|
||||
raise Exception("Type {0} not supported, add to to template file!".format(type))
|
||||
raise Exception("Type {0} not supported, add to to template file!".format(type_name))
|
||||
|
||||
print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
/**
|
||||
* @@}
|
||||
*/
|
||||
//#pragma pack(pop)
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(@(topic_name));
|
||||
@[for multi_topic in topics]@
|
||||
ORB_DECLARE(@multi_topic);
|
||||
@[end for]
|
||||
|
|
|
@ -0,0 +1,70 @@
|
|||
@###############################################
|
||||
@#
|
||||
@# EmPy template for generating uORBTopics.cpp file
|
||||
@# for logging purposes
|
||||
@#
|
||||
@###############################################
|
||||
@# Start of Template
|
||||
@#
|
||||
@# Context:
|
||||
@# - msgs (List) list of all msg files
|
||||
@###############################################
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <uORB/uORBTopics.h>
|
||||
#include <uORB/uORB.h>
|
||||
@{
|
||||
msgs_count = len(msgs)
|
||||
msg_names = [mn.replace(".msg", "") for mn in msgs]
|
||||
}@
|
||||
@[for msg_name in msg_names]@
|
||||
#include <uORB/topics/@(msg_name).h>
|
||||
@[end for]
|
||||
|
||||
const size_t _uorb_topics_count = @(msgs_count);
|
||||
const struct orb_metadata* _uorb_topics_list[_uorb_topics_count] = {
|
||||
@[for idx, msg_name in enumerate(msg_names, 1)]@
|
||||
ORB_ID(@(msg_name))@[if idx != msgs_count],@[end if]
|
||||
@[end for]
|
||||
};
|
||||
|
||||
size_t orb_topics_count()
|
||||
{
|
||||
return _uorb_topics_count;
|
||||
}
|
||||
|
||||
const struct orb_metadata **orb_get_topics()
|
||||
{
|
||||
return _uorb_topics_list;
|
||||
}
|
|
@ -1,5 +1,4 @@
|
|||
uint8 NUM_MOTOR_OUTPUTS = 8
|
||||
|
||||
uint64 timestamp # output timestamp in us since system boot
|
||||
uint32 motor_number # number of motor to spin
|
||||
float32 value # output power, range [0..1]
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # Timestamp of this position report
|
||||
uint32 ICAO_address # ICAO address
|
||||
float64 lat # Latitude, expressed as degrees
|
||||
float64 lon # Longitude, expressed as degrees
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
# This is similar to the mavlink message ATTITUDE, but for onboard use */
|
||||
uint64 timestamp # in microseconds since system start
|
||||
# @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional
|
||||
float32 roll # Roll angle (rad, Tait-Bryan, NED)
|
||||
float32 pitch # Pitch angle (rad, Tait-Bryan, NED)
|
||||
|
|
|
@ -8,7 +8,6 @@
|
|||
#
|
||||
###############################################################################################
|
||||
|
||||
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
|
||||
|
||||
float32 roll_body # body angle in NED frame
|
||||
float32 pitch_body # body angle in NED frame
|
||||
|
@ -35,3 +34,6 @@ bool fw_control_yaw # control heading with rudder (used for auto takeoff on
|
|||
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
|
||||
|
||||
bool apply_flaps
|
||||
|
||||
# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
|
||||
# TOPICS vehicle_attitude_setpoint
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
|
||||
uint64 timestamp # in microseconds since system start
|
||||
# is set whenever the writing thread stores new data
|
||||
|
||||
bool flag_armed
|
||||
|
|
|
@ -4,7 +4,6 @@
|
|||
# estimator, which will take more sources of information into account than just GPS,
|
||||
# e.g. control inputs of the vehicle in a Kalman-filter implementation.
|
||||
#
|
||||
uint64 timestamp # Time of this estimate since system start, (microseconds)
|
||||
uint64 time_utc_usec # GPS UTC timestamp, (microseconds)
|
||||
float64 lat # Latitude, (degrees)
|
||||
float64 lon # Longitude, (degrees)
|
||||
|
|
|
@ -1,3 +1,2 @@
|
|||
uint64 timestamp # timestamp of the setpoint
|
||||
bool landed # true if vehicle is currently landed on the ground
|
||||
bool freefall # true if vehicle is currently in free-fall
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
# Fused local position in NED.
|
||||
|
||||
uint64 timestamp # Time of this estimate since system start, (microseconds)
|
||||
bool xy_valid # true if x and y are valid
|
||||
bool z_valid # true if z is valid
|
||||
bool v_xy_valid # true if vy and vy are valid
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
# Local position setpoint in NED frame
|
||||
|
||||
uint64 timestamp # timestamp of the setpoint
|
||||
float32 x # in meters NED
|
||||
float32 y # in meters NED
|
||||
float32 z # in meters NED
|
||||
|
|
|
@ -8,7 +8,6 @@
|
|||
#
|
||||
###############################################################################################
|
||||
|
||||
uint64 timestamp # in microseconds since system start
|
||||
|
||||
float32 roll # body angular rates in NED frame
|
||||
float32 pitch # body angular rates in NED frame
|
||||
|
|
|
@ -41,7 +41,6 @@ uint8 RC_IN_MODE_GENERATED = 2
|
|||
|
||||
# state machine / state of vehicle.
|
||||
# Encodes the complete system state and is set by the commander app.
|
||||
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
|
||||
|
||||
uint8 nav_state # set navigation state machine to specified value
|
||||
uint8 arming_state # current arming state
|
||||
|
|
|
@ -5,7 +5,6 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2
|
|||
uint8 VEHICLE_VTOL_STATE_MC = 3
|
||||
uint8 VEHICLE_VTOL_STATE_FW = 4
|
||||
|
||||
uint64 timestamp # Microseconds since system boot
|
||||
bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode
|
||||
bool vtol_in_trans_mode
|
||||
bool vtol_transition_failsafe # vtol in transition failsafe mode
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
uint64 timestamp # Microseconds since system boot
|
||||
float32 windspeed_north # Wind component in north / X direction
|
||||
float32 windspeed_east # Wind component in east / Y direction
|
||||
float32 covariance_north # Uncertainty - set to zero (no uncertainty) if not estimated
|
||||
|
|
|
@ -375,12 +375,11 @@ Airspeed::update_status()
|
|||
{
|
||||
if (_sensor_ok != _last_published_sensor_ok) {
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
_sensor_ok,
|
||||
subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE
|
||||
};
|
||||
struct subsystem_info_s info = {};
|
||||
info.present = true;
|
||||
info.enabled = true;
|
||||
info.ok = _sensor_ok;
|
||||
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
|
||||
|
||||
if (_subsys_pub != nullptr) {
|
||||
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
|
||||
|
|
|
@ -55,10 +55,6 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/safety.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
||||
|
|
|
@ -616,12 +616,12 @@ MB12XX::start()
|
|||
work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
|
||||
};
|
||||
struct subsystem_info_s info = {};
|
||||
info.present = true;
|
||||
info.enabled = true;
|
||||
info.ok = true;
|
||||
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
|
||||
|
||||
static orb_advert_t pub = nullptr;
|
||||
|
||||
if (pub != nullptr) {
|
||||
|
|
|
@ -75,7 +75,6 @@
|
|||
#include <systemlib/mixer/mixer.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
|
|
@ -77,10 +77,6 @@
|
|||
#include <systemlib/mixer/mixer.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
|
||||
|
|
|
@ -592,12 +592,12 @@ PX4FLOW::start()
|
|||
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW
|
||||
};
|
||||
struct subsystem_info_s info = {};
|
||||
info.present = true;
|
||||
info.enabled = true;
|
||||
info.ok = true;
|
||||
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW;
|
||||
|
||||
static orb_advert_t pub = nullptr;
|
||||
|
||||
if (pub != nullptr) {
|
||||
|
|
|
@ -79,10 +79,6 @@
|
|||
#include <lib/rc/sumd.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
|
|
@ -77,10 +77,6 @@
|
|||
#include <systemlib/battery.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/safety.h>
|
||||
|
|
|
@ -54,7 +54,6 @@
|
|||
#include <arch/board/board.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
|
|
@ -51,7 +51,6 @@
|
|||
#include <termios.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <v1.0/mavlink_types.h>
|
||||
#include <v1.0/common/mavlink.h>
|
||||
|
||||
|
|
|
@ -618,12 +618,12 @@ SRF02::start()
|
|||
work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
|
||||
};
|
||||
struct subsystem_info_s info = {};
|
||||
info.present = true;
|
||||
info.enabled = true;
|
||||
info.ok = true;
|
||||
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
|
||||
|
||||
static orb_advert_t pub = nullptr;
|
||||
|
||||
if (pub != nullptr) {
|
||||
|
|
|
@ -628,12 +628,12 @@ TRONE::start()
|
|||
work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
|
||||
};
|
||||
struct subsystem_info_s info = {};
|
||||
info.present = true;
|
||||
info.enabled = true;
|
||||
info.ok = true;
|
||||
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
|
||||
|
||||
static orb_advert_t pub = nullptr;
|
||||
|
||||
if (pub != nullptr) {
|
||||
|
|
|
@ -61,10 +61,6 @@
|
|||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
|
|
@ -47,10 +47,6 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
__EXPORT int ex_hwtest_main(int argc, char *argv[]);
|
||||
|
|
|
@ -58,10 +58,6 @@
|
|||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
|
|
@ -47,10 +47,6 @@
|
|||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
|
|
@ -57,10 +57,6 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/wind_estimate.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
|
|
@ -81,10 +81,6 @@
|
|||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
|
|
|
@ -62,8 +62,6 @@
|
|||
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_virtual_fw.h>
|
||||
#include <uORB/topics/actuator_controls_virtual_mc.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/fw_virtual_rates_setpoint.h>
|
||||
#include <uORB/topics/mc_virtual_rates_setpoint.h>
|
||||
|
|
|
@ -47,8 +47,6 @@
|
|||
#include "mavlink_parameters.h"
|
||||
#include "mavlink_main.h"
|
||||
|
||||
ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
|
||||
ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
|
||||
#define HASH_PARAM "_HASH_CHECK"
|
||||
|
||||
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
|
|
|
@ -70,8 +70,6 @@
|
|||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_virtual_fw.h>
|
||||
#include <uORB/topics/actuator_controls_virtual_mc.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/fw_virtual_rates_setpoint.h>
|
||||
#include <uORB/topics/mc_virtual_rates_setpoint.h>
|
||||
|
|
|
@ -52,10 +52,6 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
|
|
|
@ -78,10 +78,6 @@
|
|||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls_0.h>
|
||||
#include <uORB/topics/actuator_controls_1.h>
|
||||
#include <uORB/topics/actuator_controls_2.h>
|
||||
#include <uORB/topics/actuator_controls_3.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
|
|
@ -136,9 +136,6 @@ UT_array *param_values;
|
|||
/** array info for the modified parameters array */
|
||||
const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
|
||||
|
||||
/** parameter update topic */
|
||||
ORB_DEFINE(parameter_update, struct parameter_update_s);
|
||||
|
||||
/** parameter update topic handle */
|
||||
static orb_advert_t param_topic = NULL;
|
||||
|
||||
|
|
|
@ -158,9 +158,6 @@ UT_array *param_values;
|
|||
/** array info for the modified parameters array */
|
||||
const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
|
||||
|
||||
/** parameter update topic */
|
||||
ORB_DEFINE(parameter_update, struct parameter_update_s);
|
||||
|
||||
/** parameter update topic handle */
|
||||
static orb_advert_t param_topic = NULL;
|
||||
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue