diff options
89 files changed, 2577 insertions, 293 deletions
diff --git a/.gitignore b/.gitignore index 8b09e4783..9d9c3d383 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,7 @@ .~lock.* Archives/* Build/* +build/* core cscope.out Firmware.sublime-workspace @@ -38,3 +39,4 @@ tags .pydevproject .ropeproject *.orig +src/modules/uORB/topics/* diff --git a/.gitmodules b/.gitmodules index 4b84afac2..4996b274b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,9 @@ [submodule "uavcan"] path = uavcan url = git://github.com/pavel-kirienko/uavcan.git +[submodule "Tools/genmsg"] + path = Tools/genmsg + url = https://github.com/ros/genmsg.git +[submodule "Tools/gencpp"] + path = Tools/gencpp + url = https://github.com/ros/gencpp.git diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..636cc07e3 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,185 @@ +cmake_minimum_required(VERSION 2.8.3) +project(px4) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + px4_msgs/rc_channels.msg + px4_msgs/vehicle_attitude.msg + px4_msgs/rc_channels.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS src/include + LIBRARIES px4 + CATKIN_DEPENDS roscpp rospy std_msgs + DEPENDS system_lib + CATKIN_DEPENDS message_runtime +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} + src/platforms + src/include +) + +## Declare a cpp library +add_library(px4 + src/platforms/ros/px4_ros_impl.cpp +) + +target_link_libraries(px4 + ${catkin_LIBRARIES} +) + +## Declare a test publisher +add_executable(publisher src/examples/publisher/publisher.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(publisher px4_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(publisher + ${catkin_LIBRARIES} + px4 +) + +## Declare a test subscriber +add_executable(subscriber src/examples/subscriber/subscriber.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(subscriber px4_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(subscriber + ${catkin_LIBRARIES} + px4 +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS px4 publisher subscriber + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) @@ -104,13 +104,13 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: checksubmodules $(DESIRED_FIRMWARES) +all: checksubmodules generateuorbtopicheaders $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. # -# XXX copying the .bin files is a hack to work around the PX4IO uploader -# not supporting .px4 files, and it should be deprecated onced that +# XXX copying the .bin files is a hack to work around the PX4IO uploader +# not supporting .px4 files, and it should be deprecated onced that # is taken care of. # $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 @@ -152,7 +152,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) # Build the NuttX export archives. # # Note that there are no explicit dependencies extended from these -# archives. If NuttX is updated, the user is expected to rebuild the +# archives. If NuttX is updated, the user is expected to rebuild the # archives/build area manually. Likewise, when the 'archives' target is # invoked, all archives are always rebuilt. # @@ -224,6 +224,22 @@ updatesubmodules: $(Q) (git submodule init) $(Q) (git submodule update) +MSG_DIR = $(PX4_BASE)msg/px4_msgs +MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates +TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics +TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary +GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src +GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src + +.PHONY: generateuorbtopicheaders +generateuorbtopicheaders: + @$(ECHO) "Generating uORB topic headers" + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) +# clean up temporary files + $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) + # # Testing targets # @@ -232,7 +248,7 @@ testbuild: # # Cleanup targets. 'clean' should remove all built products and force -# a complete re-compilation, 'distclean' should remove everything +# a complete re-compilation, 'distclean' should remove everything # that's generated leaving only files that are in source control. # .PHONY: clean diff --git a/Tools/gencpp b/Tools/gencpp new file mode 160000 +Subproject 26a86f04bcec0018af6652b3ddd3f680e6e3fa2 diff --git a/Tools/genmsg b/Tools/genmsg new file mode 160000 +Subproject 72f0383f0e6a489214c51802ae12d6e271b1e45 diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py new file mode 100755 index 000000000..2ddbd6984 --- /dev/null +++ b/Tools/px_generate_uorb_topic_headers.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python +############################################################################# +# +# Copyright (C) 2013-2014 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_headers.py +Generates c/cpp header files for uorb topics from .msg (ROS syntax) +message files +""" +from __future__ import print_function +import os +import shutil +import filecmp +import argparse +import genmsg.template_tools + +__author__ = "Thomas Gubler" +__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team." +__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' + + +def convert_file(filename, outputdir, templatedir, includepath): + """ + Converts a single .msg file to a uorb header + """ + print("Generating uORB headers from {0}".format(filename)) + genmsg.template_tools.generate_from_file(filename, + package, + outputdir, + templatedir, + includepath, + msg_template_map, + srv_template_map) + + +def convert_dir(inputdir, outputdir, templatedir): + """ + Converts all .msg files in inputdir to uORB header files + """ + includepath = incl_default + [':'.join([package, inputdir])] + for f in os.listdir(inputdir): + fn = os.path.join(inputdir, f) + if os.path.isfile(fn): + convert_file( + fn, + outputdir, + templatedir, + includepath) + + +def copy_changed(inputdir, outputdir): + """ + Copies files from inputdir to outputdir if they don't exist in + ouputdir or if their content changed + """ + for f in os.listdir(inputdir): + fni = os.path.join(inputdir, f) + if os.path.isfile(fni): + # Check if f exists in outpoutdir, copy the file if not + fno = os.path.join(outputdir, f) + if not os.path.isfile(fno): + shutil.copy(fni, fno) + print("{0}: new header file".format(f)) + continue + # The file exists in inputdir and outputdir + # only copy if contents do not match + if not filecmp.cmp(fni, fno): + shutil.copy(fni, fno) + print("{0}: updated".format(f)) + continue + + print("{0}: unchanged".format(f)) + +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): + """ + Converts all .msg files in inputdir to uORB header files + Unchanged existing files are not overwritten. + """ + # Create new headers in temporary output directory + convert_dir(inputdir, temporarydir, templatedir) + + # Copy changed headers from temporary dir to output dir + copy_changed(temporarydir, outputdir) + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description='Convert msg files to uorb headers') + 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 header files') + parser.add_argument('-t', dest='temporarydir', + help='temporary directory') + args = parser.parse_args() + + if args.file is not None: + for f in args.file: + convert_file( + f, + args.outputdir, + args.templatedir, + incl_default) + elif args.dir is not None: + convert_dir_save( + args.dir, + args.outputdir, + args.templatedir, + args.temporarydir) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 6f54b960c..844f1236a 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -34,9 +34,30 @@ MODULES += systemcmds/mtd MODULES += systemcmds/ver # -# Testing modules +# System commands +# +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver + +# +# Example modules # MODULES += examples/matlab_csv_serial +MODULES += examples/subscriber +MODULES += examples/publisher # # Library modules @@ -44,10 +65,11 @@ MODULES += examples/matlab_csv_serial MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB -LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +LIBRARIES += lib/mathlib/CMSIS +MODULES += platforms/nuttx # # Modules to test-build, but not useful for test environment @@ -59,8 +81,6 @@ MODULES += examples/flow_position_estimator # # Libraries # -LIBRARIES += lib/mathlib/CMSIS - MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests MODULES += modules/commander/commander_tests diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 4bfa7a087..435c240bf 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -37,13 +37,14 @@ # Some useful paths. # # Note that in general we always keep directory paths with the separator -# at the end, and join paths without explicit separators. This reduces +# at the end, and join paths without explicit separators. This reduces # the number of duplicate slashes we have lying around in paths, # and is consistent with joining the results of $(dir) and $(notdir). # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ +export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/ @@ -61,7 +62,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ $(PX4_INCLUDE_DIR) \ - $(PX4_LIB_DIR) + $(PX4_LIB_DIR) \ + $(PX4_PLATFORMS_DIR) # # Tools diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg new file mode 100644 index 000000000..4e0e5b494 --- /dev/null +++ b/msg/px4_msgs/rc_channels.msg @@ -0,0 +1,24 @@ +int32 RC_CHANNELS_FUNCTION_MAX=18 +uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 +uint8 RC_CHANNELS_FUNCTION_ROLL=1 +uint8 RC_CHANNELS_FUNCTION_PITCH=2 +uint8 RC_CHANNELS_FUNCTION_YAW=3 +uint8 RC_CHANNELS_FUNCTION_MODE=4 +uint8 RC_CHANNELS_FUNCTION_RETURN=5 +uint8 RC_CHANNELS_FUNCTION_POSCTL=6 +uint8 RC_CHANNELS_FUNCTION_LOITER=7 +uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8 +uint8 RC_CHANNELS_FUNCTION_ACRO=9 +uint8 RC_CHANNELS_FUNCTION_FLAPS=10 +uint8 RC_CHANNELS_FUNCTION_AUX_1=11 +uint8 RC_CHANNELS_FUNCTION_AUX_2=12 +uint8 RC_CHANNELS_FUNCTION_AUX_3=13 +uint8 RC_CHANNELS_FUNCTION_AUX_4=14 +uint8 RC_CHANNELS_FUNCTION_AUX_5=15 +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 +int8[18] function # Functions mapping +uint8 rssi # Receive signal strength index +bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/px4_msgs/unused/actuator_armed.msg b/msg/px4_msgs/unused/actuator_armed.msg new file mode 100644 index 000000000..f6bf58307 --- /dev/null +++ b/msg/px4_msgs/unused/actuator_armed.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # Microseconds since system boot +bool armed # Set to true if system is armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/px4_msgs/vehicle_attitude.msg b/msg/px4_msgs/vehicle_attitude.msg new file mode 100644 index 000000000..98018a1df --- /dev/null +++ b/msg/px4_msgs/vehicle_attitude.msg @@ -0,0 +1,18 @@ +# 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) +float32 yaw # Yaw angle (rad, Tait-Bryan, NED) +float32 rollspeed # Roll angular speed (rad/s, Tait-Bryan, NED) +float32 pitchspeed # Pitch angular speed (rad/s, Tait-Bryan, NED) +float32 yawspeed # Yaw angular speed (rad/s, Tait-Bryan, NED) +float32 rollacc # Roll angular accelration (rad/s, Tait-Bryan, NED) +float32 pitchacc # Pitch angular acceleration (rad/s, Tait-Bryan, NED) +float32 yawacc # Yaw angular acceleration (rad/s, Tait-Bryan, NED) +float32[3] rate_offsets # Offsets of the body angular rates from zero +float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) +float32[4] q # Quaternion (NED) +float32[3] g_comp # Compensated gravity vector +bool R_valid # Rotation matrix valid +bool q_valid # Quaternion valid diff --git a/msg/std_msgs/Bool.msg b/msg/std_msgs/Bool.msg new file mode 100644 index 000000000..f7cabb94f --- /dev/null +++ b/msg/std_msgs/Bool.msg @@ -0,0 +1 @@ +bool data
\ No newline at end of file diff --git a/msg/std_msgs/Byte.msg b/msg/std_msgs/Byte.msg new file mode 100644 index 000000000..d993b3455 --- /dev/null +++ b/msg/std_msgs/Byte.msg @@ -0,0 +1 @@ +byte data diff --git a/msg/std_msgs/ByteMultiArray.msg b/msg/std_msgs/ByteMultiArray.msg new file mode 100644 index 000000000..bb00bd348 --- /dev/null +++ b/msg/std_msgs/ByteMultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +byte[] data # array of data + diff --git a/msg/std_msgs/Char.msg b/msg/std_msgs/Char.msg new file mode 100644 index 000000000..39a1d46a9 --- /dev/null +++ b/msg/std_msgs/Char.msg @@ -0,0 +1 @@ +char data
\ No newline at end of file diff --git a/msg/std_msgs/ColorRGBA.msg b/msg/std_msgs/ColorRGBA.msg new file mode 100644 index 000000000..182dbc834 --- /dev/null +++ b/msg/std_msgs/ColorRGBA.msg @@ -0,0 +1,4 @@ +float32 r +float32 g +float32 b +float32 a diff --git a/msg/std_msgs/Duration.msg b/msg/std_msgs/Duration.msg new file mode 100644 index 000000000..f13931ec8 --- /dev/null +++ b/msg/std_msgs/Duration.msg @@ -0,0 +1 @@ +duration data diff --git a/msg/std_msgs/Empty.msg b/msg/std_msgs/Empty.msg new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/msg/std_msgs/Empty.msg diff --git a/msg/std_msgs/Float32.msg b/msg/std_msgs/Float32.msg new file mode 100644 index 000000000..e89740534 --- /dev/null +++ b/msg/std_msgs/Float32.msg @@ -0,0 +1 @@ +float32 data
\ No newline at end of file diff --git a/msg/std_msgs/Float32MultiArray.msg b/msg/std_msgs/Float32MultiArray.msg new file mode 100644 index 000000000..915830846 --- /dev/null +++ b/msg/std_msgs/Float32MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +float32[] data # array of data + diff --git a/msg/std_msgs/Float64.msg b/msg/std_msgs/Float64.msg new file mode 100644 index 000000000..cd09d39b8 --- /dev/null +++ b/msg/std_msgs/Float64.msg @@ -0,0 +1 @@ +float64 data
\ No newline at end of file diff --git a/msg/std_msgs/Float64MultiArray.msg b/msg/std_msgs/Float64MultiArray.msg new file mode 100644 index 000000000..0a13b928f --- /dev/null +++ b/msg/std_msgs/Float64MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +float64[] data # array of data + diff --git a/msg/std_msgs/Header.msg b/msg/std_msgs/Header.msg new file mode 100644 index 000000000..f90d622ea --- /dev/null +++ b/msg/std_msgs/Header.msg @@ -0,0 +1,15 @@ +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id diff --git a/msg/std_msgs/Int16.msg b/msg/std_msgs/Int16.msg new file mode 100644 index 000000000..c4389faf7 --- /dev/null +++ b/msg/std_msgs/Int16.msg @@ -0,0 +1 @@ +int16 data diff --git a/msg/std_msgs/Int16MultiArray.msg b/msg/std_msgs/Int16MultiArray.msg new file mode 100644 index 000000000..d2ddea1d1 --- /dev/null +++ b/msg/std_msgs/Int16MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int16[] data # array of data + diff --git a/msg/std_msgs/Int32.msg b/msg/std_msgs/Int32.msg new file mode 100644 index 000000000..0ecfe35f5 --- /dev/null +++ b/msg/std_msgs/Int32.msg @@ -0,0 +1 @@ +int32 data
\ No newline at end of file diff --git a/msg/std_msgs/Int32MultiArray.msg b/msg/std_msgs/Int32MultiArray.msg new file mode 100644 index 000000000..af60abda3 --- /dev/null +++ b/msg/std_msgs/Int32MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int32[] data # array of data + diff --git a/msg/std_msgs/Int64.msg b/msg/std_msgs/Int64.msg new file mode 100644 index 000000000..6961e00f5 --- /dev/null +++ b/msg/std_msgs/Int64.msg @@ -0,0 +1 @@ +int64 data
\ No newline at end of file diff --git a/msg/std_msgs/Int64MultiArray.msg b/msg/std_msgs/Int64MultiArray.msg new file mode 100644 index 000000000..f4f35e171 --- /dev/null +++ b/msg/std_msgs/Int64MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int64[] data # array of data + diff --git a/msg/std_msgs/Int8.msg b/msg/std_msgs/Int8.msg new file mode 100644 index 000000000..1e42e554f --- /dev/null +++ b/msg/std_msgs/Int8.msg @@ -0,0 +1 @@ +int8 data diff --git a/msg/std_msgs/Int8MultiArray.msg b/msg/std_msgs/Int8MultiArray.msg new file mode 100644 index 000000000..a59a37259 --- /dev/null +++ b/msg/std_msgs/Int8MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +int8[] data # array of data + diff --git a/msg/std_msgs/MultiArrayDimension.msg b/msg/std_msgs/MultiArrayDimension.msg new file mode 100644 index 000000000..08240462c --- /dev/null +++ b/msg/std_msgs/MultiArrayDimension.msg @@ -0,0 +1,3 @@ +string label # label of given dimension +uint32 size # size of given dimension (in type units) +uint32 stride # stride of given dimension
\ No newline at end of file diff --git a/msg/std_msgs/MultiArrayLayout.msg b/msg/std_msgs/MultiArrayLayout.msg new file mode 100644 index 000000000..5437f8542 --- /dev/null +++ b/msg/std_msgs/MultiArrayLayout.msg @@ -0,0 +1,26 @@ +# The multiarray declares a generic multi-dimensional array of a +# particular data type. Dimensions are ordered from outer most +# to inner most. + +MultiArrayDimension[] dim # Array of dimension properties +uint32 data_offset # padding bytes at front of data + +# Accessors should ALWAYS be written in terms of dimension stride +# and specified outer-most dimension first. +# +# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k] +# +# A standard, 3-channel 640x480 image with interleaved color channels +# would be specified as: +# +# dim[0].label = "height" +# dim[0].size = 480 +# dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image) +# dim[1].label = "width" +# dim[1].size = 640 +# dim[1].stride = 3*640 = 1920 +# dim[2].label = "channel" +# dim[2].size = 3 +# dim[2].stride = 3 +# +# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.
\ No newline at end of file diff --git a/msg/std_msgs/String.msg b/msg/std_msgs/String.msg new file mode 100644 index 000000000..ae721739e --- /dev/null +++ b/msg/std_msgs/String.msg @@ -0,0 +1 @@ +string data diff --git a/msg/std_msgs/Time.msg b/msg/std_msgs/Time.msg new file mode 100644 index 000000000..7f8f72171 --- /dev/null +++ b/msg/std_msgs/Time.msg @@ -0,0 +1 @@ +time data diff --git a/msg/std_msgs/UInt16.msg b/msg/std_msgs/UInt16.msg new file mode 100644 index 000000000..87d0c44eb --- /dev/null +++ b/msg/std_msgs/UInt16.msg @@ -0,0 +1 @@ +uint16 data diff --git a/msg/std_msgs/UInt16MultiArray.msg b/msg/std_msgs/UInt16MultiArray.msg new file mode 100644 index 000000000..f38970b65 --- /dev/null +++ b/msg/std_msgs/UInt16MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint16[] data # array of data + diff --git a/msg/std_msgs/UInt32.msg b/msg/std_msgs/UInt32.msg new file mode 100644 index 000000000..b6c696b42 --- /dev/null +++ b/msg/std_msgs/UInt32.msg @@ -0,0 +1 @@ +uint32 data
\ No newline at end of file diff --git a/msg/std_msgs/UInt32MultiArray.msg b/msg/std_msgs/UInt32MultiArray.msg new file mode 100644 index 000000000..b2bb0771f --- /dev/null +++ b/msg/std_msgs/UInt32MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint32[] data # array of data + diff --git a/msg/std_msgs/UInt64.msg b/msg/std_msgs/UInt64.msg new file mode 100644 index 000000000..2eb1afad3 --- /dev/null +++ b/msg/std_msgs/UInt64.msg @@ -0,0 +1 @@ +uint64 data
\ No newline at end of file diff --git a/msg/std_msgs/UInt64MultiArray.msg b/msg/std_msgs/UInt64MultiArray.msg new file mode 100644 index 000000000..30d0cd928 --- /dev/null +++ b/msg/std_msgs/UInt64MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint64[] data # array of data + diff --git a/msg/std_msgs/UInt8.msg b/msg/std_msgs/UInt8.msg new file mode 100644 index 000000000..5eefd870d --- /dev/null +++ b/msg/std_msgs/UInt8.msg @@ -0,0 +1 @@ +uint8 data diff --git a/msg/std_msgs/UInt8MultiArray.msg b/msg/std_msgs/UInt8MultiArray.msg new file mode 100644 index 000000000..31f7d6a21 --- /dev/null +++ b/msg/std_msgs/UInt8MultiArray.msg @@ -0,0 +1,6 @@ +# Please look at the MultiArrayLayout message definition for +# documentation on all multiarrays. + +MultiArrayLayout layout # specification of data layout +uint8[] data # array of data + diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template new file mode 100644 index 000000000..5fce506b6 --- /dev/null +++ b/msg/templates/msg.h.template @@ -0,0 +1,152 @@ +@############################################### +@# +@# 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-2014 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 + +cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace +cpp_class = '%s_'%spec.short_name +cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) +cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name) +cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include <stdint.h> +#include "../uORB.h" + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (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 +@[for constant in spec.constants]@ +#define @(constant.name) @(int(constant.val)) +@[end for] + +/** + * @@addtogroup topics + * @@{ + */ + +@############################## +@# Main struct of message +@############################## +@{ + +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', + 'bool': 'bool'} + +# Function to print a standard ros type +def print_field_def(field): + type = field.type + # detect embedded types + sl_pos = type.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type = type[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type[a_pos:] + type = type[:a_pos] + + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) + +} +struct @(spec.short_name)_s { +@{ +# 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) +}@ +}; + +/** + * @@} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(@(spec.short_name)); diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index f3ce53b4a..5ae27aa00 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -90,7 +90,7 @@ else ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) else - # Linux/Cygwin-native toolchain + # Linux/Cygwin-native toolchain MKDEP = $(TOPDIR)/tools/mkdeps.sh ARCHINCLUDES = -I. -isystem $(TOPDIR)/include ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx @@ -117,7 +117,7 @@ ARCHOPTIMIZATION += -g endif ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++11 ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..bc23cdd18 --- /dev/null +++ b/package.xml @@ -0,0 +1,59 @@ +<?xml version="1.0"?> +<package> + <name>px4</name> + <version>1.0.0</version> + <description>The PX4 Flight Stack package</description> + + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="lorenz@px4.io">Lorenz Meier</maintainer> + + + <!-- One license tag required, multiple allowed, one license per tag --> + <!-- Commonly used license strings: --> + <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> + <license>BSD</license> + + + <!-- Url tags are optional, but mutiple are allowed, one per tag --> + <!-- Optional attribute type can be: website, bugtracker, or repository --> + <!-- Example: --> + <url type="website">http://px4.io/ros</url> + + + <!-- Author tags are optional, mutiple are allowed, one per tag --> + <!-- Authors do not have to be maintianers, but could be --> + <!-- Example: --> + <!-- <author email="jane.doe@example.com">Jane Doe</author> --> + + + <!-- The *_depend tags are used to specify dependencies --> + <!-- Dependencies can be catkin packages or system dependencies --> + <!-- Examples: --> + <!-- Use build_depend for packages you need at compile time: --> + <build_depend>message_generation</build_depend> + <!-- Use buildtool_depend for build tool packages: --> + <!-- <buildtool_depend>catkin</buildtool_depend> --> + <!-- Use run_depend for packages you need at runtime: --> + <run_depend>message_runtime</run_depend> + <!-- Use test_depend for packages you need only for testing: --> + <!-- <test_depend>gtest</test_depend> --> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>roscpp</build_depend> + <build_depend>rospy</build_depend> + <build_depend>std_msgs</build_depend> + <run_depend>roscpp</run_depend> + <run_depend>rospy</run_depend> + <run_depend>std_msgs</run_depend> + + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- You can specify that this package is a metapackage here: --> + <!-- <metapackage/> --> + + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index fdaa7f27b..eb9453b50 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -65,7 +65,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, _pulsesPerRev(pulsesPerRev), _uart(0), _controlPoll(), - _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _actuators(ORB_ID(actuator_controls_0), 20), _motor1Position(0), _motor1Speed(0), _motor1Overflow(0), diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 0b8c01f79..5a23932d3 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -67,6 +67,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <poll.h> +#include <platforms/px4_defines.h> #include "flow_position_estimator_params.h" @@ -337,7 +338,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) { float sum = 0.0f; for(uint8_t j = 0; j < 3; j++) - sum = sum + speed[j] * att.R[i][j]; + sum = sum + speed[j] * PX4_R(att.R, i, j); global_speed[i] = sum; } diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk new file mode 100644 index 000000000..0fc4316ec --- /dev/null +++ b/src/examples/publisher/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = publisher + +SRCS = publisher.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp new file mode 100644 index 000000000..8ef147493 --- /dev/null +++ b/src/examples/publisher/publisher.cpp @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * 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. + * * Neither the names of Stanford University or Willow Garage, Inc. 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 <px4.h> + +using namespace px4; + +/** + * This tutorial demonstrates simple sending of messages over the PX4 middleware system. + */ + +namespace px4 +{ +bool task_should_exit = false; +} + +PX4_MAIN_FUNCTION(publisher) +{ + + px4::init(argc, argv, "px4_publisher"); + + px4::NodeHandle n; + + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels); + + + px4::Rate loop_rate(10); + + /** + * A count of how many messages we have sent. This is used to create + * a unique string for each message. + */ + int count = 0; + + while (px4::ok()) { + /** + * This is a message object. You stuff it with data, and then publish it. + */ + PX4_TOPIC_T(rc_channels) msg; + + msg.timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg.timestamp_last_valid); + + /** + * The publish() function is how you send messages. The parameter + * is the message object. The type of this object must agree with the type + * given as a template parameter to the advertise<>() call, as was done + * in the constructor above. + */ + rc_channels_pub->publish(msg); + + n.spinOnce(); + + loop_rate.sleep(); + ++count; + } + + return 0; +} diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk new file mode 100644 index 000000000..90b4d8ffc --- /dev/null +++ b/src/examples/subscriber/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Subscriber Example Application +# + +MODULE_COMMAND = subscriber + +SRCS = subscriber.cpp \ + subscriber_params.c + +MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp new file mode 100644 index 000000000..68003b6df --- /dev/null +++ b/src/examples/subscriber/subscriber.cpp @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * 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. + * * Neither the names of Stanford University or Willow Garage, Inc. 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 <px4.h> +#include "subscriber_params.h" + +using namespace px4; + +/** + * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. + */ +void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); +} +void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid); +} + +class RCHandler { +public: + void callback(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid); + } +}; + +RCHandler rchandler; + + +namespace px4 +{ +bool task_should_exit = false; +} + +PX4_MAIN_FUNCTION(subscriber) { + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of px4::init() before using any other + * part of the PX4/ ROS system. + */ + px4::init(argc, argv, "listener"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + px4::NodeHandle n; + + /* Define parameters */ + px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); + px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF); + + /* Read the parameter back for testing */ + int32_t sub_interval; + float test_float; + PX4_PARAM_GET(p_sub_interv, &sub_interval); + PX4_INFO("Param SUB_INTERV = %d", sub_interval); + PX4_PARAM_GET(p_test_float, &test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float); + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called chatterCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); + PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); + PX4_INFO("subscribed"); + + /** + * px4::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). px4::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + n.spin(); + PX4_INFO("finished, returning"); + + return 0; +} diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c new file mode 100644 index 000000000..a43817b5b --- /dev/null +++ b/src/examples/subscriber/subscriber_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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. + * + ****************************************************************************/ + +/** + * @file subscriber_params.c + * Parameters for the subscriber example + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <px4_defines.h> +#include "subscriber_params.h" + +/** + * Interval of one subscriber in the example in ms + * + * @group Subscriber Example + */ +PX4_PARAM_DEFINE_INT32(SUB_INTERV); + +/** + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example + */ +PX4_PARAM_DEFINE_FLOAT(SUB_TESTF); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h new file mode 100644 index 000000000..f6f1d6ba0 --- /dev/null +++ b/src/examples/subscriber/subscriber_params.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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. + * + ****************************************************************************/ + +/** + * @file subscriber_params.h + * Default parameters for the subscriber example + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#pragma once + +#define PARAM_SUB_INTERV_DEFAULT 100 +#define PARAM_SUB_TESTF_DEFAULT 3.14f diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 13cbda938..5c0ba59fd 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -38,6 +38,7 @@ */ #pragma once +#include <cstddef> template<class T> class __EXPORT ListNode diff --git a/src/include/px4.h b/src/include/px4.h new file mode 100644 index 000000000..ca702d63c --- /dev/null +++ b/src/include/px4.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4.h + * + * Main system header with common convenience functions + */ + +#pragma once + +#include "../platforms/px4_includes.h" +#include "../platforms/px4_defines.h" +#include "../platforms/px4_middleware.h" +#include "../platforms/px4_nodehandle.h" +#include "../platforms/px4_subscriber.h" diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c1f032b49..9784ea1a1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -568,7 +568,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds R = R_decl * R_body; /* copy rotation matrix */ - memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); + memcpy(&att.R[0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 6768bfa7e..f3cd87728 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -101,7 +101,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - uORB::SubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionNode *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -119,7 +119,7 @@ void Block::updateSubscriptions() void Block::updatePublications() { - uORB::PublicationBase *pub = getPublications().getHead(); + uORB::PublicationNode *pub = getPublications().getHead(); int count = 0; while (pub != NULL) { diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 9bd80b15b..d2f9cdf07 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -46,8 +46,8 @@ // forward declaration namespace uORB { - class SubscriptionBase; - class PublicationBase; + class SubscriptionNode; + class PublicationNode; } namespace control @@ -83,15 +83,15 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; } - List<uORB::PublicationBase *> & getPublications() { return _publications; } + List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; } + List<uORB::PublicationNode *> & getPublications() { return _publications; } List<BlockParamBase *> & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List<uORB::SubscriptionBase *> _subscriptions; - List<uORB::PublicationBase *> _publications; + List<uORB::SubscriptionNode *> _subscriptions; + List<uORB::PublicationNode *> _publications; List<BlockParamBase *> _params; private: diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e8fecef0d..454d0db19 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions - _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), - _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), - _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), - _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), - _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), - _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + _att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()), + _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()), + _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()), + _pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()), + _missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()), + _manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()), + _status(ORB_ID(vehicle_status), 20, &getSubscriptions()), + _param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz // publications - _actuators(&getPublications(), ORB_ID(actuator_controls_0)) + _actuators(ORB_ID(actuator_controls_0), &getPublications()) { } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e7805daa9..c0b1bb404 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -83,6 +83,7 @@ #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> +#include <platforms/px4_defines.h> #include "estimator_23states.h" @@ -1384,7 +1385,7 @@ FixedwingEstimator::task_main() math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); + PX4_R(_att.R, i, j) = R(i, j); _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e770c11a2..57c1e72f3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -75,6 +75,7 @@ #include <ecl/attitude_fw/ecl_pitch_controller.h> #include <ecl/attitude_fw/ecl_roll_controller.h> #include <ecl/attitude_fw/ecl_yaw_controller.h> +#include <platforms/px4_defines.h> /** * Fixedwing attitude control app start / stop handling function @@ -824,9 +825,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; - speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; - speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; + speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; + speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; + speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6017369aa..e07bcc225 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -90,6 +90,7 @@ #include <external_lgpl/tecs/tecs.h> #include "landingslope.h" #include "mtecs/mTecs.h" +#include <platforms/px4_defines.h> static int _control_task = -1; /**< task handle for sensor task */ @@ -704,7 +705,7 @@ FixedwingPositionControl::vehicle_attitude_poll() /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _R_nb(i, j) = _att.R[i][j]; + _R_nb(i, j) = PX4_R(_att.R, i, j); } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index bffeefc1f..ecdab2936 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -52,7 +52,7 @@ mTecs::mTecs() : _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ - _status(&getPublications(), ORB_ID(tecs_status)), + _status(ORB_ID(tecs_status), &getPublications()), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5918d6bc5..cea847603 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -73,6 +73,7 @@ #include <mathlib/mathlib.h> #include <lib/geo/geo.h> #include <mavlink/mavlink_log.h> +#include <platforms/px4_defines.h> #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -1147,11 +1148,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; + if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att.R, 2, 2); - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + } else if (PX4_R(_att.R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; saturation_z = true; } else { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 296919c04..de6357d31 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -68,6 +68,7 @@ #include <geo/geo.h> #include <systemlib/systemlib.h> #include <drivers/drv_hrt.h> +#include <platforms/px4_defines.h> #include "position_estimator_inav_params.h" #include "inertial_filter.h" @@ -282,13 +283,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; - + float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; - + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -461,7 +462,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) acc[i] = 0.0f; for (int j = 0; j < 3; j++) { - acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } @@ -494,7 +495,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && - (att.R[2][2] > 0.7f) && + (PX4_R(att.R, 2, 2) > 0.7f) && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { sonar_time = t; @@ -531,15 +532,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ - float flow_dist = dist_bottom / att.R[2][2]; + float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { - body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; + body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ @@ -562,7 +563,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { - flow_v[i] += att.R[i][j] * flow_m[j]; + flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } @@ -571,7 +572,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy @@ -646,13 +647,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; - /* only reset the z estimate if the z weight parameter is not zero */ + /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } - + vision_valid = true; warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); @@ -912,7 +913,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += att.R[j][i] * accel_bias_corr[j]; + c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { @@ -937,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += att.R[j][i] * accel_bias_corr[j]; + c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (isfinite(c)) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd..672dc52c3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -182,13 +182,13 @@ private: /** * Get and limit value for specified RC function. Returns NAN if not mapped. */ - float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + float get_rc_value(uint8_t func, float min_value, float max_value); /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); - switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); + switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -771,25 +771,25 @@ Sensors::parameters_update() _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ - _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[ROLL] = _parameters.rc_map_roll - 1; - _rc.function[PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[YAW] = _parameters.rc_map_yaw - 1; - - _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - - _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; + _rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -1492,7 +1492,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } float -Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +Sensors::get_rc_value(uint8_t func, float min_value, float max_value) { if (_rc.function[func] >= 0) { float value = _rc.channels[_rc.function[func]]; @@ -1513,7 +1513,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } switch_pos_t -Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) +Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1534,7 +1534,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo } switch_pos_t -Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) +Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; @@ -1668,24 +1668,24 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value(ROLL, -1.0, 1.0); - manual.x = get_rc_value(PITCH, -1.0, 1.0); - manual.r = get_rc_value(YAW, -1.0, 1.0); - manual.z = get_rc_value(THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 71757e1f4..41a866968 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -49,15 +49,16 @@ #include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" +#include "topics/rc_channels.h" namespace uORB { template<class T> Publication<T>::Publication( - List<PublicationBase *> * list, - const struct orb_metadata *meta) : + const struct orb_metadata *meta, + List<PublicationNode *> * list) : T(), // initialize data structure to zero - PublicationBase(list, meta) { + PublicationNode(meta, list) { } template<class T> @@ -80,5 +81,6 @@ template class __EXPORT Publication<actuator_outputs_s>; template class __EXPORT Publication<actuator_direct_s>; template class __EXPORT Publication<encoders_s>; template class __EXPORT Publication<tecs_status_s>; +template class __EXPORT Publication<rc_channels_s>; } diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df8..fd1ee4dec 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -38,6 +38,8 @@ #pragma once +#include <assert.h> + #include <uORB/uORB.h> #include <containers/List.hpp> @@ -49,55 +51,112 @@ namespace uORB * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *> +class __EXPORT PublicationBase { public: - PublicationBase( - List<PublicationBase *> * list, - const struct orb_metadata *meta) : + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + */ + PublicationBase(const struct orb_metadata *meta) : _meta(meta), _handle(-1) { - if (list != NULL) list->add(this); } - void update() { + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (_handle > 0) { - orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + orb_publish(getMeta(), getHandle(), data); } else { - setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + setHandle(orb_advertise(getMeta(), data)); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~PublicationBase() { orb_unsubscribe(getHandle()); } +// accessors const struct orb_metadata *getMeta() { return _meta; } int getHandle() { return _handle; } protected: +// accessors void setHandle(orb_advert_t handle) { _handle = handle; } +// attributes const struct orb_metadata *_meta; orb_advert_t _handle; }; /** + * alias class name so it is clear that the base class + * can be used by itself if desired + */ +typedef PublicationBase PublicationTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT PublicationNode : + public PublicationBase, + public ListNode<PublicationNode *> +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + PublicationNode(const struct orb_metadata *meta, + List<PublicationNode *> * list=nullptr) : + PublicationBase(meta) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +}; + +/** * Publication wrapper class */ template<class T> class Publication : public T, // this must be first! - public PublicationBase + public PublicationNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param list A list interface for adding to + * list during construction */ - Publication(List<PublicationBase *> * list, - const struct orb_metadata *meta); + Publication(const struct orb_metadata *meta, + List<PublicationNode *> * list=nullptr); + + /** + * Deconstructor + **/ virtual ~Publication(); + /* * XXX * This function gets the T struct, assuming @@ -106,6 +165,13 @@ public: * seem to be available */ void *getDataVoidPtr(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + PublicationBase::update(getDataVoidPtr()); + } }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7..61609d009 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -52,25 +52,18 @@ #include "topics/vehicle_local_position.h" #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" +#include "topics/rc_channels.h" namespace uORB { -bool __EXPORT SubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} - template<class T> Subscription<T>::Subscription( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta, unsigned interval) : + const struct orb_metadata *meta, + unsigned interval, + List<SubscriptionNode *> * list) : T(), // initialize data structure to zero - SubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); + SubscriptionNode(meta, interval, list) { } template<class T> @@ -101,5 +94,6 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>; template class __EXPORT Subscription<vehicle_local_position_s>; template class __EXPORT Subscription<vehicle_attitude_setpoint_s>; template class __EXPORT Subscription<vehicle_rates_setpoint_s>; +template class __EXPORT Subscription<rc_channels_s>; } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 34e9a83e0..f82586285 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -38,10 +38,11 @@ #pragma once +#include <assert.h> + #include <uORB/uORB.h> #include <containers/List.hpp> - namespace uORB { @@ -49,8 +50,7 @@ namespace uORB * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT SubscriptionBase : - public ListNode<SubscriptionBase *> +class __EXPORT SubscriptionBase { public: // methods @@ -58,23 +58,42 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * + * @param interval The minimum interval in milliseconds + * between updates */ - SubscriptionBase( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta) : + SubscriptionBase(const struct orb_metadata *meta, + unsigned interval=0) : _meta(meta), _handle() { - if (list != NULL) list->add(this); + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); } - bool updated(); - void update() { + + /** + * Check if there is a new update. + * */ + bool updated() { + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; + } + + /** + * Update the struct + * @param data The uORB message struct we are updating. + */ + void update(void * data) { if (updated()) { - orb_copy(_meta, _handle, getDataVoidPtr()); + orb_copy(_meta, _handle, data); } } - virtual void *getDataVoidPtr() = 0; + + /** + * Deconstructor + */ virtual ~SubscriptionBase() { orb_unsubscribe(_handle); } @@ -90,30 +109,86 @@ protected: }; /** + * alias class name so it is clear that the base class + */ +typedef SubscriptionBase SubscriptionTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT SubscriptionNode : + + public SubscriptionBase, + public ListNode<SubscriptionNode *> +{ +public: + /** + * Constructor + * + * + * @param meta The uORB metadata (usually from the ORB_ID() + * macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A pointer to a list of subscriptions + * that this should be appended to. + */ + SubscriptionNode(const struct orb_metadata *meta, + unsigned interval=0, + List<SubscriptionNode *> * list=nullptr) : + SubscriptionBase(meta, interval), + _interval(interval) { + if (list != nullptr) list->add(this); + } + + /** + * This function is the callback for list traversal + * updates, a child class must implement it. + */ + virtual void update() = 0; +// accessors + unsigned getInterval() { return _interval; } +protected: +// attributes + unsigned _interval; + +}; + +/** * Subscription wrapper class */ template<class T> class __EXPORT Subscription : public T, // this must be first! - public SubscriptionBase + public SubscriptionNode { public: /** * Constructor * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param interval The minimum interval in milliseconds between updates + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param interval The minimum interval in milliseconds + * between updates + * @param list A list interface for adding to + * list during construction */ - Subscription( - List<SubscriptionBase *> * list, - const struct orb_metadata *meta, unsigned interval); + Subscription(const struct orb_metadata *meta, + unsigned interval=0, + List<SubscriptionNode *> * list=nullptr); /** * Deconstructor */ virtual ~Subscription(); + + /** + * Create an update function that uses the embedded struct. + */ + void update() { + SubscriptionBase::update(getDataVoidPtr()); + } + /* * XXX * This function gets the T struct, assuming diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h deleted file mode 100755 index 1df1433ac..000000000 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * - * 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. - * - ****************************************************************************/ - -/** - * @file vehicle_attitude.h - * Definition of the attitude uORB topic. - */ - -#ifndef VEHICLE_ATTITUDE_H_ -#define VEHICLE_ATTITUDE_H_ - -#include <stdint.h> -#include <stdbool.h> -#include <platforms/px4_defines.h> - -/** - * @addtogroup topics - * @{ - */ - -/** - * Attitude in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ -struct vehicle_attitude_s { - - uint64_t timestamp; /**< in microseconds since system start */ - - /* This is similar to the mavlink message ATTITUDE, but for onboard use */ - - /** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */ - - float roll; /**< Roll angle (rad, Tait-Bryan, NED) */ - float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) */ - float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) */ - float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ - float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ - float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ - float rollacc; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ - float pitchacc; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ - float yawacc; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ - float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ - float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q[4]; /**< Quaternion (NED) */ - float g_comp[3]; /**< Compensated gravity vector */ - bool R_valid; /**< Rotation matrix valid */ - bool q_valid; /**< Quaternion valid */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude); - -#endif diff --git a/platforms/empty.c b/src/platforms/empty.c index 139531354..139531354 100644 --- a/platforms/empty.c +++ b/src/platforms/empty.c diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk new file mode 100644 index 000000000..4a2aff824 --- /dev/null +++ b/src/platforms/nuttx/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# NuttX / uORB adapter library +# + +SRCS = px4_nuttx_impl.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp new file mode 100644 index 000000000..ec557e8aa --- /dev/null +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_nodehandle.cpp + * + * PX4 Middleware Wrapper Nodehandle + */ +#include <px4.h> + +namespace px4 +{ + +} diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp new file mode 100644 index 000000000..70e292320 --- /dev/null +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_nuttx_impl.cpp + * + * PX4 Middleware Wrapper NuttX Implementation + */ + +#include <px4.h> +#include <drivers/drv_hrt.h> + + +namespace px4 +{ + +void init(int argc, char *argv[], const char *process_name) +{ + PX4_WARN("process: %s", process_name); +} + +uint64_t get_time_micros() +{ + return hrt_absolute_time(); +} + +} diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp new file mode 100644 index 000000000..3bd70272f --- /dev/null +++ b/src/platforms/nuttx/px4_publisher.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_publisher.cpp + * + * PX4 Middleware Wrapper for Publisher + */ +#include <platforms/px4_publisher.h> + + diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp new file mode 100644 index 000000000..426e646c9 --- /dev/null +++ b/src/platforms/nuttx/px4_subscriber.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_subscriber.cpp + * + * PX4 Middleware Wrapper Subscriber + */ +#include <platforms/px4_subscriber.h> + diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 535cf67fe..af7188f32 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -36,19 +36,105 @@ * * Generally used magic defines */ + #pragma once +/* Get the name of the default value fiven the param name */ +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT + +/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */ +#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) +#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* * Building for running within the ROS environment */ #define __EXPORT -//#define PX4_MAIN_FUNCTION(_prefix) -#define ORB_DECLARE(x) +#include "ros/ros.h" +/* Main entry point */ +#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) + +/* Print/output wrappers */ +#define PX4_WARN ROS_WARN +#define PX4_INFO ROS_INFO + +/* Topic Handle */ +#define PX4_TOPIC(_name) #_name + +/* Topic type */ +#define PX4_TOPIC_T(_name) _name + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); + +/* Parameter handle datatype */ +typedef const char *px4_param_t; + +/* Helper fucntions to set ROS params, only int and float supported */ +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) +{ + ros::param::set(name, value); + return (px4_param_t)name; +}; +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) +{ + ros::param::set(name, value); + return (px4_param_t)name; +}; + +/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */ +#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) + +/* Get value of parameter */ +#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) #else -#include <nuttx/config.h> -//#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } -#include <modules/uORB/uORB.h> +/* + * Building for NuttX + */ +#include <platforms/px4_includes.h> +/* Main entry point */ +#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) + +/* Print/output wrappers */ +#define PX4_WARN warnx +#define PX4_INFO warnx + +/* Topic Handle */ +#define PX4_TOPIC(_name) ORB_ID(_name) + +/* Topic type */ +#define PX4_TOPIC_T(_name) _name##_s + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) + +/* Parameter handle datatype */ +typedef param_t px4_param_t; +/* Initialize a param, get param handle */ +#define PX4_PARAM_INIT(_name) param_find(#_name) + +/* Get value of parameter */ +#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif + +/* Shortcut for subscribing to topics + * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback + */ +#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME +#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) + +/* shortcut for advertising topics */ +#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name)) + +/* wrapper for 2d matrices */ +#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) + +/* wrapper for rotation matrices stored in arrays */ +#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h new file mode 100644 index 000000000..a3b59b766 --- /dev/null +++ b/src/platforms/px4_includes.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_includes.h + * + * Includes headers depending on the build target + */ + +#pragma once + +#include <stdbool.h> + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#include "ros/ros.h" +#include "px4/rc_channels.h" + +#else +/* + * Building for NuttX + */ +#include <nuttx/config.h> +#include <uORB/uORB.h> +#include <uORB/topics/rc_channels.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> + +#endif diff --git a/src/modules/uORB/topics/rc_channels.h b/src/platforms/px4_middleware.h index 16916cc4d..c1465b4b1 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/platforms/px4_middleware.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 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 @@ -32,65 +32,54 @@ ****************************************************************************/ /** - * @file rc_channels.h - * Definition of the rc_channels uORB topic. + * @file px4_middleware.h * - * @deprecated DO NOT USE FOR NEW CODE + * PX4 generic middleware wrapper */ -#ifndef RC_CHANNELS_H_ -#define RC_CHANNELS_H_ +#pragma once #include <stdint.h> -#include "../uORB.h" +#include <unistd.h> -/** - * This defines the mapping of the RC functions. - * The value assigned to the specific function corresponds to the entry of - * the channel array channels[]. - */ -enum RC_CHANNELS_FUNCTION { - THROTTLE = 0, - ROLL, - PITCH, - YAW, - MODE, - RETURN, - POSCTL, - LOITER, - OFFBOARD, - ACRO, - FLAPS, - AUX_1, - AUX_2, - AUX_3, - AUX_4, - AUX_5 -}; +namespace px4 +{ -// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS +__EXPORT void init(int argc, char *argv[], const char *process_name); -#define RC_CHANNELS_FUNCTION_MAX 18 +__EXPORT uint64_t get_time_micros(); +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /** - * @addtogroup topics - * @{ + * Returns true if the app/task should continue to run */ -struct rc_channels_s { - uint64_t timestamp; /**< Timestamp in microseconds since boot time */ - uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ - float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ - uint8_t channel_count; /**< Number of valid channels */ - int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ - uint8_t rssi; /**< Receive signal strength index */ - bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ -}; /**< radio control channels. */ - +bool ok() { return ros::ok(); } +#else +extern bool task_should_exit; /** - * @} + * Returns true if the app/task should continue to run */ +bool ok() { return !task_should_exit; } +#endif -/* register this as object request broker structure */ -ORB_DECLARE(rc_channels); +class Rate +{ -#endif +public: + /** + * Construct the Rate object and set rate + * @param rate_hz rate from which sleep time is calculated in Hz + */ + explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; } + + /** + * Sleep for 1/rate_hz s + */ + void sleep() { usleep(sleep_interval); } + +private: + uint64_t sleep_interval; + +}; + +} // namespace px4 diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h new file mode 100644 index 000000000..5b7247b20 --- /dev/null +++ b/src/platforms/px4_nodehandle.h @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_nodehandle.h + * + * PX4 Middleware Wrapper Node Handle + */ +#pragma once + +/* includes for all platforms */ +#include "px4_subscriber.h" +#include "px4_publisher.h" +#include "px4_middleware.h" + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ +#include "ros/ros.h" +#include <list> +#include <inttypes.h> +#else +/* includes when building for NuttX */ +#include <poll.h> +#endif + +namespace px4 +{ +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +class NodeHandle : + private ros::NodeHandle +{ +public: + NodeHandle() : + ros::NodeHandle(), + _subs(), + _pubs() + {} + + ~NodeHandle() + { + //XXX empty lists + }; + + /** + * Subscribe with callback to function + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ + template<typename M> + Subscriber *subscribe(const char *topic, void(*fp)(M)) + { + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); + Subscriber *sub = new Subscriber(ros_sub); + _subs.push_back(sub); + return sub; + } + + /** + * Subscribe with callback to class method + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ + template<typename M, typename T> + Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj) + { + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); + Subscriber *sub = new Subscriber(ros_sub); + _subs.push_back(sub); + return sub; + } + + /** + * Advertise topic + * @param topic Name of the topic + */ + template<typename M> + Publisher *advertise(const char *topic) + { + ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault); + Publisher *pub = new Publisher(ros_pub); + _pubs.push_back(pub); + return pub; + } + + /** + * Calls all callback waiting to be called + */ + void spinOnce() { ros::spinOnce(); } + + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ + void spin() { ros::spin(); } + + +private: + static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ + std::list<Subscriber *> _subs; /**< Subcriptions of node */ + std::list<Publisher *> _pubs; /**< Publications of node */ +}; +#else +class __EXPORT NodeHandle +{ +public: + NodeHandle() : + _subs(), + _pubs(), + _sub_min_interval(nullptr) + {} + + ~NodeHandle() {}; + + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + + template<typename M> + Subscriber *subscribe(const struct orb_metadata *meta, + std::function<void(const M &)> callback, + unsigned interval) + { + SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs); + + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + _sub_min_interval = sub_px4; + } + + return (Subscriber *)sub_px4; + } + + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ + template<typename M> + Publisher *advertise(const struct orb_metadata *meta) + { + //XXX + Publisher *pub = new Publisher(meta, &_pubs); + return pub; + } + + /** + * Calls all callback waiting to be called + */ + void spinOnce() + { + /* Loop through subscriptions, call callback for updated subscriptions */ + uORB::SubscriptionNode *sub = _subs.getHead(); + int count = 0; + + while (sub != nullptr) { + if (count++ > kMaxSubscriptions) { + PX4_WARN("exceeded max subscriptions"); + break; + } + + sub->update(); + sub = sub->getSibling(); + } + } + + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ + void spin() + { + while (ok()) { + const int timeout_ms = 100; + + /* Only continue in the loop if the nodehandle has subscriptions */ + if (_sub_min_interval == nullptr) { + usleep(timeout_ms * 1000); + continue; + } + + /* Poll fd with smallest interval */ + struct pollfd pfd; + pfd.fd = _sub_min_interval->getHandle(); + pfd.events = POLLIN; + + if (poll(&pfd, 1, timeout_ms) <= 0) { + /* timed out */ + continue; + } + + spinOnce(); + } + } +private: + static const uint16_t kMaxSubscriptions = 100; + List<uORB::SubscriptionNode *> _subs; /**< Subcriptions of node */ + List<uORB::PublicationNode *> _pubs; /**< Publications of node */ + uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + of all Subscriptions in _subs*/ +}; +#endif +} diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h new file mode 100644 index 000000000..cb15eeb58 --- /dev/null +++ b/src/platforms/px4_publisher.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_nodehandle.h + * + * PX4 Middleware Wrapper Node Handle + */ +#pragma once +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ +#include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Publication.hpp> +#include <containers/List.hpp> +#endif + +namespace px4 +{ +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +class Publisher +{ +public: + /** + * Construct Publisher by providing a ros::Publisher + * @param ros_pub the ros publisher which will be used to perform the publications + */ + Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) + {} + + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ + template<typename M> + int publish(const M &msg) { _ros_pub.publish(msg); return 0; } +private: + ros::Publisher _ros_pub; /**< Handle to the ros publisher */ +}; +#else +class Publisher : + public uORB::PublicationNode +{ +public: + /** + * Construct Publisher by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param list publisher is added to this list + */ + Publisher(const struct orb_metadata *meta, + List<uORB::PublicationNode *> *list) : + uORB::PublicationNode(meta, list) + {} + + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ + template<typename M> + int publish(const M &msg) + { + uORB::PublicationBase::update((void *)&msg); + return 0; + } + + /** + * Empty callback for list traversal + */ + void update() {} ; +}; +#endif +} diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h new file mode 100644 index 000000000..e995c6e49 --- /dev/null +++ b/src/platforms/px4_subscriber.h @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_subscriber.h + * + * PX4 Middleware Wrapper Subscriber + */ +#pragma once +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ +#include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Subscription.hpp> +#include <containers/List.hpp> +#include <functional> +#endif + +namespace px4 +{ + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +class Subscriber +{ +public: + /** + * Construct Subscriber by providing a ros::Subscriber + * @param ros_sub the ros subscriber which will be used to perform the publications + */ + Subscriber(ros::Subscriber ros_sub) : + _ros_sub(ros_sub) + {} + + ~Subscriber() {}; +private: + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ +}; + +#else + +/** + * Subscriber class which is used by nodehandle when building for NuttX + */ +class Subscriber +{ +public: + Subscriber() {}; + ~Subscriber() {}; +private: +}; + +/** + * Subscriber class that is templated with the uorb subscription message type + */ +template<typename M> +class SubscriberPX4 : + public Subscriber, + public uORB::Subscription<M> +{ +public: + /** + * Construct SubscriberPX4 by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + * @param list subscriber is added to this list + */ + SubscriberPX4(const struct orb_metadata *meta, + unsigned interval, + std::function<void(const M &)> callback, + List<uORB::SubscriptionNode *> *list) : + Subscriber(), + uORB::Subscription<M>(meta, interval, list), + _callback(callback) + //XXX store callback + {} + + ~SubscriberPX4() {}; + + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + * If new data is available the callback is called + */ + void update() + { + if (!uORB::Subscription<M>::updated()) { + /* Topic not updated, do not call callback */ + return; + } + + /* get latest data */ + uORB::Subscription<M>::update(); + + /* Call callback which performs actions based on this data */ + _callback(uORB::Subscription<M>::getData()); + + }; +private: + std::function<void(const M &)> _callback; /**< Callback handle, + called when new data is available */ + +}; +#endif + +} diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp new file mode 100644 index 000000000..6ac3c76d3 --- /dev/null +++ b/src/platforms/ros/px4_nodehandle.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_nodehandle.cpp + * + * PX4 Middleware Wrapper Nodehandle + */ +#include <px4_nodehandle.h> + +namespace px4 +{ +} + diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp new file mode 100644 index 000000000..f02dbe4c9 --- /dev/null +++ b/src/platforms/ros/px4_publisher.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_publisher.cpp + * + * PX4 Middleware Wrapper for Publisher + */ +#include <px4_publisher.h> + + diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp new file mode 100644 index 000000000..854986a7f --- /dev/null +++ b/src/platforms/ros/px4_ros_impl.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_ros_impl.cpp + * + * PX4 Middleware Wrapper ROS Implementation + */ + +#include <px4.h> + +namespace px4 +{ + +void init(int argc, char *argv[], const char *process_name) +{ + ros::init(argc, argv, process_name); +} + +uint64_t get_time_micros() +{ + ros::Time time = ros::Time::now(); + return time.sec * 1e6 + time.nsec / 1000; +} + +} diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp new file mode 100644 index 000000000..d040b860d --- /dev/null +++ b/src/platforms/ros/px4_subscriber.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file px4_subscriber.cpp + * + * PX4 Middleware Wrapper Subscriber + */ + +#include <px4_subscriber.h> + |