diff options
author | Trent Lukaczyk <aerialhedgehog@gmail.com> | 2015-02-05 20:19:04 -0800 |
---|---|---|
committer | Trent Lukaczyk <aerialhedgehog@gmail.com> | 2015-02-05 20:19:04 -0800 |
commit | 531eaa231486c6af46394f6842d420447cb0ee0e (patch) | |
tree | e7a90d8c50d700a2b7aff05c5a545162810d4967 | |
parent | 6798aee13a5bb885966960cdba6ab57b14278ab0 (diff) | |
parent | 7e6198b3dd517e1158431c8344c5912a6c28b363 (diff) | |
download | px4-firmware-531eaa231486c6af46394f6842d420447cb0ee0e.tar.gz px4-firmware-531eaa231486c6af46394f6842d420447cb0ee0e.tar.bz2 px4-firmware-531eaa231486c6af46394f6842d420447cb0ee0e.zip |
Merge remote-tracking branch 'upstream/master'
215 files changed, 10948 insertions, 2450 deletions
diff --git a/.gitignore b/.gitignore index 8ea2698e7..611325444 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,7 @@ .~lock.* Archives/* Build/* +build/* core cscope.out Firmware.sublime-workspace @@ -38,6 +39,10 @@ tags .pydevproject .ropeproject *.orig +src/modules/uORB/topics/* +src/platforms/nuttx/px4_messages/* +src/platforms/ros/px4_messages/* Firmware.zip unittests/build *.generated.h +.vagrant diff --git a/.gitmodules b/.gitmodules index 0c8c91da4..c869803b7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,6 +7,12 @@ [submodule "uavcan"] path = src/lib/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 [submodule "unittests/gtest"] path = unittests/gtest url = https://github.com/sjwilks/gtest.git diff --git a/.travis.yml b/.travis.yml index 8677bf91b..a994d3471 100644 --- a/.travis.yml +++ b/.travis.yml @@ -12,7 +12,7 @@ before_script: - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - - sudo apt-get install python-serial python-argparse + - sudo apt-get install python-serial python-argparse python-empy - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . - cd ~ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..0c81607b0 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,289 @@ +cmake_minimum_required(VERSION 2.8.3) +project(px4) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +add_definitions(-D__PX4_ROS) +add_definitions(-D__EXPORT=) + +## 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 + cmake_modules + gazebo_msgs + sensor_msgs + mav_msgs +) +find_package(Eigen REQUIRED) + +## 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 + rc_channels.msg + vehicle_attitude.msg + vehicle_attitude_setpoint.msg + manual_control_setpoint.msg + actuator_controls.msg + actuator_controls_0.msg + actuator_controls_virtual_mc.msg + vehicle_rates_setpoint.msg + mc_virtual_rates_setpoint.msg + vehicle_attitude.msg + vehicle_control_mode.msg + actuator_armed.msg + parameter_update.msg + vehicle_status.msg + vehicle_local_position.msg + position_setpoint.msg + position_setpoint_triplet.msg + vehicle_local_position_setpoint.msg + vehicle_global_velocity_setpoint.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 + gazebo_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 message_runtime roscpp rospy std_msgs + DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} + src/platforms + src/platforms/ros/px4_messages + src/include + src/modules + src/ + src/lib + ${EIGEN_INCLUDE_DIRS} +) + +## generate multiplatform wrapper headers +## note that the message header files are generated as in any ROS project with generate_messages() +set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages) +set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros) +set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary) +set(MULTIPLATFORM_PREFIX px4_) +add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py + -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR} + -t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX}) + +## Declare a cpp library +add_library(px4 + src/platforms/ros/px4_ros_impl.cpp + src/platforms/ros/perf_counter.cpp + src/platforms/ros/geo.cpp + src/lib/mathlib/math/Limits.cpp + src/modules/systemlib/circuit_breaker.cpp +) +add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) + +target_link_libraries(px4 + ${catkin_LIBRARIES} +) + +## Declare a test publisher +add_executable(publisher + src/examples/publisher/publisher_main.cpp + src/examples/publisher/publisher_example.cpp) +add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) +target_link_libraries(publisher + ${catkin_LIBRARIES} + px4 +) + +## Declare a test subscriber +add_executable(subscriber + src/examples/subscriber/subscriber_main.cpp + src/examples/subscriber/subscriber_example.cpp) +add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) +target_link_libraries(subscriber + ${catkin_LIBRARIES} + px4 +) + +## MC Attitude Control +add_executable(mc_att_control + src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp + src/modules/mc_att_control_multiplatform/mc_att_control.cpp + src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) +add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_att_control + ${catkin_LIBRARIES} + px4 +) + +## MC Position Control +add_executable(mc_pos_control + src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp + src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp) +add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_pos_control + ${catkin_LIBRARIES} + px4 +) + +## Attitude Estimator dummy +add_executable(attitude_estimator + src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) +add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(attitude_estimator + ${catkin_LIBRARIES} + px4 +) + +## Position Estimator dummy +add_executable(position_estimator + src/platforms/ros/nodes/position_estimator/position_estimator.cpp) +add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(position_estimator + ${catkin_LIBRARIES} + px4 +) + +## Manual input +add_executable(manual_input + src/platforms/ros/nodes/manual_input/manual_input.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(manual_input + ${catkin_LIBRARIES} + px4 +) + +## Multicopter Mixer dummy +add_executable(mc_mixer + src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) +add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_mixer + ${catkin_LIBRARIES} + px4 +) + +## Commander +add_executable(commander + src/platforms/ros/nodes/commander/commander.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(commander + ${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 ${PROJECT_NAME} + 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) @@ -1,5 +1,5 @@ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -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: $(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 @@ -124,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 .PHONY: $(FIRMWARES) $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ -$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checksubmodules generateuorbtopicheaders @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% @@ -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. # @@ -161,7 +161,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) # NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives -archives: $(NUTTX_ARCHIVES) +archives: checksubmodules $(NUTTX_ARCHIVES) # We cannot build these parallel; note that we also force -j1 for the # sub-make invocations. @@ -211,8 +211,7 @@ menuconfig: @$(ECHO) "" endif -$(NUTTX_SRC): - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) +$(NUTTX_SRC): checksubmodules $(UAVCAN_DIR): $(Q) (./Tools/check_submodules.sh) @@ -226,6 +225,29 @@ updatesubmodules: $(Q) (git submodule init) $(Q) (git submodule update) +MSG_DIR = $(PX4_BASE)msg +UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb +MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb +TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics +MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages +MULTIPLATFORM_PREFIX = px4_ +TOPICHEADER_TEMP_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):$(PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) + @$(ECHO) "Generating multiplatform uORB topic wrapper headers" + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) +# clean up temporary files + $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) + # # Testing targets # @@ -237,12 +259,12 @@ testbuild: # Unittest targets. Builds and runs the host-level # unit tests. .PHONY: tests -tests: +tests: generateuorbtopicheaders $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) # # 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/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 033b3b640..2ecc104df 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -8,8 +8,19 @@ attitude_estimator_ekf start #ekf_att_pos_estimator start position_estimator_inav start -mc_att_control start -mc_pos_control start +if mc_att_control start +then +else + # try the multiplatform version + mc_att_control_m start +fi + +if mc_pos_control start +then +else + # try the multiplatform version + mc_pos_control_m start +fi # # Start Land Detector diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 4b9a9b68a..3d1c4a907 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,6 +2,8 @@ # # PX4FMU startup script for test hackery. # +set unit_test_failure 0 + uorb start if sercon @@ -41,6 +43,9 @@ fi if px4io start then echo "PX4IO OK" +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_start" fi if px4io checkcrc $io_file @@ -51,7 +56,6 @@ else tone_alarm MBABGP if px4io forceupdate 14662 $io_file then - usleep 500000 if px4io start then echo "PX4IO restart OK" @@ -59,12 +63,14 @@ else else echo "PX4IO restart failed" tone_alarm MNGGG - sleep 5 - reboot + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_flash" fi else echo "PX4IO update failed" tone_alarm MNGGG + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_flash" fi fi @@ -81,8 +87,6 @@ fi # Add new unit tests using the same pattern as below. # -set unit_test_failure 0 - if mavlink_tests then else diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 3904a2775..5e6e57164 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -14,15 +14,14 @@ if [ -d NuttX/nuttx ]; else echo "" echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" echo " NuttX sub repo not at correct version. Try 'git submodule update'" echo " or follow instructions on http://pixhawk.org/dev/git/submodules" echo "" echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!" - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" exit 1 fi else @@ -39,13 +38,12 @@ if [ -d mavlink/include/mavlink/v1.0 ]; else echo "" echo "" - echo "mavlink sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - echo "" - echo "" echo "New commits required:" echo "$(git submodule summary)" echo "" + echo "" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi else @@ -63,13 +61,56 @@ then else echo "" echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" echo "uavcan sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init; + git submodule update; +fi + +if [ -d Tools/gencpp ] +then + STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked gencpp submodule, correct version found" + else echo "" echo "" echo "New commits required:" echo "$(git submodule summary)" echo "" + echo "" + echo "gencpp sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init; + git submodule update; +fi + +if [ -d Tools/genmsg ] +then + STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked genmsg submodule, correct version found" + else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" + echo "genmsg sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi else 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..3e24e66b2 --- /dev/null +++ b/Tools/px_generate_uorb_topic_headers.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python +############################################################################# +# +# Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +""" +px_generate_uorb_topic_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 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, prefix=''): + """ + Copies files from inputdir to outputdir if they don't exist in + ouputdir or if their content changed + """ + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + + 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, prefix + 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, prefix): + """ + 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, prefix) + +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') + parser.add_argument('-p', dest='prefix', default='', + help='string added as prefix to the output file ' + ' name when converting directories') + args = parser.parse_args() + + if args.file is not None: + for f in args.file: + 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, + args.prefix) diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile new file mode 100644 index 000000000..1242b56b5 --- /dev/null +++ b/Tools/ros/docker/px4-ros-full/Dockerfile @@ -0,0 +1,57 @@ +# +# PX4 full ROS container +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +FROM ubuntu:14.04.1 +MAINTAINER Andreas Antener <andreas@uaventure.com> + +# Install basics +## Use the "noninteractive" debconf frontend +ENV DEBIAN_FRONTEND noninteractive + +RUN apt-get update \ + && apt-get -y install wget git mercurial + +# Main ROS Setup +# Following http://wiki.ros.org/indigo/Installation/Ubuntu +# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install + +## add ROS repositories and keys +## install main ROS pacakges +RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \ + && wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \ + && apt-get update \ + && apt-get -y install ros-indigo-desktop-full + +RUN rosdep init \ + && rosdep update + +## setup environment variables +RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc + +## get rosinstall +RUN apt-get -y install python-rosinstall + +## additional dependencies +RUN apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy + +## install drcsim +RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \ + && wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \ + && apt-get update \ + && apt-get -y install drcsim + +# Install x11-utils to get xdpyinfo, for X11 display debugging +# mesa-utils provides glxinfo, handy for understanding the 3D support +RUN apt-get -y install x11-utils mesa-utils + +# Some QT-Apps/Gazebo don't not show controls without this +ENV QT_X11_NO_MITSHM 1 + +# FIXME: this doesn't work when building from vagrant +COPY scripts/setup-workspace.sh /root/scripts/ +RUN chmod +x -R /root/scripts/* +RUN chown -R root:root /root/scripts/* + +CMD ["/usr/bin/xterm"] diff --git a/Tools/ros/docker/px4-ros-full/README.md b/Tools/ros/docker/px4-ros-full/README.md new file mode 100644 index 000000000..af5170c70 --- /dev/null +++ b/Tools/ros/docker/px4-ros-full/README.md @@ -0,0 +1,12 @@ +# PX4 ROS # + +Full desktop ROS container. + +License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +**TODO:** + +- use https://github.com/phusion/baseimage-docker as base +- add user, best synced with host +- configure ssh to work with vagrant out of the box + diff --git a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh new file mode 100644 index 000000000..2de5f8bec --- /dev/null +++ b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh @@ -0,0 +1,45 @@ +#!/bin/sh +# +# Create workspace at current location and fetch source repositories +# + +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +WDIR=`pwd` +WORKSPACE=$WDIR/catkin_ws + +# Setup workspace +mkdir -p $WORKSPACE/src +cd $WORKSPACE/src +catkin_init_workspace +cd $WORKSPACE +catkin_make +echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc + +# PX4 Firmware +cd $WORKSPACE/src +git clone https://github.com/PX4/Firmware.git \ + && cd Firmware \ + && git checkout ros + +# euroc simulator +cd $WORKSPACE/src +git clone https://github.com/PX4/euroc_simulator.git \ + && cd euroc_simulator \ + && git checkout px4_nodes + +# mav comm +cd $WORKSPACE/src +git clone https://github.com/PX4/mav_comm.git + +# glog catkin +cd $WORKSPACE/src +git clone https://github.com/ethz-asl/glog_catkin.git + +# catkin simple +cd $WORKSPACE/src +git clone https://github.com/catkin/catkin_simple.git + +cd $WORKSPACE +catkin_make + diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh new file mode 100755 index 000000000..7efc400cd --- /dev/null +++ b/Tools/ros/px4_ros_installation_ubuntu.sh @@ -0,0 +1,41 @@ +#!/bin/sh +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +# main ROS Setup +# following http://wiki.ros.org/indigo/Installation/Ubuntu +# also adding drcsim http://gazebosim.org/tutorials?tut=drcsim_install +# run this file with . ./px4_ros_setup_ubuntu.sh + +## add ROS repository +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' + +## add key +wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \ + sudo apt-key add - + +## Install main ROS pacakges +sudo apt-get update +sudo apt-get -y install ros-indigo-desktop-full +sudo rosdep init +rosdep update + +## Setup environment variables +echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc +. ~/.bashrc + +# get rosinstall +sudo apt-get -y install python-rosinstall + +# additional dependencies +sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy + +## drcsim setup (for models) +### add osrf repository +sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list' + +### add key +wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add - + +### install drcsim +sudo apt-get update +sudo apt-get -y install drcsim diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh new file mode 100755 index 000000000..cf80bcf8d --- /dev/null +++ b/Tools/ros/px4_workspace_create.sh @@ -0,0 +1,9 @@ +#!/bin/sh +# this script creates a catkin_ws in the current folder +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +mkdir -p catkin_ws/src +cd catkin_ws/src +catkin_init_workspace +cd .. +catkin_make diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh new file mode 100755 index 000000000..53568b4fb --- /dev/null +++ b/Tools/ros/px4_workspace_setup.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +# run this script from the root of your catkin_ws +source devel/setup.bash +cd src + +# PX4 Firmware +git clone https://github.com/PX4/Firmware.git +cd Firmware +git checkout ros +cd .. + +# euroc simulator +git clone https://github.com/PX4/euroc_simulator.git +cd euroc_simulator +git checkout px4_nodes +cd .. + +# mav comm +git clone https://github.com/PX4/mav_comm.git + +# glog catkin +git clone https://github.com/ethz-asl/glog_catkin.git + +# catkin simple +git clone https://github.com/catkin/catkin_simple.git + +# drcsim (for scenery and models) + +cd .. + +catkin_make diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile new file mode 100644 index 000000000..06e4e897d --- /dev/null +++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile @@ -0,0 +1,58 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +# +# Vagrantfile to create docker-host-base +# +# Maintainer: Andreas Antener <andreas@uaventure.com> +# +# After build, do "vagrant package --base docker-host-base" to package, +# and import as box: "vagrant box add --name uaventure/docker-host-base package.box" +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +Vagrant.configure(2) do |config| + config.vm.box = "ubuntu/trusty64" + + config.vm.define "docker-host-base" + + config.vm.provider "virtualbox" do |vb| + vb.name = "docker-host-base" + vb.gui = true + vb.memory = "1024" + end + + config.vm.provision "file", source: "config/docker-default", destination: "/home/vagrant/docker-default" + config.vm.provision "file", source: "config/xsessionrc", destination: "/home/vagrant/.xsessionrc" + + config.vm.provision "shell", inline: <<-SHELL + # Update and install apps + export DEBIAN_FRONTEND=noninteractive + sudo apt-get update + sudo apt-get upgrade -y + sudo apt-get install -y --no-install-recommends ubuntu-desktop + sudo apt-get install -y gnome-terminal unity-lens-applications + + # Reset the ssh key (because vagrant regenerates it during provisioning) + sudo wget --no-check-certificate https://raw.github.com/mitchellh/vagrant/master/keys/vagrant.pub -O /home/vagrant/.ssh/authorized_keys + sudo chmod 0700 /home/vagrant/.ssh + sudo chmod 0600 /home/vagrant/.ssh/authorized_keys + sudo chown -R vagrant /home/vagrant/.ssh + + # Copy docker config + sudo mv /home/vagrant/docker-default /etc/default/docker + + # Enable autologin so docker can start GUI apps + sudo echo "autologin-user=vagrant" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf + sudo echo "autologin-user-timeout=0" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf + + # X session RC + chmod +x /home/vagrant/.xsessionrc + SHELL + + config.vm.provision "docker" + + # Shutdown after provisioning. "vagrant halt" doesn't recognize the original ssh key anymore + # and would just kill the VM. This might lead to FS inconsistencies (e.g. in the docker DB). + config.vm.provision "shell", inline: "sudo shutdown -h now" +end diff --git a/Tools/ros/vagrant/docker-host-base/config/docker-default b/Tools/ros/vagrant/docker-host-base/config/docker-default new file mode 100644 index 000000000..19c466b6b --- /dev/null +++ b/Tools/ros/vagrant/docker-host-base/config/docker-default @@ -0,0 +1,29 @@ +# +# Default config for docker /etc/default/docker +# Copied from a provisioned vagrant box +# +# Modifications: +# - listen to TCP port +# - removing deprecated "-r=true" option which apparently doesn't work anymore +# > use restart policies for specific containers if necessary +# + +# Docker Upstart and SysVinit configuration file + +# Customize location of Docker binary (especially for development testing). +#DOCKER="/usr/local/bin/docker" + +# Use DOCKER_OPTS to modify the daemon startup options. +#DOCKER_OPTS="--dns 8.8.8.8 --dns 8.8.4.4" + +# If you need Docker to use an HTTP proxy, it can also be specified here. +#export http_proxy="http://127.0.0.1:3128/" + +# This is also a handy place to tweak where Docker's temporary files go. +#export TMPDIR="/mnt/bigdrive/docker-tmp" + +# Expose TCP port in addition to socket + +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +DOCKER_OPTS="${DOCKER_OPTS} -H unix:///var/run/docker.sock -H 0.0.0.0:2375" diff --git a/Tools/ros/vagrant/docker-host-base/config/xsessionrc b/Tools/ros/vagrant/docker-host-base/config/xsessionrc new file mode 100644 index 000000000..1bd4f8d3f --- /dev/null +++ b/Tools/ros/vagrant/docker-host-base/config/xsessionrc @@ -0,0 +1,6 @@ +#!/bin/sh +# +# Disable X access control so we can easily start GUI apps +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository +xhost + diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile new file mode 100644 index 000000000..2a0a02271 --- /dev/null +++ b/Tools/ros/vagrant/docker-host/Vagrantfile @@ -0,0 +1,38 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +# +# Actual docker host VM to run. +# +# Maintainer: Andreas Antener <andreas@uaventure.com> +# +# To add local docker images into the docker host, configure your local +# docker client to control the docker daemon on the running "docker-host" VM. +# The box ("docker-host-base") configures docker to listen on any IP on port 2375. +# You can then load an existing image, e.g.: +# "docker load -i container-image.tar" +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +Vagrant.configure(2) do |config| + config.vm.box = "uaventure/docker-host-base" + + config.vm.define "docker-host" + + config.vm.provider "virtualbox" do |vb| + vb.name = "docker-host" + vb.gui = true + vb.memory = "4096" + vb.cpus = 2 + vb.customize ["modifyvm", :id, "--graphicscontroller", "vboxvga"] + vb.customize ["modifyvm", :id, "--accelerate3d", "on"] + vb.customize ["modifyvm", :id, "--ioapic", "on"] + vb.customize ["modifyvm", :id, "--vram", "128"] + vb.customize ["modifyvm", :id, "--hwvirtex", "on"] + end + + config.vm.network "private_network", ip: "192.168.59.104" + + # TBD: would it be better to provision docker here instead of in the base box? + #config.vm.provision "docker" +end diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile new file mode 100644 index 000000000..5b372a94d --- /dev/null +++ b/Tools/ros/vagrant/px4-ros/Vagrantfile @@ -0,0 +1,61 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +# +# Boot docker SITL environment +# +# Maintainer: Andreas Antener <andreas@uaventure.com> +# +# "vagrant up" will build the images. Should eventually start "xterm" from within the docker container. +# +# Notes: +# (will change, need proper docs) +# +# Build with multiple dependent docker containers: +# Use the "--no-parallel" option so the containers will be built/started in order. +# e.g.: "vagrant up --no-parallel" +# +# Running apps directly: +# "vagrant docker-run ros -- <cmd>" +# Attention: will loose all data when stopped, vagrant runs docker always with "--rm" +# +# TODO +# - do not run the docker container with "--rm" (vagrant default). is that even possible? +# - maybe map a local working directory to compile stuff without loosing it in side the docker container +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +Vagrant.configure(2) do |config| + # Configure docker host + config.vm.provider "docker" do |d| + d.vagrant_machine = "docker-host" + d.vagrant_vagrantfile = "../docker-host/Vagrantfile" + end + + # Configure docker apps to run + config.vm.define "ros" do |app| + app.vm.provider "docker" do |d| + d.name = "ros" + d.image = "uaventure/px4-ros-full" + #d.build_dir = "../../docker/px4-ros-full" + #d.build_args = ["-t=uaventure/px4-ros-full"] + + # Share docker host x11 socket + # Run privileged to support 3d acceleration + d.volumes = [ + "/tmp/.X11-unix:/tmp/.X11-unix:ro" + ] + d.create_args = ["--privileged"] + + # TODO: get display number from host system + d.env = { + "DISPLAY" => ":0" + } + + d.remains_running = true + d.cmd = ["xterm"] + #d.has_ssh = true + end + end + +end diff --git a/launch/ardrone.launch b/launch/ardrone.launch new file mode 100644 index 000000000..3173e7cf1 --- /dev/null +++ b/launch/ardrone.launch @@ -0,0 +1,15 @@ +<launch> + +<include file="$(find px4)/launch/multicopter_x.launch" /> + +<group ns="px4_multicopter"> + <param name="MPC_XY_P" type="double" value="1.0" /> + <param name="MPC_XY_FF" type="double" value="0.0" /> + <param name="MPC_XY_VEL_P" type="double" value="0.01" /> + <param name="MPC_XY_VEL_I" type="double" value="0.0" /> + <param name="MPC_XY_VEL_D" type="double" value="0.01" /> + <param name="MPC_XY_VEL_MAX" type="double" value="2.0" /> + <param name="vehicle_model" type="string" value="ardrone" /> +</group> + +</launch> diff --git a/launch/example.launch b/launch/example.launch new file mode 100644 index 000000000..39da4b3d4 --- /dev/null +++ b/launch/example.launch @@ -0,0 +1,8 @@ +<launch> + +<group ns="px4_example"> + <node pkg="px4" name="subscriber" type="subscriber"/> + <node pkg="px4" name="publisher" type="publisher"/> +</group> + +</launch> diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch new file mode 100644 index 000000000..395e70b00 --- /dev/null +++ b/launch/gazebo_ardrone_empty_world.launch @@ -0,0 +1,6 @@ +<launch> + +<include file="$(find mav_gazebo)/launch/ardrone_empty_world_with_joy.launch" /> +<include file="$(find px4)/launch/ardrone.launch" /> + +</launch> diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch new file mode 100644 index 000000000..f43c16b50 --- /dev/null +++ b/launch/gazebo_ardrone_house_world.launch @@ -0,0 +1,6 @@ +<launch> + +<include file="$(find mav_gazebo)/launch/ardrone_house_world_with_joy.launch" /> +<include file="$(find px4)/launch/ardrone.launch" /> + +</launch> diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch new file mode 100644 index 000000000..833aaf07a --- /dev/null +++ b/launch/gazebo_iris_empty_world.launch @@ -0,0 +1,6 @@ +<launch> + +<include file="$(find mav_gazebo)/launch/iris_empty_world_with_joy.launch" /> +<include file="$(find px4)/launch/iris.launch" /> + +</launch> diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch new file mode 100644 index 000000000..bfdf9fe31 --- /dev/null +++ b/launch/gazebo_iris_house_world.launch @@ -0,0 +1,6 @@ +<launch> + +<include file="$(find mav_gazebo)/launch/iris_house_world_with_joy.launch" /> +<include file="$(find px4)/launch/iris.launch" /> + +</launch> diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch new file mode 100644 index 000000000..c2975a050 --- /dev/null +++ b/launch/gazebo_iris_outdoor_world.launch @@ -0,0 +1,6 @@ +<launch> + +<include file="$(find mav_gazebo)/launch/iris_outdoor_world_with_joy.launch" /> +<include file="$(find px4)/launch/iris.launch" /> + +</launch> diff --git a/launch/iris.launch b/launch/iris.launch new file mode 100644 index 000000000..be33cb85f --- /dev/null +++ b/launch/iris.launch @@ -0,0 +1,16 @@ +<launch> + +<include file="$(find px4)/launch/multicopter_w.launch" /> + +<group ns="px4_multicopter"> + <param name="mixer" type="string" value="i" /> + <param name="MPC_XY_P" type="double" value="1.0" /> + <param name="MPC_XY_FF" type="double" value="0.0" /> + <param name="MPC_XY_VEL_P" type="double" value="0.01" /> + <param name="MPC_XY_VEL_I" type="double" value="0.0" /> + <param name="MPC_XY_VEL_D" type="double" value="0.01" /> + <param name="MPC_XY_VEL_MAX" type="double" value="2.0" /> + <param name="vehicle_model" type="string" value="iris" /> +</group> + +</launch> diff --git a/launch/multicopter.launch b/launch/multicopter.launch new file mode 100644 index 000000000..95400bd82 --- /dev/null +++ b/launch/multicopter.launch @@ -0,0 +1,15 @@ +<launch> + +<group ns="px4_multicopter"> + <node pkg="joy" name="joy_node" type="joy_node"/> + <node pkg="px4" name="manual_input" type="manual_input"/> + <node pkg="px4" name="commander" type="commander"/> + <node pkg="px4" name="mc_mixer" type="mc_mixer"/> + <node pkg="px4" name="attitude_estimator" type="attitude_estimator"/> + <node pkg="px4" name="position_estimator" type="position_estimator"/> + <node pkg="px4" name="mc_att_control" type="mc_att_control"/> + <node pkg="px4" name="mc_pos_control" type="mc_pos_control"/> + <!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> --> +</group> + +</launch> diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch new file mode 100644 index 000000000..f124082aa --- /dev/null +++ b/launch/multicopter_w.launch @@ -0,0 +1,9 @@ +<launch> + +<include file="$(find px4)/launch/multicopter.launch" /> + +<group ns="px4_multicopter"> + <param name="mixer" type="string" value="w" /> +</group> + +</launch> diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch new file mode 100644 index 000000000..6355b87be --- /dev/null +++ b/launch/multicopter_x.launch @@ -0,0 +1,9 @@ +<launch> + +<include file="$(find px4)/launch/multicopter.launch" /> + +<group ns="px4_multicopter"> + <param name="mixer" type="string" value="x" /> +</group> + +</launch> diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 852edb788..f30733694 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -24,18 +24,18 @@ MODULES += drivers/l3gd20 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -#MODULES += drivers/ll40ls +MODULES += drivers/ll40ls MODULES += drivers/trone -#MODULES += drivers/mb12xx +MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -#MODULES += drivers/blinkm +MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/airspeed -#MODULES += drivers/ets_airspeed +MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -#MODULES += drivers/frsky_telemetry +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # @@ -111,6 +111,7 @@ MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection +MODULES += platforms/nuttx # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82f..256c2a636 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,15 +27,15 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -# MODULES += drivers/sf0x +MODULES += drivers/sf0x MODULES += drivers/ll40ls -# MODULES += drivers/trone +MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil -# MODULES += drivers/hott -# MODULES += drivers/hott/hott_telemetry -# MODULES += drivers/hott/hott_sensors -# MODULES += drivers/blinkm +MODULES += drivers/hott +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed @@ -116,6 +116,7 @@ MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion MODULES += lib/launchdetection +MODULES += platforms/nuttx # # OBC challenge diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk new file mode 100644 index 000000000..29eb68096 --- /dev/null +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -0,0 +1,170 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +# MODULES += drivers/sf0x +MODULES += drivers/ll40ls +# MODULES += drivers/trone +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +# MODULES += drivers/blinkm +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry +MODULES += modules/sensors +MODULES += drivers/mkblctrl +MODULES += drivers/px4flow + +# +# 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 + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/gpio_led +# MODULES += modules/uavcan + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator +MODULES += modules/position_estimator_inav + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control +# MODULES += modules/mc_att_control +MODULES += modules/mc_att_control_multiplatform +MODULES += examples/subscriber +MODULES += examples/publisher +# MODULES += modules/mc_pos_control +MODULES += modules/mc_pos_control_multiplatform +MODULES += modules/vtol_att_control + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB +MODULES += modules/dataman + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion +MODULES += lib/launchdetection +MODULES += platforms/nuttx + +# +# OBC challenge +# +MODULES += modules/bottle_drop + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +#MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# Generate parameter XML file +GEN_PARAM_XML = 1 + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 91fc2a751..22563838b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -26,6 +26,9 @@ MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/pca8574 MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests @@ -33,11 +36,33 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver MODULES += systemcmds/top +MODULES += modules/sensors # -# 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 @@ -49,6 +74,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +MODULES += platforms/nuttx # # Example modules to test-build diff --git a/makefiles/setup.mk b/makefiles/setup.mk index c932a6758..351b9f1b7 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/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d4d73fb84..dc8d3f712 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -128,7 +128,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ # Generic warnings # diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg new file mode 100644 index 000000000..b83adb8f2 --- /dev/null +++ b/msg/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/actuator_controls.msg b/msg/actuator_controls.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_0.msg b/msg/actuator_controls_0.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_0.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_1.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_2.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_3.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_virtual_fw.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_virtual_mc.msg b/msg/actuator_controls_virtual_mc.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_virtual_mc.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/fw_virtual_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg new file mode 100644 index 000000000..d3cfb078b --- /dev/null +++ b/msg/manual_control_setpoint.msg @@ -0,0 +1,44 @@ + +uint8 SWITCH_POS_NONE = 0 # switch is not mapped +uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) +uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) +uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) +uint64 timestamp + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. +# The variable names follow the definition of the +# MANUAL_CONTROL mavlink message. +# The default range is from -1 to 1 (mavlink message -1000 to 1000) +# The range for the z variable is defined from 0 to 1. (The z field of +# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) + +float32 x # stick position in x direction -1..1 + # in general corresponds to forward/back motion or pitch of vehicle, + # in general a positive value means forward or negative pitch and + # a negative value means backward or positive pitch +float32 y # stick position in y direction -1..1 + # in general corresponds to right/left motion or roll of vehicle, + # in general a positive value means right or positive roll and + # a negative value means left or negative roll +float32 z # throttle stick position 0..1 + # in general corresponds to up/down motion or thrust of vehicle, + # in general the value corresponds to the demanded throttle by the user, + # if the input is used for setting the setpoint of a vertical position + # controller any value > 0.5 means up and any value < 0.5 means down +float32 r # yaw stick/twist positon, -1..1 + # in general corresponds to the righthand rotation around the vertical + # (downwards) axis of the vehicle +float32 flaps # flap position +float32 aux1 # default function: camera yaw / azimuth +float32 aux2 # default function: camera pitch / tilt +float32 aux3 # default function: camera trigger +float32 aux4 # default function: camera roll +float32 aux5 # default function: payload drop + +uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO +uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL +uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER +uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD diff --git a/msg/mc_virtual_rates_setpoint.msg b/msg/mc_virtual_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/mc_virtual_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg new file mode 100644 index 000000000..39dc336e0 --- /dev/null +++ b/msg/parameter_update.msg @@ -0,0 +1 @@ +uint64 timestamp # time at which the latest parameter was updated diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg new file mode 100644 index 000000000..ae6756aa8 --- /dev/null +++ b/msg/position_setpoint.msg @@ -0,0 +1,35 @@ +# this file is only used in the position_setpoint triple as a dependency + +uint8 SETPOINT_TYPE_POSITION=0 # position setpoint +uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint +uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) +uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard + +bool valid # true if setpoint is valid +uint8 type # setpoint type to adjust behavior of position controller +float32 x # local position setpoint in m in NED +float32 y # local position setpoint in m in NED +float32 z # local position setpoint in m in NED +bool position_valid # true if local position setpoint valid +float32 vx # local velocity setpoint in m/s in NED +float32 vy # local velocity setpoint in m/s in NED +float32 vz # local velocity setpoint in m/s in NED +bool velocity_valid # true if local velocity setpoint valid +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m +float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw +bool yaw_valid # true if yaw setpoint valid +float32 yawspeed # yawspeed (only for multirotors, in rad/s) +bool yawspeed_valid # true if yawspeed setpoint valid +float32 loiter_radius # loiter radius (only for fixed wing), in m +int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW +float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints +float32 a_x # acceleration x setpoint +float32 a_y # acceleration y setpoint +float32 a_z # acceleration z setpoint +bool acceleration_valid # true if acceleration setpoint is valid/should be used +bool acceleration_is_force # interprete acceleration as force diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg new file mode 100644 index 000000000..8717f65d0 --- /dev/null +++ b/msg/position_setpoint_triplet.msg @@ -0,0 +1,8 @@ +# Global position setpoint triplet in WGS84 coordinates. +# This are the three next waypoints (or just the next two or one). + +px4/position_setpoint previous +px4/position_setpoint current +px4/position_setpoint next + +uint8 nav_state # report the navigation state diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg new file mode 100644 index 000000000..0fa5ed2fc --- /dev/null +++ b/msg/rc_channels.msg @@ -0,0 +1,27 @@ +int32 RC_CHANNELS_FUNCTION_MAX=19 +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 +uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 +uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 +uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 +uint64 timestamp # Timestamp in microseconds since boot time +uint64 timestamp_last_valid # Timestamp of last valid RC signal +float32[19] channels # Scaled to -1..1 (throttle: 0..1) +uint8 channel_count # Number of valid channels +int8[19] 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/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template new file mode 100644 index 000000000..76abab163 --- /dev/null +++ b/msg/templates/px4/ros/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating <msg>.h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include "px4/@(topic_name).h" +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return "@(topic_name)";} +}; + +}; diff --git a/msg/templates/px4/uorb/msg.h.template b/msg/templates/px4/uorb/msg.h.template new file mode 100644 index 000000000..2d4251107 --- /dev/null +++ b/msg/templates/px4/uorb/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating <msg>.h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include <uORB/topics/@(topic_name).h> +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class __EXPORT @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return ORB_ID(@(topic_name));} +}; + +}; diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template new file mode 100644 index 000000000..f16095c97 --- /dev/null +++ b/msg/templates/uorb/msg.h.template @@ -0,0 +1,175 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating <msg>.h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include <stdint.h> +#include <uORB/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 c style +#ifndef __cplusplus +@[for constant in spec.constants]@ +#define @(constant.name) @(int(constant.val)) +@[end for] +#endif + +/** + * @@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', + 'float64': 'double', + 'bool': 'bool', + 'fence_vertex': 'fence_vertex', + 'position_setpoint': 'position_setpoint'} + +# 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)) + +} +#ifdef __cplusplus +@#class @(uorb_struct) { +struct __EXPORT @(uorb_struct) { +@#public: +#else +struct @(uorb_struct) { +#endif +@{ +# loop over all fields and print the type and name +for field in spec.parsed_fields(): + if (not field.is_header): + print_field_def(field) +}@ +#ifdef __cplusplus +@# Constants again c++-ified +@{ +for constant in spec.constants: + type = constant.type + 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('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val))) +} +#endif +}; + +/** + * @@} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(@(topic_name)); diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg new file mode 100644 index 000000000..98018a1df --- /dev/null +++ b/msg/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/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg new file mode 100644 index 000000000..1a8e6e3d5 --- /dev/null +++ b/msg/vehicle_attitude_setpoint.msg @@ -0,0 +1,21 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg new file mode 100644 index 000000000..153a642bb --- /dev/null +++ b/msg/vehicle_control_mode.msg @@ -0,0 +1,22 @@ + +uint64 timestamp # in microseconds since system start + # is set whenever the writing thread stores new data + +bool flag_armed + +bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing + +# XXX needs yet to be set by state machine helper +bool flag_system_hil_enabled + +bool flag_control_manual_enabled # true if manual input is mixed in +bool flag_control_auto_enabled # true if onboard autopilot should act +bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_rates_enabled # true if rates are stabilized +bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_force_enabled # true if force control is mixed in +bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled +bool flag_control_position_enabled # true if position is controlled +bool flag_control_altitude_enabled # true if altitude is controlled +bool flag_control_climb_rate_enabled # true if climb rate is controlled +bool flag_control_termination_enabled # true if flighttermination is enabled diff --git a/msg/vehicle_global_velocity_setpoint.msg b/msg/vehicle_global_velocity_setpoint.msg new file mode 100644 index 000000000..ca7dcc826 --- /dev/null +++ b/msg/vehicle_global_velocity_setpoint.msg @@ -0,0 +1,4 @@ +# Velocity setpoint in NED frame +float32 vx # in m/s NED +float32 vy # in m/s NED +float32 vz # in m/s NED diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg new file mode 100644 index 000000000..4da027ae7 --- /dev/null +++ b/msg/vehicle_local_position.msg @@ -0,0 +1,36 @@ +# Fused local position in NED. + +uint64 timestamp # Time of this estimate, in microseconds since system start +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid + +# Position in local NED frame +float32 x # X position in meters in NED earth-fixed frame +float32 y # X position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) + +# Velocity in NED frame +float32 vx # Ground X Speed (Latitude), m/s in NED +float32 vy # Ground Y Speed (Longitude), m/s in NED +float32 vz # Ground Z Speed (Altitude), m/s in NED + +# Heading +float32 yaw + +# Reference position in GPS / WGS84 frame +bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) +bool z_global # true if z is valid and has valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set +float64 ref_lat # Reference point latitude in degrees +float64 ref_lon # Reference point longitude in degrees +float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + +# Distance to surface +float32 dist_bottom # Distance to bottom surface (ground) +float32 dist_bottom_rate # Distance to bottom surface (ground) change rate +uint64 surface_bottom_timestamp # Time when new bottom surface found +bool dist_bottom_valid # true if distance to bottom surface is valid +float32 eph +float32 epv diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg new file mode 100644 index 000000000..a2ff8a4ae --- /dev/null +++ b/msg/vehicle_local_position_setpoint.msg @@ -0,0 +1,7 @@ +# Local position in NED frame + +uint64 timestamp # timestamp of the setpoint +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED +float32 yaw # in radians NED -PI..+PI diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/vehicle_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg new file mode 100644 index 000000000..18df3252f --- /dev/null +++ b/msg/vehicle_status.msg @@ -0,0 +1,159 @@ +# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. +uint8 MAIN_STATE_MANUAL = 0 +uint8 MAIN_STATE_ALTCTL = 1 +uint8 MAIN_STATE_POSCTL = 2 +uint8 MAIN_STATE_AUTO_MISSION = 3 +uint8 MAIN_STATE_AUTO_LOITER = 4 +uint8 MAIN_STATE_AUTO_RTL = 5 +uint8 MAIN_STATE_ACRO = 6 +uint8 MAIN_STATE_OFFBOARD = 7 +uint8 MAIN_STATE_MAX = 8 + +# If you change the order, add or remove arming_state_t states make sure to update the arrays +# in state_machine_helper.cpp as well. +uint8 ARMING_STATE_INIT = 0 +uint8 ARMING_STATE_STANDBY = 1 +uint8 ARMING_STATE_ARMED = 2 +uint8 ARMING_STATE_ARMED_ERROR = 3 +uint8 ARMING_STATE_STANDBY_ERROR = 4 +uint8 ARMING_STATE_REBOOT = 5 +uint8 ARMING_STATE_IN_AIR_RESTORE = 6 +uint8 ARMING_STATE_MAX = 7 + +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# Navigation state, i.e. "what should vehicle do". +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6 # RC recover mode +uint8 NAVIGATION_STATE_AUTO_RTGS = 7 # Auto return to groundstation on data link loss +uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure +uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down) +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_LAND = 11 # Land mode +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_MAX = 15 + +# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol +uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 +uint8 VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64 +uint8 VEHICLE_MODE_FLAG_HIL_ENABLED = 32 +uint8 VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16 +uint8 VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8 +uint8 VEHICLE_MODE_FLAG_AUTO_ENABLED = 4 +uint8 VEHICLE_MODE_FLAG_TEST_ENABLED = 2 +uint8 VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 + +# VEHICLE_TYPE, should match 1:1 MAVLink's MAV_TYPE ENUM +uint8 VEHICLE_TYPE_GENERIC = 0 # Generic micro air vehicle. +uint8 VEHICLE_TYPE_FIXED_WING = 1 # Fixed wing aircraft. +uint8 VEHICLE_TYPE_QUADROTOR = 2 # Quadrotor +uint8 VEHICLE_TYPE_COAXIAL = 3 # Coaxial helicopter +uint8 VEHICLE_TYPE_HELICOPTER = 4 # Normal helicopter with tail rotor. +uint8 VEHICLE_TYPE_ANTENNA_TRACKER = 5 # Ground installation +uint8 VEHICLE_TYPE_GCS = 6 # Operator control unit / ground control station +uint8 VEHICLE_TYPE_AIRSHIP = 7 # Airship, controlled +uint8 VEHICLE_TYPE_FREE_BALLOON = 8 # Free balloon, uncontrolled +uint8 VEHICLE_TYPE_ROCKET = 9 # Rocket +uint8 VEHICLE_TYPE_GROUND_ROVER = 10 # Ground rover +uint8 VEHICLE_TYPE_SURFACE_BOAT = 11 # Surface vessel, boat, ship +uint8 VEHICLE_TYPE_SUBMARINE = 12 # Submarine +uint8 VEHICLE_TYPE_HEXAROTOR = 13 # Hexarotor +uint8 VEHICLE_TYPE_OCTOROTOR = 14 # Octorotor +uint8 VEHICLE_TYPE_TRICOPTER = 15 # Octorotor +uint8 VEHICLE_TYPE_FLAPPING_WING = 16 # Flapping wing +uint8 VEHICLE_TYPE_KITE = 17 # Kite +uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18 # Onboard companion controller +uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19 # Vtol with two engines +uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20 # Vtol with four engines*/ +uint8 VEHICLE_TYPE_ENUM_END = 21 + +uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning active +uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage +uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage + +# state machine / state of vehicle. +# Encodes the complete system state and is set by the commander app. +uint16 counter # incremented by the writing thread everytime new data is stored +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +uint8 main_state # main state machine +uint8 nav_state # set navigation state machine to specified value +uint8 arming_state # current arming state +uint8 hil_state # current hil state +bool failsafe # true if system is in failsafe state + +int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum +int32 system_id # system id, inspired by MAVLink's system ID field +int32 component_id # subsystem / component id, inspired by MAVLink's component ID field + +bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter +bool is_vtol # True if the system is VTOL capable +bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode + +bool condition_battery_voltage_valid +bool condition_system_in_air_restore # true if we can restore in mid air +bool condition_system_sensors_initialized +bool condition_system_returned_to_home +bool condition_auto_mission_available +bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation +bool condition_launch_position_valid # indicates a valid launch position +bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) +bool condition_local_position_valid +bool condition_local_altitude_valid +bool condition_airspeed_valid # set to true by the commander app if there is a valid airspeed measurement available +bool condition_landed # true if vehicle is landed, always true if disarmed +bool condition_power_input_valid # set if input power is valid +float32 avionics_power_rail_voltage # voltage of the avionics power rail + +bool rc_signal_found_once +bool rc_signal_lost # true if RC reception lost +uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost +bool rc_signal_lost_cmd # true if RC lost mode is commanded +bool rc_input_blocked # set if RC input should be ignored + +bool data_link_lost # datalink to GCS lost +bool data_link_lost_cmd # datalink to GCS lost mode commanded +uint8 data_link_lost_counter # counts unique data link lost events +bool engine_failure # Set to true if an engine failure is detected +bool engine_failure_cmd # Set to true if an engine failure mode is commanded +bool gps_failure # Set to true if a gps failure is detected +bool gps_failure_cmd # Set to true if a gps failure mode is commanded + +bool barometer_failure # Set to true if a barometer failure is detected + +bool offboard_control_signal_found_once +bool offboard_control_signal_lost +bool offboard_control_signal_weak +uint64 offboard_control_signal_lost_interval # interval in microseconds without an offboard control message +bool offboard_control_set_by_command # true if the offboard mode was set by a mavlink command and should not be overridden by RC + +# see SYS_STATUS mavlink message for the following +uint32 onboard_control_sensors_present +uint32 onboard_control_sensors_enabled +uint32 onboard_control_sensors_health + +float32 load # processor load from 0 to 1 +float32 battery_voltage +float32 battery_current +float32 battery_remaining + +uint8 battery_warning # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum +uint16 drop_rate_comm +uint16 errors_comm +uint16 errors_count1 +uint16 errors_count2 +uint16 errors_count3 +uint16 errors_count4 + +bool circuit_breaker_engaged_power_check +bool circuit_breaker_engaged_airspd_check +bool circuit_breaker_engaged_enginefailure_check +bool circuit_breaker_engaged_gpsfailure_check diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..666200390 --- /dev/null +++ b/package.xml @@ -0,0 +1,61 @@ +<?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> + <build_depend>eigen</build_depend> + <run_depend>roscpp</run_depend> + <run_depend>rospy</run_depend> + <run_depend>std_msgs</run_depend> + <run_depend>eigen</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/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 7f1b21a95..7d4b7d880 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,10 @@ #include <uORB/uORB.h> #include <uORB/topics/safety.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/vehicle_control_mode.h> @@ -98,7 +102,7 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ @@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - + /* for now only spin if armed and immediately shut down * if in failsafe */ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 6b14f5945..d0253a8d1 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -571,7 +571,7 @@ BlinkM::led() printf("<blinkm> cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -595,7 +595,7 @@ BlinkM::led() led_color_8 = LED_BLUE; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -647,14 +647,14 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) + if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; - else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 188885be2..882ec6aa2 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -114,7 +114,6 @@ __BEGIN_DECLS * Note that these are unshifted addresses. */ #define PX4_I2C_OBDEV_HMC5883 0x1e -#define PX4_I2C_OBDEV_MS5611 0x76 #define PX4_I2C_OBDEV_EEPROM NOTDEFINED #define PX4_I2C_OBDEV_LED 0x55 diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1eca6155b..52e024c91 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -97,6 +97,8 @@ ORB_DECLARE(sensor_accel); /** set the accel internal sample rate to at least (arg) Hz */ #define ACCELIOCSSAMPLERATE _ACCELIOC(0) +#define ACCEL_SAMPLERATE_DEFAULT 1000003 /**< default sample rate */ + /** return the accel internal sample rate in Hz */ #define ACCELIOCGSAMPLERATE _ACCELIOC(1) diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 5e0334a18..1f2bc35c4 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -93,6 +93,8 @@ ORB_DECLARE(sensor_gyro); /** set the gyro internal sample rate to at least (arg) Hz */ #define GYROIOCSSAMPLERATE _GYROIOC(0) +#define GYRO_SAMPLERATE_DEFAULT 1000003 /**< default sample rate */ + /** return the gyro internal sample rate in Hz */ #define GYROIOCGSAMPLERATE _GYROIOC(1) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 058425676..767ff5803 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,8 @@ #include <systemlib/mixer/mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_outputs.h> @@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[]) int HIL::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -269,19 +271,19 @@ HIL::set_mode(Mode mode) /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_8PWM: debug("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_12PWM: debug("MODE_12PWM"); /* multi-port as 12 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_16PWM: debug("MODE_16PWM"); /* multi-port as 16 PWM outs */ @@ -514,12 +516,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate g_hil->set_pwm_rate(arg); break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate break; case PWM_SERVO_SET(2): @@ -660,7 +662,7 @@ int hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; - + // /* reset to all-inputs */ // g_hil->ioctl(0, GPIO_RESET, 0); @@ -692,17 +694,17 @@ hil_new_mode(PortMode new_mode) /* select 2-pin PWM mode */ servo_mode = HIL::MODE_2PWM; break; - + case PORT2_8PWM: /* select 8-pin PWM mode */ servo_mode = HIL::MODE_8PWM; break; - + case PORT2_12PWM: /* select 12-pin PWM mode */ servo_mode = HIL::MODE_12PWM; break; - + case PORT2_16PWM: /* select 16-pin PWM mode */ servo_mode = HIL::MODE_16PWM; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f583bced4..547bb6868 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -784,8 +784,9 @@ L3GD20::set_samplerate(unsigned frequency) { uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; - if (frequency == 0) + if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) { frequency = _is_l3g4200d ? 800 : 760; + } /* * Use limits good for H or non-H models. Rates are slightly different diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2a0bf522b..6edb9d787 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1307,8 +1307,9 @@ LSM303D::accel_set_samplerate(unsigned frequency) uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; - if (frequency == 0) + if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) { frequency = 1600; + } if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 15412984b..1331744ae 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/esc_status.h> diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e322e8b3a..1c4dfb89e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -402,7 +402,7 @@ private: /* set sample rate (approximate) - 1kHz to 5Hz */ - void _set_sample_rate(uint16_t desired_sample_rate_hz); + void _set_sample_rate(unsigned desired_sample_rate_hz); /* check that key registers still have the right value @@ -794,13 +794,19 @@ MPU6000::probe() set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro */ void -MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) +MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) { - uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); - _sample_rate = 1000 / div; + if (desired_sample_rate_hz == 0 || + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE; + } + + uint8_t div = 1000 / desired_sample_rate_hz; + if(div>200) div=200; + if(div<1) div=1; + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; } /* diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 75b1f65fd..31dbdbcd3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -69,6 +69,14 @@ #include "ms5611.h" +enum MS5611_BUS { + MS5611_BUS_ALL = 0, + MS5611_BUS_I2C_INTERNAL, + MS5611_BUS_I2C_EXTERNAL, + MS5611_BUS_SPI_INTERNAL, + MS5611_BUS_SPI_EXTERNAL +}; + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -784,15 +792,38 @@ MS5611::print_info() namespace ms5611 { -/* initialize explicitely for clarity */ -MS5611 *g_dev_ext = nullptr; -MS5611 *g_dev_int = nullptr; +/* + list of supported bus configurations + */ +struct ms5611_bus_option { + enum MS5611_BUS busid; + const char *devpath; + MS5611_constructor interface_constructor; + uint8_t busnum; + MS5611 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, +#endif +#ifdef PX4_SPIDEV_BARO + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(bool external_bus); -void test(bool external_bus); -void reset(bool external_bus); +bool start_bus(struct ms5611_bus_option &bus); +struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); +void start(enum MS5611_BUS busid); +void test(enum MS5611_BUS busid); +void reset(enum MS5611_BUS busid); void info(); -void calibrate(unsigned altitude, bool external_bus); +void calibrate(unsigned altitude, enum MS5611_BUS busid); void usage(); /** @@ -845,89 +876,87 @@ crc4(uint16_t *n_prom) /** * Start the driver. */ -void -start(bool external_bus) +bool +start_bus(struct ms5611_bus_option &bus) { - int fd; - prom_u prom_buf; - - if (external_bus && (g_dev_ext != nullptr)) { - /* if already started, the still command succeeded */ - errx(0, "ext already started"); - } else if (!external_bus && (g_dev_int != nullptr)) { - /* if already started, the still command succeeded */ - errx(0, "int already started"); - } - - device::Device *interface = nullptr; - - /* create the driver, try SPI first, fall back to I2C if unsuccessful */ - if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(prom_buf, external_bus); - if (interface == nullptr && (MS5611_i2c_interface != nullptr)) - interface = MS5611_i2c_interface(prom_buf); - - if (interface == nullptr) - errx(1, "failed to allocate an interface"); + if (bus.dev != nullptr) + errx(1,"bus option already started"); + prom_u prom_buf; + device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); if (interface->init() != OK) { delete interface; - errx(1, "interface init failed"); + warnx("no device on bus %u", (unsigned)bus.busid); + return false; } - if (external_bus) { - g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT); - if (g_dev_ext == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); - } - - if (g_dev_ext->init() != OK) - goto fail; - - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - - } else { - - g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT); - if (g_dev_int == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); - } - - if (g_dev_int->init() != OK) - goto fail; - - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - + bus.dev = new MS5611(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; } + + int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ - - if (fd < 0) { - warnx("can't open baro device"); - goto fail; + if (fd == -1) { + errx(1, "can't open baro device"); } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warnx("failed setting default poll rate"); - goto fail; + close(fd); + errx(1, "failed setting default poll rate"); } - exit(0); + close(fd); + return true; +} -fail: - if (g_dev_int != nullptr) { - delete g_dev_int; - g_dev_int = nullptr; - } +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum MS5611_BUS busid) +{ + uint8_t i; + bool started = false; - if (g_dev_ext != nullptr) { - delete g_dev_ext; - g_dev_ext = nullptr; + for (i=0; i<NUM_BUS_OPTIONS; i++) { + if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + started |= start_bus(bus_options[i]); } - errx(1, "driver start failed"); + if (!started) + errx(1, "driver start failed"); + + // one or more drivers started OK + exit(0); +} + + +/** + * find a bus structure for a busid + */ +struct ms5611_bus_option &find_bus(enum MS5611_BUS busid) +{ + for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) { + if ((busid == MS5611_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != NULL) { + return bus_options[i]; + } + } + errx(1,"bus %u not started", (unsigned)busid); } /** @@ -936,20 +965,16 @@ fail: * and automatic modes. */ void -test(bool external_bus) +test(enum MS5611_BUS busid) { + struct ms5611_bus_option &bus = find_bus(busid); struct baro_report report; ssize_t sz; int ret; int fd; - if (external_bus) { - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - } else { - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - } - + fd = open(bus.devpath, O_RDONLY); if (fd < 0) err(1, "open failed (try 'ms5611 start' if the driver is not running)"); @@ -998,6 +1023,7 @@ test(bool external_bus) warnx("time: %lld", report.timestamp); } + close(fd); errx(0, "PASS"); } @@ -1005,16 +1031,12 @@ test(bool external_bus) * Reset the driver. */ void -reset(bool external_bus) +reset(enum MS5611_BUS busid) { + struct ms5611_bus_option &bus = find_bus(busid); int fd; - if (external_bus) { - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - } else { - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - } - + fd = open(bus.devpath, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1033,19 +1055,13 @@ reset(bool external_bus) void info() { - if (g_dev_ext == nullptr && g_dev_int == nullptr) - errx(1, "driver not running"); - - if (g_dev_ext) { - warnx("ext:"); - g_dev_ext->print_info(); - } - - if (g_dev_int) { - warnx("int:"); - g_dev_int->print_info(); + for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) { + struct ms5611_bus_option &bus = bus_options[i]; + if (bus.dev != nullptr) { + warnx("%s", bus.devpath); + bus.dev->print_info(); + } } - exit(0); } @@ -1053,19 +1069,16 @@ info() * Calculate actual MSL pressure given current altitude */ void -calibrate(unsigned altitude, bool external_bus) +calibrate(unsigned altitude, enum MS5611_BUS busid) { + struct ms5611_bus_option &bus = find_bus(busid); struct baro_report report; float pressure; float p1; int fd; - if (external_bus) { - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - } else { - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - } + fd = open(bus.devpath, O_RDONLY); if (fd < 0) err(1, "open failed (try 'ms5611 start' if the driver is not running)"); @@ -1120,6 +1133,7 @@ calibrate(unsigned altitude, bool external_bus) if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) err(1, "BAROIOCSMSLPRESSURE"); + close(fd); exit(0); } @@ -1128,7 +1142,10 @@ usage() { warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); warnx("options:"); - warnx(" -X (external bus)"); + warnx(" -X (external I2C bus)"); + warnx(" -I (intternal I2C bus)"); + warnx(" -S (external SPI bus)"); + warnx(" -s (internal SPI bus)"); } } // namespace @@ -1136,14 +1153,23 @@ usage() int ms5611_main(int argc, char *argv[]) { - bool external_bus = false; + enum MS5611_BUS busid = MS5611_BUS_ALL; int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XISs")) != EOF) { switch (ch) { case 'X': - external_bus = true; + busid = MS5611_BUS_I2C_EXTERNAL; + break; + case 'I': + busid = MS5611_BUS_I2C_INTERNAL; + break; + case 'S': + busid = MS5611_BUS_SPI_EXTERNAL; + break; + case 's': + busid = MS5611_BUS_SPI_INTERNAL; break; default: ms5611::usage(); @@ -1153,29 +1179,23 @@ ms5611_main(int argc, char *argv[]) const char *verb = argv[optind]; - if (argc > optind+1) { - if (!strcmp(argv[optind+1], "-X")) { - external_bus = true; - } - } - /* * Start/load the driver. */ if (!strcmp(verb, "start")) - ms5611::start(external_bus); + ms5611::start(busid); /* * Test the driver/device. */ if (!strcmp(verb, "test")) - ms5611::test(external_bus); + ms5611::test(busid); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - ms5611::reset(external_bus); + ms5611::reset(busid); /* * Print driver information. @@ -1192,7 +1212,7 @@ ms5611_main(int argc, char *argv[]) long altitude = strtol(argv[optind+1], nullptr, 10); - ms5611::calibrate(altitude, external_bus); + ms5611::calibrate(altitude, busid); } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 3f1f6c473..c8211b8dd 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function; -extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; - +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; +typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 87d9b94a6..ca651409f 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -56,14 +56,6 @@ #include "board_config.h" -#ifdef PX4_I2C_OBDEV_MS5611 - -#ifndef PX4_I2C_BUS_ONBOARD - #define MS5611_BUS 1 -#else - #define MS5611_BUS PX4_I2C_BUS_ONBOARD -#endif - #define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ #define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ @@ -74,7 +66,7 @@ device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf); class MS5611_I2C : public device::I2C { public: - MS5611_I2C(int bus, ms5611::prom_u &prom_buf); + MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf); virtual ~MS5611_I2C(); virtual int init(); @@ -113,12 +105,12 @@ private: }; device::Device * -MS5611_i2c_interface(ms5611::prom_u &prom_buf) +MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) { - return new MS5611_I2C(MS5611_BUS, prom_buf); + return new MS5611_I2C(busnum, prom_buf); } -MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) : +MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : I2C("MS5611_I2C", nullptr, bus, 0, 400000), _prom(prom) { @@ -274,5 +266,3 @@ MS5611_I2C::_read_prom() /* calculate CRC and return success/failure accordingly */ return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; } - -#endif /* PX4_I2C_OBDEV_MS5611 */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 5234ce8d6..554a1d659 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -60,14 +60,14 @@ #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#ifdef PX4_SPIDEV_BARO +#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO) device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus); class MS5611_SPI : public device::SPI { public: - MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf); + MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf); virtual ~MS5611_SPI(); virtual int init(); @@ -115,20 +115,21 @@ private: }; device::Device * -MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) +MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) { - if (external_bus) { - #ifdef PX4_SPI_BUS_EXT - return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); - #else +#ifdef PX4_SPI_BUS_EXT + if (busnum == PX4_SPI_BUS_EXT) { +#ifdef PX4_SPIDEV_EXT_BARO + return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); +#else return nullptr; - #endif - } else { - return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); +#endif } +#endif + return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } -MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : +MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) : SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { @@ -281,4 +282,4 @@ MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) return transfer(send, recv, len); } -#endif /* PX4_SPIDEV_BARO */ +#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 363d2cdcf..8fcdc8023 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -70,6 +70,10 @@ #include <drivers/drv_rc_input.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> @@ -138,11 +142,11 @@ private: uint32_t _groups_required; uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; int _actuator_output_topic_instance; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; pwm_limit_t _pwm_limit; @@ -508,7 +512,7 @@ PX4FMU::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); @@ -585,7 +589,7 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); } @@ -612,7 +616,7 @@ PX4FMU::task_main() /* get controls for required topics */ unsigned poll_id = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); @@ -745,7 +749,7 @@ PX4FMU::task_main() } - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; @@ -1142,7 +1146,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) * and PWM under control of the flight config * parameters. Note that this does not allow for * changing a set of pins to be used for serial on - * FMUv1 + * FMUv1 */ switch (arg) { case 0: diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 653d0d5d7..3a9e8ef24 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,10 @@ #include <systemlib/circuit_breaker.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/safety.h> @@ -1199,7 +1203,7 @@ PX4IO::io_set_arming_state() if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; - + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } @@ -2585,7 +2589,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_RC_CONFIG: { /* enable setting of RC configuration without relying - on param_get() + on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; if (config->channel >= RC_INPUT_MAX_CHANNELS) { @@ -3267,7 +3271,23 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "safety_off")) { + int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); + if (ret != OK) { + printf("failed to disable safety\n"); + exit(1); + } + exit(0); + } + if (!strcmp(argv[1], "safety_on")) { + int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); + if (ret != OK) { + printf("failed to enable safety\n"); + exit(1); + } + exit(0); + } if (!strcmp(argv[1], "recovery")) { @@ -3396,6 +3416,6 @@ px4io_main(int argc, char *argv[]) out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n" - "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n" + "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index fdaa7f27b..5b945e452 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -54,6 +54,7 @@ #include <mavlink/mavlink_log.h> #include <uORB/Publication.hpp> +#include <uORB/topics/actuator_controls_0.h> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> @@ -65,7 +66,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), @@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor) uint8_t rbuf[50]; usleep(5000); int nread = read(_uart, rbuf, 50); - if (nread < 6) { + if (nread < 6) { printf("failed to read\n"); return -1; } @@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; uint8_t checksum = rbuf[5]; - uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & + uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & checksum_mask; // check if checksum is valid if (checksum != checksum_computed) { @@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor) static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; - _motor1Position = float(int64_t(count) + + _motor1Position = float(int64_t(count) + _motor1Overflow*overflowAmount)/_pulsesPerRev; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; - _motor2Position = float(int64_t(count) + + _motor2Position = float(int64_t(count) + _motor2Overflow*overflowAmount)/_pulsesPerRev; } return 0; @@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) return -1; } -int RoboClaw::resetEncoders() +int RoboClaw::resetEncoders() { uint16_t sum = 0; return _sendCommand(CMD_RESET_ENCODERS, @@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) return sum; } -int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, +int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum) { tcflush(_uart, TCIOFLUSH); // flush buffers @@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, return write(_uart, buf, n_data + 3); } -int roboclawTest(const char *deviceName, uint8_t address, +int roboclawTest(const char *deviceName, uint8_t address, uint16_t pulsesPerRev) { printf("roboclaw test: starting\n"); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index fcbb54c8e..d8b777b91 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -61,6 +61,10 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/parameter_update.h> @@ -108,7 +112,7 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); /** @@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { - /* + /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing * moments applied to the vehicle frame. This vector * is structured as: * * Control Group 0 (attitude): - * + * * 0 - roll (-1..+1) * 1 - pitch (-1..+1) * 2 - yaw (-1..+1) @@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) * * Wikipedia description: * http://en.wikipedia.org/wiki/Publish–subscribe_pattern - * + * */ @@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* * Declare and safely initialize all structs to zero. - * + * * These structs contain the system state and things * like attitude, position, the current waypoint, etc. */ @@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (ret < 0) { /* * Poll error, this will not really happen in practice, - * but its good design practice to make output an error message. + * but its good design practice to make output an error message. */ warnx("poll error"); @@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } if (manual_sp_updated) - /* get the RC (or otherwise user based) input */ + /* get the RC (or otherwise user based) input */ orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ 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 a89ccf933..1ac0c2d04 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/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 95ff346bb..8fd8870da 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -47,6 +47,10 @@ #include <systemlib/err.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/uORB.h> __EXPORT int ex_hwtest_main(int argc, char *argv[]); diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk new file mode 100644 index 000000000..62a5f8dd1 --- /dev/null +++ b/src/examples/publisher/module.mk @@ -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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = publisher + +SRCS = publisher_main.cpp \ + publisher_start_nuttx.cpp \ + publisher_example.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp new file mode 100644 index 000000000..e7ab9eb5a --- /dev/null +++ b/src/examples/publisher/publisher_example.cpp @@ -0,0 +1,83 @@ + +/**************************************************************************** + * + * 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 publisher_example.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "publisher_example.h" + +using namespace px4; + +PublisherExample::PublisherExample() : + _n(), + _rc_channels_pub(_n.advertise<px4_rc_channels>()), + _v_att_pub(_n.advertise<px4_vehicle_attitude>()), + _parameter_update_pub(_n.advertise<px4_parameter_update>()) +{ +} + +int PublisherExample::main() +{ + px4::Rate loop_rate(10); + + while (px4::ok()) { + loop_rate.sleep(); + _n.spinOnce(); + + /* Publish example message */ + px4_rc_channels rc_channels_msg; + rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid); + _rc_channels_pub->publish(rc_channels_msg); + + /* Publish example message */ + px4_vehicle_attitude v_att_msg; + v_att_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("att: %llu", v_att_msg.data().timestamp); + _v_att_pub->publish(v_att_msg); + + /* Publish example message */ + px4_parameter_update parameter_update_msg; + parameter_update_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp); + _parameter_update_pub->publish(parameter_update_msg); + + } + + return 0; +} diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h new file mode 100644 index 000000000..8165b0ffc --- /dev/null +++ b/src/examples/publisher/publisher_example.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 publisher.h + * Example publisher for ros and px4 + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#pragma once +#include <px4.h> + +class PublisherExample { +public: + PublisherExample(); + + ~PublisherExample() {}; + + int main(); +protected: + px4::NodeHandle _n; + px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub; + px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub; + px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub; +}; diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp new file mode 100644 index 000000000..11439acff --- /dev/null +++ b/src/examples/publisher/publisher_main.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 publisher_main.cpp + * Example publisher for ros and px4 + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include "publisher_example.h" + +bool thread_running = false; /**< Deamon status flag */ + +int main(int argc, char **argv) +{ + px4::init(argc, argv, "publisher"); + + PX4_INFO("starting"); + PublisherExample p; + thread_running = true; + p.main(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp new file mode 100644 index 000000000..db9d85269 --- /dev/null +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 publisher_start_nuttx.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int publisher_main(int argc, char *argv[]); +int publisher_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: publisher {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("publisher", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk new file mode 100644 index 000000000..0e02c65b4 --- /dev/null +++ b/src/examples/subscriber/module.mk @@ -0,0 +1,45 @@ +############################################################################ +# +# 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_main.cpp \ + subscriber_start_nuttx.cpp \ + subscriber_example.cpp \ + subscriber_params.c + +MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp new file mode 100644 index 000000000..e1292f788 --- /dev/null +++ b/src/examples/subscriber/subscriber_example.cpp @@ -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 subscriber_example.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "subscriber_params.h" +#include "subscriber_example.h" + +using namespace px4; + +void rc_channels_callback_function(const px4_rc_channels &msg) { + PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); +} + +SubscriberExample::SubscriberExample() : + _n(), + _p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)), + _interval(0), + _p_test_float(PX4_PARAM_INIT(SUB_TESTF)), + _test_float(0.0f) +{ + /* Read the parameter back as example */ + PX4_PARAM_GET(_p_sub_interv, &_interval); + PX4_INFO("Param SUB_INTERV = %d", _interval); + PX4_PARAM_GET(_p_test_float, &_test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); + + /* Do some subscriptions */ + /* Function */ + _n.subscribe<px4_rc_channels>(rc_channels_callback_function, _interval); + + /* No callback */ + _sub_rc_chan = _n.subscribe<px4_rc_channels>(500); + + /* Class method */ + _n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000); + + /* Another class method */ + _n.subscribe<px4_vehicle_attitude>(&SubscriberExample::vehicle_attitude_callback, this, 1000); + + /* Yet antoher class method */ + _n.subscribe<px4_parameter_update>(&SubscriberExample::parameter_update_callback, this, 1000); + + PX4_INFO("subscribed"); +} + +/** + * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. + * Also the current value of the _sub_rc_chan subscription is printed + */ +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { + PX4_INFO("rc_channels_callback (method): [%llu]", + msg.data().timestamp_last_valid); + PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", + _sub_rc_chan->data().timestamp_last_valid); +} + +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { + PX4_INFO("vehicle_attitude_callback (method): [%llu]", + msg.data().timestamp); +} + +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { + PX4_INFO("parameter_update_callback (method): [%llu]", + msg.data().timestamp); + PX4_PARAM_GET(_p_sub_interv, &_interval); + PX4_INFO("Param SUB_INTERV = %d", _interval); + PX4_PARAM_GET(_p_test_float, &_test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); +} diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h new file mode 100644 index 000000000..9b6d890e3 --- /dev/null +++ b/src/examples/subscriber/subscriber_example.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 subscriber_example.h + * Example subscriber for ros and px4 + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <px4.h> + +using namespace px4; + +void rc_channels_callback_function(const px4_rc_channels &msg); + +class SubscriberExample { +public: + SubscriberExample(); + + ~SubscriberExample() {}; + + void spin() {_n.spin();} +protected: + px4::NodeHandle _n; + px4_param_t _p_sub_interv; + int32_t _interval; + px4_param_t _p_test_float; + float _test_float; + px4::Subscriber<px4_rc_channels> * _sub_rc_chan; + + void rc_channels_callback(const px4_rc_channels &msg); + void vehicle_attitude_callback(const px4_vehicle_attitude &msg); + void parameter_update_callback(const px4_parameter_update &msg); + +}; diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp new file mode 100644 index 000000000..798c74163 --- /dev/null +++ b/src/examples/subscriber/subscriber_main.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 subscriber_main.cpp + * Example subscriber for ros and px4 + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include "subscriber_example.h" +bool thread_running = false; /**< Deamon status flag */ + +int main(int argc, char **argv) +{ + px4::init(argc, argv, "subscriber"); + + PX4_INFO("starting"); + SubscriberExample s; + thread_running = true; + s.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/examples/subscriber/subscriber_params.c index 5dac877d0..a43817b5b 100644 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ b/src/examples/subscriber/subscriber_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin <anton.babushkin@me.com> + * 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 @@ -33,31 +32,25 @@ ****************************************************************************/ /** - * @file vehicle_global_velocity_setpoint.h - * Definition of the global velocity setpoint uORB topic. + * @file subscriber_params.c + * Parameters for the subscriber example + * + * @author Thomas Gubler <thomasgubler@gmail.com> */ -#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ -#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ - -#include "../uORB.h" +#include <px4_defines.h> +#include "subscriber_params.h" /** - * @addtogroup topics - * @{ + * Interval of one subscriber in the example in ms + * + * @group Subscriber Example */ - -struct vehicle_global_velocity_setpoint_s { - float vx; /**< in m/s NED */ - float vy; /**< in m/s NED */ - float vz; /**< in m/s NED */ -}; /**< Velocity setpoint in NED frame */ +PX4_PARAM_DEFINE_INT32(SUB_INTERV); /** - * @} + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_velocity_setpoint); - -#endif +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/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp new file mode 100644 index 000000000..6129b19ac --- /dev/null +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 subscriber_start_nuttx.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); +int subscriber_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: subscriber {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("subscriber", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/include/px4.h b/src/include/px4.h new file mode 100644 index 000000000..34137ee6f --- /dev/null +++ b/src/include/px4.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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" +#ifdef __cplusplus +#include "../platforms/px4_middleware.h" +#include "../platforms/px4_nodehandle.h" +#include "../platforms/px4_subscriber.h" +#endif diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 2311e0a7c..ca3587b93 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -44,10 +44,7 @@ */ #pragma once - -#include "uORB/topics/fence.h" -#include "uORB/topics/vehicle_global_position.h" - +#include <platforms/px4_defines.h> __BEGIN_DECLS #include "geo_lookup/geo_mag_declination.h" diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index d4c892d8a..e16f33bd6 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -46,6 +46,9 @@ namespace math { +#ifndef CONFIG_ARCH_ARM +#define M_PI_F 3.14159265358979323846f +#endif float __EXPORT min(float val1, float val2) { @@ -143,4 +146,4 @@ double __EXPORT degrees(double radians) return (radians / M_PI) * 180.0; } -}
\ No newline at end of file +} diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fb778dd66..fca4197b8 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,7 +39,7 @@ #pragma once -#include <nuttx/config.h> +#include <platforms/px4_defines.h> #include <stdint.h> namespace math { @@ -84,4 +84,4 @@ float __EXPORT degrees(float radians); double __EXPORT degrees(double radians); -}
\ No newline at end of file +} diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ac1f1538f..f6f4fc5ea 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -44,12 +44,20 @@ #define MATRIX_HPP #include <stdio.h> +#include <math.h> + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include <platforms/ros/eigen_math.h> +#include <Eigen/Eigen> +#endif +#include <platforms/px4_defines.h> namespace math { -template <unsigned int M, unsigned int N> +template<unsigned int M, unsigned int N> class __EXPORT Matrix; // MxN matrix with float elements @@ -65,7 +73,11 @@ public: /** * struct for using arm_math functions */ +#ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_mat; +#else + eigen_matrix_instance arm_mat; +#endif /** * trivial ctor @@ -114,6 +126,15 @@ public: memcpy(data, d, sizeof(data)); } +#if defined(__PX4_ROS) + /** + * set data from boost::array + */ + void set(const boost::array<float, 9ul> d) { + set(static_cast<const float*>(d.data())); + } +#endif + /** * access by index */ @@ -273,27 +294,53 @@ public: */ template <unsigned int P> Matrix<M, P> operator *(const Matrix<N, P> &m) const { +#ifdef CONFIG_ARCH_ARM Matrix<M, P> res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> > + (this->arm_mat.pData); + Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> > + (m.arm_mat.pData); + Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him; + Matrix<M, P> res(Product.data()); + return res; +#endif } /** * transpose the matrix */ Matrix<N, M> transposed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix<N, M> res; arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> > + (this->arm_mat.pData); + Me.transposeInPlace(); + Matrix<N, M> res(Me.data()); + return res; +#endif } /** * invert the matrix */ Matrix<M, N> inversed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix<M, N> res; arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> > + (this->arm_mat.pData); + Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea + Matrix<M, N> res(MyInverse.data()); + return res; +#endif } /** @@ -352,8 +399,17 @@ public: * multiplication by a vector */ Vector<M> operator *(const Vector<N> &v) const { +#ifdef CONFIG_ARCH_ARM Vector<M> res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); +#else + //probably nicer if this could go into a function like "eigen_mat_mult" or so + Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> > + (this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N); + Eigen::VectorXf Product = Me * Vec; + Vector<M> res(Product.data()); +#endif return res; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 21d05c7ef..b3cca30c6 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,7 +44,7 @@ #define QUATERNION_HPP #include <math.h> -#include "../CMSIS/Include/arm_math.h" + #include "Vector.hpp" #include "Matrix.hpp" diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 0ddf77615..781c14d53 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -45,7 +45,14 @@ #include <stdio.h> #include <math.h> + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include <platforms/ros/eigen_math.h> +#endif + +#include <platforms/px4_defines.h> namespace math { @@ -65,7 +72,12 @@ public: /** * struct for using arm_math functions, represents column vector */ + #ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_col; + #else + eigen_matrix_instance arm_col; + #endif + /** * trivial ctor 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 b0086676a..86f59f0e6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -592,7 +592,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/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 4580b338d..b267209fe 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -57,8 +57,13 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/wind_estimate.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_global_position.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 13ab966ab..d9e7e21fc 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -221,17 +221,17 @@ int do_accel_calibration(int mavlink_fd) accel_scale.z_scale = accel_T_rotated(2, 2); /* set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + if (param_set(param_find("CAL_ACC0_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } - if (param_set(param_find("SENS_ACC_ID"), &(device_id))) { + if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) { res = ERROR; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 247a2c5b8..8f491acae 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -74,6 +74,10 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/differential_pressure.h> @@ -364,31 +368,31 @@ void print_status() const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: + case vehicle_status_s::ARMING_STATE_INIT: armed_str = "INIT"; break; - case ARMING_STATE_STANDBY: + case vehicle_status_s::ARMING_STATE_STANDBY: armed_str = "STANDBY"; break; - case ARMING_STATE_ARMED: + case vehicle_status_s::ARMING_STATE_ARMED: armed_str = "ARMED"; break; - case ARMING_STATE_ARMED_ERROR: + case vehicle_status_s::ARMING_STATE_ARMED_ERROR: armed_str = "ARMED_ERROR"; break; - case ARMING_STATE_STANDBY_ERROR: + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: armed_str = "STANDBY_ERROR"; break; - case ARMING_STATE_REBOOT: + case vehicle_status_s::ARMING_STATE_REBOOT: armed_str = "REBOOT"; break; - case ARMING_STATE_IN_AIR_RESTORE: + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: armed_str = "IN_AIR_RESTORE"; break; @@ -411,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { @@ -448,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t main_ret = TRANSITION_NOT_CHANGED; /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state @@ -458,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } } } @@ -521,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Flick to inair restore first if this comes from an onboard system if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { - status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; + status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); @@ -546,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -664,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; - static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL; - if (status_local->main_state != MAIN_STATE_OFFBOARD) { + if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } if (cmd->param1 > 0.5f) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -794,41 +798,41 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); - const char *main_states_str[MAIN_STATE_MAX]; - main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; - main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; - main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; - main_states_str[MAIN_STATE_ACRO] = "ACRO"; - main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD"; - - const char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[ARMING_STATE_INIT] = "INIT"; - arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; - arming_states_str[ARMING_STATE_ARMED] = "ARMED"; - arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - - const char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; - nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; - nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; - nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; - nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; + const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; + main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; + + const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; + arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -849,10 +853,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = true; // initialize to safe value // We want to accept RC inputs as default status.rc_input_blocked = false; - status.main_state = MAIN_STATE_MANUAL; - status.nav_state = NAVIGATION_STATE_MANUAL; - status.arming_state = ARMING_STATE_INIT; - status.hil_state = HIL_STATE_OFF; + status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; + status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + status.arming_state = vehicle_status_s::ARMING_STATE_INIT; + status.hil_state = vehicle_status_s::HIL_STATE_OFF; status.failsafe = false; /* neither manual nor offboard control commands have been received */ @@ -865,7 +869,7 @@ int commander_thread_main(int argc, char *argv[]) status.data_link_lost = true; /* set battery warning flag */ - status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized @@ -1139,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[]) } /* disable manual override for all systems that rely on electronic stabilization */ - if (status.system_type == VEHICLE_TYPE_COAXIAL || - status.system_type == VEHICLE_TYPE_HELICOPTER || - status.system_type == VEHICLE_TYPE_TRICOPTER || - status.system_type == VEHICLE_TYPE_QUADROTOR || - status.system_type == VEHICLE_TYPE_HEXAROTOR || - status.system_type == VEHICLE_TYPE_OCTOROTOR || - (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { + if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { status.is_rotary_wing = true; @@ -1155,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1306,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { @@ -1340,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[]) status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) { + if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1512,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f @@ -1520,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1531,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1545,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1658,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && + (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -1684,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && + if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1692,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state != MAIN_STATE_MANUAL) { + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1715,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[]) } if (arming_ret == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { @@ -1853,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the data link and the gps system have been checked * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ - if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && + status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && + status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1877,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the rc signal and the gps system have been checked * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ - if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && + if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || + status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1972,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -2068,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu bool set_normal_color = false; /* set mode */ - if (status_local->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status_local->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -2087,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (set_normal_color) { /* set color */ - if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { + if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); - /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ } else { if (status_local->condition_local_position_valid) { @@ -2145,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* if offboard is set allready by a mavlink command, abort */ if (status.offboard_control_set_by_command) { - return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); } /* offboard switch overrides main switch */ - if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2162,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switched off or denied, check main mode switch */ switch (sp_man->mode_switch) { - case SWITCH_POS_NONE: + case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; break; - case SWITCH_POS_OFF: // MANUAL - if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_ACRO); + case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL + if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); } else { - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST + if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2189,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctl_switch != SWITCH_POS_ON) { + if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); + case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2215,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2231,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2240,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2248,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -2279,11 +2283,11 @@ set_control_mode() /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); - control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); @@ -2295,7 +2299,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2307,7 +2311,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2319,12 +2323,12 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_MISSION: - case NAVIGATION_STATE_AUTO_LOITER: - case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RCRECOVER: - case NAVIGATION_STATE_AUTO_RTGS: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2336,7 +2340,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2348,7 +2352,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2361,7 +2365,7 @@ set_control_mode() break; - case NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2374,7 +2378,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: /* TODO: check if this makes sense */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2387,7 +2391,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -2400,7 +2404,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; @@ -2599,7 +2603,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; @@ -2663,7 +2667,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 2022e99fb..8a4451100 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -72,16 +72,16 @@ static const int ERROR = -1; bool is_multirotor(const struct vehicle_status_s *current_status) { - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL); } static int buzzer = -1; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 3cfa8b4c6..4ddb4e0fb 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool armed; // actuator_armed_s.armed bool ready_to_arm; // actuator_armed_s.ready_to_arm } ArmingTransitionVolatileState_t; - + // This structure represents a test case for arming_state_transition. It contains the machine // state prior to transtion, the requested state to transition to and finally the expected // machine state after transition. @@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition transition_result_t expected_transition_result; // Expected result from arming_state_transition } ArmingTransitionTest_t; - + // We use these defines so that our test cases are more readable #define ATT_ARMED true #define ATT_DISARMED false @@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) #define ATT_SAFETY_NOT_AVAILABLE true #define ATT_SAFETY_OFF true #define ATT_SAFETY_ON false - + // These are test cases for arming_state_transition static const ArmingTransitionTest_t rgArmingTransitionTests[] = { // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s - + { "transition: init to standby", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to standby error", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to reboot", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to init", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to standby error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to reboot", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to standby", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to armed error", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed error to standby error", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby error to reboot", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to armed", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to reboot", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // hil on tests, standby error to standby not normally allowed - + { "transition: standby error to standby, hil on", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // Safety switch arming tests - + { "transition: standby to armed, no safety switch", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to armed, safety switch off", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // standby error { "transition: armed error to standby error requested standby", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // TRANSITION_DENIED tests - + // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s - + { "no transition: init to armed", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby to armed error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to init", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to reboot", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to armed", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to reboot", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to armed", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to standby", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: reboot to armed", - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: in air restore to standby", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Sensor tests - + { "no transition: init to standby - sensors not initialized", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Safety switch arming tests - + { "no transition: init to standby, safety switch on", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, }; - + struct vehicle_status_s status; struct safety_s safety; struct actuator_armed_s armed; - + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); for (size_t i=0; i<cArmingTransitionTests; i++) { const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i]; - + // Setup initial machine state status.arming_state = test->current_state.arming_state; status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; @@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) safety.safety_off = test->safety_off; armed.armed = test->current_state.armed; armed.ready_to_arm = test->current_state.ready_to_arm; - + // Attempt transition transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); @@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) main_state_t to_state; // State to transition to transition_result_t expected_transition_result; // Expected result from main_state_transition call } MainTransitionTest_t; - + // Bits for condition_bits #define MTT_ALL_NOT_VALID 0 #define MTT_ROTARY_WING 1 << 0 @@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) #define MTT_LOC_POS_VALID 1 << 2 #define MTT_HOME_POS_VALID 1 << 3 #define MTT_GLOBAL_POS_VALID 1 << 4 - + static const MainTransitionTest_t rgMainTransitionTests[] = { - + // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + { "transition: MANUAL to ACRO", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, { "transition: ACRO to MANUAL", MTT_ALL_NOT_VALID, - MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, { "transition: AUTO_LOITER to MANUAL - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - not rotary", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", MTT_ROTARY_WING | MTT_LOC_ALT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: ALTCTL to MANUAL - local altitude valid", MTT_LOC_ALT_VALID, - MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position not valid, global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position valid, global position not valid", MTT_LOC_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: POSCTL to MANUAL - local position valid, global position valid", MTT_LOC_POS_VALID, - MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, // TRANSITION_DENIED tests { "no transition: MANUAL to AUTO_MISSION - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_LOITER - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", MTT_ROTARY_WING, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED }, }; - + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); for (size_t i=0; i<cMainTransitionTests; i++) { const MainTransitionTest_t* test = &rgMainTransitionTests[i]; @@ -432,10 +432,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; - + // Attempt transition transition_result_t result = main_state_transition(¤t_state, test->to_state); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); if (test->expected_transition_result == result) { @@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void) ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); - + return (_tests_failed == 0); } -ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
\ No newline at end of file +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8410297ef..e941e327c 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,6 +51,7 @@ #include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> #include <systemlib/err.h> +#include <systemlib/mcu_version.h> /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -80,6 +81,13 @@ int do_gyro_calibration(int mavlink_fd) int res = OK; + /* store board ID */ + uint32_t mcu_id[3]; + mcu_unique_id(&mcu_id[0]); + + /* store last 32bit number - not unique, but unique in a given set */ + param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); + /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); @@ -149,9 +157,9 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* set offset parameters to new values */ - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) { mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } @@ -275,13 +283,13 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* set scale parameters to new values */ - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) { mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } - if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) { + if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) { res = ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 2afb9a440..7147fb283 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -263,30 +263,30 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ - if (param_set(param_find("SENS_MAG_ID"), &(device_id))) { + if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) { res = ERROR; } - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) { res = ERROR; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 465f9cdc5..40da9c77b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -76,19 +76,19 @@ static const int ERROR = -1; // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. -static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { +static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char * const state_names[ARMING_STATE_MAX] = { +static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid - ASSERT(ARMING_STATE_INIT == 0); - ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); + ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; @@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s int prearm_ret = OK; /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } @@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s irqstate_t flags = irqsave(); /* enforce lockdown in HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; } else { @@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (valid_transition) { // We have a good transition. Now perform any secondary validation. - if (new_arming_state == ARMING_STATE_ARMED) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore - // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == HIL_STATE_OFF) { + // Allow if vehicle_status_s::HIL_STATE_ON + if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { @@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } - } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { - new_arming_state = ARMING_STATE_STANDBY_ERROR; + } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby - if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); feedback_provided = true; valid_transition = false; @@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Finish up the state transition if (valid_transition) { - armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } @@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { - case MAIN_STATE_MANUAL: - case MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || @@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { @@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_AUTO_MISSION: - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* need offboard signal */ if (!status->offboard_control_signal_lost) { @@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_MAX: + case vehicle_status_s::MAIN_STATE_MAX: default: break; } @@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } else { switch (new_state) { - case HIL_STATE_OFF: + case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; - case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { + case vehicle_status_s::HIL_STATE_ON: + if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { /* Disable publication of all attached sensors */ /* list directory */ @@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en { navigation_state_t nav_state_old = status->nav_state; - bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); + bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ switch (status->main_state) { - case MAIN_STATE_ACRO: - case MAIN_STATE_MANUAL: - case MAIN_STATE_ALTCTL: - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { switch (status->main_state) { - case MAIN_STATE_ACRO: - status->nav_state = NAVIGATION_STATE_ACRO; + case vehicle_status_s::MAIN_STATE_ACRO: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; break; - case MAIN_STATE_MANUAL: - status->nav_state = NAVIGATION_STATE_MANUAL; + case vehicle_status_s::MAIN_STATE_MANUAL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; - case MAIN_STATE_ALTCTL: - status->nav_state = NAVIGATION_STATE_ALTCTL; + case vehicle_status_s::MAIN_STATE_ALTCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; - case MAIN_STATE_POSCTL: - status->nav_state = NAVIGATION_STATE_POSCTL; + case vehicle_status_s::MAIN_STATE_POSCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; break; default: - status->nav_state = NAVIGATION_STATE_MANUAL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; } } break; - case MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: /* go into failsafe * - if commanded to do so @@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* first look at the commands */ if (status->engine_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->gps_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* datalink loss enabled: * check for datalink lost: this should always trigger RTGS */ @@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* datalink loss disabled: @@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* go into failsafe on a engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* go into failsafe if RC is lost and datalink loss is not set up */ @@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* don't bother if RC is lost if datalink is connected */ } else if (status->rc_signal_lost) { /* this mode is ok, we don't need RC for loitering */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } else { /* everything is perfect */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } break; - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* require global position and home, also go into failsafe on an engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { status->failsafe = true; - status->nav_state = NAVIGATION_STATE_POSCTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_OFFBOARD; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } default: break; 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/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index a8a70507e..491d4681d 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -47,6 +47,10 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/parameter_update.h> #include <stdio.h> 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 d0f5fb6f8..38660b433 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 @@ -84,6 +84,7 @@ #include <mathlib/mathlib.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> #include <mavlink/mavlink_log.h> +#include <platforms/px4_defines.h> #include "estimator_22states.h" @@ -816,7 +817,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ - bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); + bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -825,7 +826,7 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ - if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); @@ -1416,7 +1417,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 4cb6c1aef..00c2080a0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,7 +59,11 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_virtual_fw.h> +#include <uORB/topics/actuator_controls_virtual_mc.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/fw_virtual_rates_setpoint.h> +#include <uORB/topics/mc_virtual_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> @@ -76,6 +80,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 @@ -742,15 +747,15 @@ FixedwingAttitudeControl::task_main() _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); - _att.R[0][0] = R_adapted(0, 0); - _att.R[0][1] = R_adapted(0, 1); - _att.R[0][2] = R_adapted(0, 2); - _att.R[1][0] = R_adapted(1, 0); - _att.R[1][1] = R_adapted(1, 1); - _att.R[1][2] = R_adapted(1, 2); - _att.R[2][0] = R_adapted(2, 0); - _att.R[2][1] = R_adapted(2, 1); - _att.R[2][2] = R_adapted(2, 2); + PX4_R(_att.R, 0, 0) = R_adapted(0, 0); + PX4_R(_att.R, 0, 1) = R_adapted(0, 1); + PX4_R(_att.R, 0, 2) = R_adapted(0, 2); + PX4_R(_att.R, 1, 0) = R_adapted(1, 0); + PX4_R(_att.R, 1, 1) = R_adapted(1, 1); + PX4_R(_att.R, 1, 2) = R_adapted(1, 2); + PX4_R(_att.R, 2, 0) = R_adapted(2, 0); + PX4_R(_att.R, 2, 1) = R_adapted(2, 1); + PX4_R(_att.R, 2, 2) = R_adapted(2, 2); /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; @@ -924,9 +929,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 dbf15d98a..0bdc285e7 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 @@ -77,6 +77,7 @@ #include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_status.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/pid/pid.h> @@ -90,6 +91,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 */ @@ -735,7 +737,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); } } @@ -814,7 +816,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -961,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -972,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, @@ -985,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1138,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed); } - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ if (launchDetector.launchDetectionEnabled() && @@ -1233,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset landing state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1325,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ @@ -1339,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state == LAUNCHDETECTION_RES_NONE)) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } 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/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8eeeb5bd7..899a0d4a6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status.hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); } /* check for requested subscriptions */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4a095a765..be7d91c65 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = 0; /* HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED + || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set switch (status->nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */ - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -183,7 +183,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; break; - case NAVIGATION_STATE_MAX: + case vehicle_status_s::NAVIGATION_STATE_MAX: /* this is an unused case, ignore */ break; @@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = custom_mode.data; /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; } else { @@ -1431,7 +1431,7 @@ protected: updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); updated |= _status_sub->update(&_status_time, &status); - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -2198,7 +2198,7 @@ protected: msg.param2 = 0; msg.param3 = 0; /* set camera capture ON/OFF depending on arming state */ - msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0; msg.param5 = 0; msg.param6 = 0; msg.param7 = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 16d0422c7..06153fdac 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -616,7 +616,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ @@ -798,7 +798,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); @@ -985,10 +985,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; - if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = offset_ns; warnx("[timesync] Hard setting offset."); } else { diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c828f1f94..f20bba52e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -67,7 +67,11 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_virtual_fw.h> +#include <uORB/topics/actuator_controls_virtual_mc.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/fw_virtual_rates_setpoint.h> +#include <uORB/topics/mc_virtual_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_status.h> @@ -619,14 +623,14 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + memcpy(&_v_att_sp.R_body[0], &R_sp.data[0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp new file mode 100644 index 000000000..1e40c2b05 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * 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 mc_att_control.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + */ + +#include "mc_att_control.h" +#include "mc_att_control_params.h" +#include "math.h" + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +namespace mc_att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + MulticopterAttitudeControlBase(), + _task_should_exit(false), + _actuators_0_circuit_breaker_enabled(false), + + /* publications */ + _att_sp_pub(nullptr), + _v_rates_sp_pub(nullptr), + _actuators_0_pub(nullptr), + _n(), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + +{ + _params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P); + _params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P); + _params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I); + _params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D); + _params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P); + _params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P); + _params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I); + _params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D); + _params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P); + _params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P); + _params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I); + _params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D); + _params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF); + _params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX); + _params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX); + _params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX); + _params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX); + _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); + _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); + _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); + + /* fetch initial parameter values */ + parameters_update(); + + /* + * do subscriptions + */ + _v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0); + _v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0); + _v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0); + _parameter_update = _n.subscribe<px4_parameter_update>( + &MulticopterAttitudeControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0); + _armed = _n.subscribe<px4_actuator_armed>(0); + _v_status = _n.subscribe<px4_vehicle_status>(0); + +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + /* roll gains */ + PX4_PARAM_GET(_params_handles.roll_p, &v); + _params.att_p(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_p, &v); + _params.rate_p(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + PX4_PARAM_GET(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch gains */ + PX4_PARAM_GET(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_p, &v); + _params.rate_p(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + PX4_PARAM_GET(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw gains */ + PX4_PARAM_GET(_params_handles.yaw_p, &v); + _params.att_p(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + PX4_PARAM_GET(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff); + PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); + + /* manual control scale */ + PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max); + PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max); + PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max); + _params.man_roll_max = math::radians(_params.man_roll_max); + _params.man_pitch_max = math::radians(_params.man_pitch_max); + _params.man_yaw_max = math::radians(_params.man_yaw_max); + + /* acro control scale */ + PX4_PARAM_GET(_params_handles.acro_roll_max, &v); + _params.acro_rate_max(0) = math::radians(v); + PX4_PARAM_GET(_params_handles.acro_pitch_max, &v); + _params.acro_rate_max(1) = math::radians(v); + PX4_PARAM_GET(_params_handles.acro_yaw_max, &v); + _params.acro_rate_max(2) = math::radians(v); + + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + + return OK; +} + +void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg) +{ + parameters_update(); +} + +void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { + + perf_begin(_loop_perf); + + /* run controller on attitude changes */ + static uint64_t last_run = 0; + float dt = (px4::get_time_micros() - last_run) / 1000000.0f; + last_run = px4::get_time_micros(); + + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } + + if (_v_control_mode->data().flag_control_attitude_enabled) { + control_attitude(dt); + + /* publish the attitude setpoint if needed */ + if (_publish_att_sp && _v_status->data().is_rotary_wing) { + _v_att_sp_mod.data().timestamp = px4::get_time_micros(); + + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_v_att_sp_mod); + + } else { + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } + } + + /* publish attitude rates setpoint */ + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); + + if (_v_rates_sp_pub != nullptr) { + _v_rates_sp_pub->publish(_v_rates_sp_mod); + } else { + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>(); + } else { + _v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>(); + } + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode->data().flag_control_manual_enabled) { + /* manual rates control - ACRO mode */ + _rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x, + _manual_control_sp->data().r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp->data().z; + + /* reset yaw setpoint after ACRO */ + _reset_yaw_sp = true; + + /* publish attitude rates setpoint */ + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); + + if (_v_rates_sp_pub != nullptr) { + _v_rates_sp_pub->publish(_v_rates_sp_mod); + + } else { + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>(); + } else { + _v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>(); + } + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + _rates_sp(0) = _v_rates_sp->data().roll; + _rates_sp(1) = _v_rates_sp->data().pitch; + _rates_sp(2) = _v_rates_sp->data().yaw; + _thrust_sp = _v_rates_sp->data().thrust; + } + } + + if (_v_control_mode->data().flag_control_rates_enabled) { + control_attitude_rates(dt); + + /* publish actuator controls */ + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().timestamp = px4::get_time_micros(); + + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub != nullptr) { + _actuators_0_pub->publish(_actuators); + + } else { + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _actuators_0_pub = _n.advertise<px4_actuator_controls_virtual_mc>(); + } else { + _actuators_0_pub = _n.advertise<px4_actuator_controls_0>(); + } + } + } + } +} diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h new file mode 100644 index 000000000..95d608684 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * 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 mc_att_control.h + * Multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <systemlib/perf_counter.h> +#include <systemlib/circuit_breaker.h> +#include <lib/mathlib/mathlib.h> + +#include "mc_att_control_base.h" + +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + /* Callbacks for topics */ + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); + + void spin() { _n.spin(); } + +private: + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + + px4::Publisher<px4_vehicle_attitude_setpoint> * _att_sp_pub; /**< attitude setpoint publication */ + px4::Publisher<px4_vehicle_rates_setpoint> * _v_rates_sp_pub; /**< rate setpoint publication */ + px4::Publisher<px4_actuator_controls_0> * _actuators_0_pub; /**< attitude actuator controls publication */ + + px4::NodeHandle _n; + + struct { + px4_param_t roll_p; + px4_param_t roll_rate_p; + px4_param_t roll_rate_i; + px4_param_t roll_rate_d; + px4_param_t pitch_p; + px4_param_t pitch_rate_p; + px4_param_t pitch_rate_i; + px4_param_t pitch_rate_d; + px4_param_t yaw_p; + px4_param_t yaw_rate_p; + px4_param_t yaw_rate_i; + px4_param_t yaw_rate_d; + px4_param_t yaw_ff; + px4_param_t yaw_rate_max; + + px4_param_t man_roll_max; + px4_param_t man_pitch_max; + px4_param_t man_yaw_max; + px4_param_t acro_roll_max; + px4_param_t acro_pitch_max; + px4_param_t acro_yaw_max; + + px4_param_t autostart_id; + } _params_handles; /**< handles for interesting parameters */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /** + * Update our local parameter cache. + */ + int parameters_update(); +}; + diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp new file mode 100644 index 000000000..1f0d88bcd --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -0,0 +1,310 @@ +/* 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 mc_att_control_base.cpp + * + * MC Attitude Controller : Control and math code + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include "mc_att_control_base.h" +#include <geo/geo.h> +#include <math.h> +#include <lib/mathlib/mathlib.h> + +#ifdef CONFIG_ARCH_ARM +#else +#include <cmath> +using namespace std; +#endif + +MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _v_att_sp_mod(), + _v_rates_sp_mod(), + _actuators(), + _publish_att_sp(false) + +{ + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); +} + +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() +{ +} + +void MulticopterAttitudeControlBase::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + _publish_att_sp = false; + + + if (_v_control_mode->data().flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode->data().flag_control_velocity_enabled + || _v_control_mode->data().flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); + } + + if (!_v_control_mode->data().flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp_mod.data().thrust = _manual_control_sp->data().z; + _publish_att_sp = true; + } + + if (!_armed->data().armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp_mod.data().thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _v_att_sp_mod.data().yaw_body = _wrap_pi( + _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt); + float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); + float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw); + + if (yaw_offs < -yaw_offs_max) { + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max); + } + + _v_att_sp_mod.data().R_valid = false; + // _publish_att_sp = true; + } + + /* reset yaw setpoint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; + _v_att_sp_mod.data().R_valid = false; + // _publish_att_sp = true; + } + + if (!_v_control_mode->data().flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x + * _params.man_pitch_max; + _v_att_sp_mod.data().R_valid = false; + // _publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp_mod.data().thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp_mod.data().R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp_mod.data().R_body[0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body, + _v_att_sp_mod.data().yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0], + sizeof(_v_att_sp_mod.data().R_body)); + _v_att_sp_mod.data().R_valid = true; + } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att->data().R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(R.transposed() * R_sp); + math::Vector < 3 > e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, + _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed->data().armed || !_v_status->data().is_rotary_wing) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector < 3 > rates; + rates(0) = _v_att->data().rollspeed; + rates(1) = _v_att->data().pitchspeed; + rates(2) = _v_att->data().yawspeed; + + /* angular rates error */ + math::Vector < 3 > rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > MIN_TAKEOFF_THRUST) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite( + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } + +} + +void MulticopterAttitudeControlBase::set_actuator_controls() +{ + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; +} diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h new file mode 100644 index 000000000..124147079 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -0,0 +1,136 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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 mc_att_control_base.h + * + * MC Attitude Controller : Control and math code + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + */ +#include <px4.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <errno.h> +#include <math.h> + +#include <systemlib/perf_counter.h> +#include <lib/mathlib/mathlib.h> + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +using namespace px4; + +class MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlBase(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlBase(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + + void set_actuator_controls(); + +protected: + px4::Subscriber<px4_vehicle_attitude> *_v_att; /**< vehicle attitude */ + px4::Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */ + px4::Subscriber<px4_vehicle_rates_setpoint> *_v_rates_sp; /**< vehicle rates setpoint */ + px4::Subscriber<px4_vehicle_control_mode> *_v_control_mode; /**< vehicle control mode */ + px4::Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */ + px4::Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */ + px4::Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */ + px4::Subscriber<px4_vehicle_status> *_v_status; /**< vehicle status */ + + px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint + that gets published eventually */ + px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint + that gets published eventually*/ + px4_actuator_controls_0 _actuators; /**< actuator controls */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> _I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + int32_t autostart_id; + } _params; + + bool _publish_att_sp; + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp new file mode 100644 index 000000000..5a79a8c6b --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 mc_att_control_main.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include "mc_att_control.h" + +bool thread_running = false; /**< Deamon status flag */ + +int main(int argc, char **argv) +{ + px4::init(argc, argv, "mc_att_control_m"); + + PX4_INFO("starting"); + MulticopterAttitudeControl attctl; + thread_running = true; + attctl.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} + + diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c new file mode 100644 index 000000000..a0b1706f2 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * 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 mc_att_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <px4_defines.h> +#include "mc_att_control_params.h" +#include <systemlib/param/param.h> + +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P); + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAW_P); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D); + +/** + * Yaw feed forward + * + * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); + +/** + * Max yaw rate + * + * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); + +/** + * Max manual roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); + +/** + * Max manual pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); + +/** + * Max acro roll rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX); + +/** + * Max acro pitch rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX); + +/** + * Max acro yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h new file mode 100644 index 000000000..59dd5240f --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h @@ -0,0 +1,65 @@ + +/**************************************************************************** + * + * 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 mc_att_control_params.h + * Parameters for multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#pragma once + +#define PARAM_MC_ROLL_P_DEFAULT 6.0f +#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f +#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f +#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f +#define PARAM_MC_PITCH_P_DEFAULT 6.0f +#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f +#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f +#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f +#define PARAM_MC_YAW_P_DEFAULT 2.0f +#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f +#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f +#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f +#define PARAM_MC_YAW_FF_DEFAULT 0.5f +#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f +#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f +#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f +#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp new file mode 100644 index 000000000..d516d7e52 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp @@ -0,0 +1,142 @@ +/* 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 mc_att_control_sim.cpp + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include "mc_att_control_sim.h" +#include <geo/geo.h> +#include <math.h> + +#ifdef CONFIG_ARCH_ARM +#else +#include <cmath> +using namespace std; +#endif + +MulticopterAttitudeControlSim::MulticopterAttitudeControlSim() +{ + /* setup standard gains */ + //XXX: make these configurable + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; +} + +MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim() +{ +} + +void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) +{ + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3, 3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0, 0); + _v_att.R[1][0] = Rot(1, 0); + _v_att.R[2][0] = Rot(2, 0); + _v_att.R[0][1] = Rot(0, 1); + _v_att.R[1][1] = Rot(1, 1); + _v_att.R[2][1] = Rot(2, 1); + _v_att.R[0][2] = Rot(0, 2); + _v_att.R[1][2] = Rot(1, 2); + _v_att.R[2][2] = Rot(2, 2); + + _v_att.R_valid = true; +} + +void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate) +{ + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); +} + +void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference) +{ + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30; + + // setup rotation matrix + math::Matrix<3, 3> Rot_sp; + Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0, 0); + _v_att_sp.R_body[1][0] = Rot_sp(1, 0); + _v_att_sp.R_body[2][0] = Rot_sp(2, 0); + _v_att_sp.R_body[0][1] = Rot_sp(0, 1); + _v_att_sp.R_body[1][1] = Rot_sp(1, 1); + _v_att_sp.R_body[2][1] = Rot_sp(2, 1); + _v_att_sp.R_body[0][2] = Rot_sp(0, 2); + _v_att_sp.R_body[1][2] = Rot_sp(1, 2); + _v_att_sp.R_body[2][2] = Rot_sp(2, 2); +} + +void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs) +{ + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h new file mode 100644 index 000000000..a1bf44fc9 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h @@ -0,0 +1,97 @@ +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* 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 mc_att_control_sim.h + * + * MC Attitude Controller Interface for usage in a simulator + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <errno.h> +#include <math.h> +#include <drivers/drv_hrt.h> + +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_armed.h> +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> +#include <lib/mathlib/mathlib.h> +#inlcude "mc_att_control_base.h" + + + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlSim : + public MulticopterAttitudeControlBase + +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControlSim(); + + /** + * Destructor + */ + ~MulticopterAttitudeControlSim(); + + /* setters and getters for interface with euroc-gazebo simulator */ + void set_attitude(const Eigen::Quaternion<double> attitude); + void set_attitude_rates(const Eigen::Vector3d &angular_rate); + void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d &motor_inputs); + +protected: + void vehicle_attitude_setpoint_poll() {}; + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index 668f8f164..c2b847075 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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,51 +32,68 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file mc_att_control_m_start_nuttx.cpp * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller + * @author Thomas Gubler <thomasgubler@gmail.com> */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> -#ifndef TOPIC_ACTUATOR_CONTROLS_H -#define TOPIC_ACTUATOR_CONTROLS_H +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; -#include <stdint.h> -#include "../uORB.h" +extern int main(int argc, char **argv); -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); +int mc_att_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_att_control_m {start|stop|status}"); + } -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + if (!strcmp(argv[1], "start")) { -/** - * @addtogroup topics - * @{ - */ + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } -struct actuator_controls_s { - uint64_t timestamp; - uint64_t timestamp_sample; /**< the timestamp the data this control response is based on was sampled */ - float control[NUM_ACTUATOR_CONTROLS]; -}; + task_should_exit = false; -/** - * @} - */ + daemon_task = task_spawn_cmd("mc_att_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_0); -ORB_DECLARE(actuator_controls_1); -ORB_DECLARE(actuator_controls_2); -ORB_DECLARE(actuator_controls_3); -ORB_DECLARE(actuator_controls_virtual_mc); -ORB_DECLARE(actuator_controls_virtual_fw); + } else { + warnx("not started"); + } + exit(0); + } -#endif + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk new file mode 100644 index 000000000..959f6492b --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = mc_att_control_m + +SRCS = mc_att_control_main.cpp \ + mc_att_control_start_nuttx.cpp \ + mc_att_control.cpp \ + mc_att_control_base.cpp \ + mc_att_control_params.c 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 cad6cf3ae..b9692ffbf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -49,8 +49,9 @@ * @author Anton Babushkin <anton.babushkin@me.com> */ -#include <nuttx/config.h> -#include <stdio.h> +#include <px4.h> +#include <functional> +#include <cstdio> #include <stdlib.h> #include <string.h> #include <unistd.h> @@ -60,8 +61,7 @@ #include <poll.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> + #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> @@ -73,12 +73,13 @@ #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> +// #include <systemlib/param/param.h> +// #include <systemlib/err.h> #include <systemlib/systemlib.h> #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 @@ -537,9 +538,9 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } @@ -767,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ math::Vector<3> prev_sp; map_projection_project(&_ref_pos, @@ -997,10 +998,10 @@ MulticopterPositionControl::task_main() } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; @@ -1036,7 +1037,7 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -1123,7 +1124,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; @@ -1161,11 +1162,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 { @@ -1273,7 +1274,7 @@ MulticopterPositionControl::task_main() } /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -1288,7 +1289,7 @@ MulticopterPositionControl::task_main() R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp new file mode 100644 index 000000000..a46163e6f --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -0,0 +1,949 @@ +/**************************************************************************** + * + * 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 mc_pos_control.cpp + * Multicopter position controller. + * + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "mc_pos_control.h" +#include "mc_pos_control_params.h" + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f +#define MIN_DIST 0.01f + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + _mavlink_fd(-1), + +/* publications */ + _att_sp_pub(nullptr), + _local_pos_sp_pub(nullptr), + _global_vel_sp_pub(nullptr), + + _ref_alt(0.0f), + _ref_timestamp(0), + + _reset_pos_sp(true), + _reset_alt_sp(true), + _mode_auto(false), + _thrust_int(), + _R() +{ + memset(&_ref_pos, 0, sizeof(_ref_pos)); + + /* + * Do subscriptions + */ + _att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0); + _control_mode = _n.subscribe<px4_vehicle_control_mode>(0); + _parameter_update = _n.subscribe<px4_parameter_update>( + &MulticopterPositionControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0); + _armed = _n.subscribe<px4_actuator_armed>(0); + _local_pos = _n.subscribe<px4_vehicle_local_position>(0); + _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0); + _local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0); + _global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0); + + + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _pos_sp.zero(); + _vel.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); + + _params_handles.thr_min = PX4_PARAM_INIT(MPC_THR_MIN); + _params_handles.thr_max = PX4_PARAM_INIT(MPC_THR_MAX); + _params_handles.z_p = PX4_PARAM_INIT(MPC_Z_P); + _params_handles.z_vel_p = PX4_PARAM_INIT(MPC_Z_VEL_P); + _params_handles.z_vel_i = PX4_PARAM_INIT(MPC_Z_VEL_I); + _params_handles.z_vel_d = PX4_PARAM_INIT(MPC_Z_VEL_D); + _params_handles.z_vel_max = PX4_PARAM_INIT(MPC_Z_VEL_MAX); + _params_handles.z_ff = PX4_PARAM_INIT(MPC_Z_FF); + _params_handles.xy_p = PX4_PARAM_INIT(MPC_XY_P); + _params_handles.xy_vel_p = PX4_PARAM_INIT(MPC_XY_VEL_P); + _params_handles.xy_vel_i = PX4_PARAM_INIT(MPC_XY_VEL_I); + _params_handles.xy_vel_d = PX4_PARAM_INIT(MPC_XY_VEL_D); + _params_handles.xy_vel_max = PX4_PARAM_INIT(MPC_XY_VEL_MAX); + _params_handles.xy_ff = PX4_PARAM_INIT(MPC_XY_FF); + _params_handles.tilt_max_air = PX4_PARAM_INIT(MPC_TILTMAX_AIR); + _params_handles.land_speed = PX4_PARAM_INIT(MPC_LAND_SPEED); + _params_handles.tilt_max_land = PX4_PARAM_INIT(MPC_TILTMAX_LND); + + /* fetch initial parameter values */ + parameters_update(); + + _R.identity(); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ +} + +int +MulticopterPositionControl::parameters_update() +{ + PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min); + PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max); + PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air); + _params.tilt_max_air = math::radians(_params.tilt_max_air); + PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed); + PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land); + _params.tilt_max_land = math::radians(_params.tilt_max_land); + + float v; + PX4_PARAM_GET(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + PX4_PARAM_GET(_params_handles.z_p, &v); + _params.pos_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + PX4_PARAM_GET(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + PX4_PARAM_GET(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + + return OK; +} + + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::update_ref() +{ + if (_local_pos->data().ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp = 0.0f; + + if (_ref_timestamp != 0) { + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon); + _ref_alt = _local_pos->data().ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); + } + + _ref_timestamp = _local_pos->data().ref_timestamp; + } +} + +void +MulticopterPositionControl::reset_pos_sp() +{ + if (_reset_pos_sp) { + _reset_pos_sp = false; + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) + // - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) + // - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1)); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2)); + } +} + +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual_control_sp->data().x; + _sp_move_rate(1) = _manual_control_sp->data().y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + if (_pos_sp_triplet->data().current.valid) { + if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet->data().current.x; + _pos_sp(1) = _pos_sp_triplet->data().current.y; + } else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet->data().current.vx; + _sp_move_rate(1) = _pos_sp_triplet->data().current.vy; + } + + if (_pos_sp_triplet->data().current.yaw_valid) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } else if (_pos_sp_triplet->data().current.yawspeed_valid) { + _att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt; + } + + if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet->data().current.z; + } else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet->data().current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto(float dt) +{ + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + + if (_pos_sp_triplet->data().current.valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* project setpoint to local frame */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt); + + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + + /* by default use current setpoint as is */ + math::Vector<3> pos_sp_s = curr_sp_s; + + if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet->data().next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); + + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; + + /* cos(b), b = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a_curr_next /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + pos_sp_s += pos_ff; + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; + } + + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); + } + } + } + } + + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + + /* difference between current and desired position setpoints, 1 = max speed */ + math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); + float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { + pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet->data().current.yaw)) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } + + } else { + /* no waypoint, do nothing, setpoint was already reset */ + } +} + +void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg) +{ + parameters_update(); +} + +void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) +{ + static bool reset_int_z = true; + static bool reset_int_z_manual = false; + static bool reset_int_xy = true; + static bool was_armed = false; + static uint64_t t_prev = 0; + + uint64_t t = get_time_micros(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f; + t_prev = t; + + if (_control_mode->data().flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + _reset_pos_sp = true; + _reset_alt_sp = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = _control_mode->data().flag_armed; + + update_ref(); + + if (_control_mode->data().flag_control_altitude_enabled || + _control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_climb_rate_enabled || + _control_mode->data().flag_control_velocity_enabled) { + + _pos(0) = _local_pos->data().x; + _pos(1) = _local_pos->data().y; + _pos(2) = _local_pos->data().z; + + _vel(0) = _local_pos->data().vx; + _vel(1) = _local_pos->data().vy; + _vel(2) = _local_pos->data().vz; + + _vel_ff.zero(); + _sp_move_rate.zero(); + + /* select control source */ + if (_control_mode->data().flag_control_manual_enabled) { + /* manual control */ + control_manual(dt); + _mode_auto = false; + + } else if (_control_mode->data().flag_control_offboard_enabled) { + /* offboard control */ + control_offboard(dt); + _mode_auto = false; + + } else { + /* AUTO */ + control_auto(dt); + } + + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>(); + } + + + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + _R.identity(); + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + _att_sp_msg.data().yaw_body = _att->data().yaw; + _att_sp_msg.data().thrust = 0.0f; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } + + } else { + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + + if (!_control_mode->data().flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode->data().flag_control_position_enabled) { + _reset_pos_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + /* use constant descend rate when landing, ignore altitude setpoint */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + _global_vel_sp_msg.data().vx = _vel_sp(0); + _global_vel_sp_msg.data().vy = _vel_sp(1); + _global_vel_sp_msg.data().vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub != nullptr) { + _global_vel_sp_pub->publish(_global_vel_sp_msg); + + } else { + _global_vel_sp_pub = _n.advertise<px4_vehicle_global_velocity_setpoint>(); + } + + if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode->data().flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual_control_sp->data().z; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + _thrust_int(2) = -i; + } + + } else { + reset_int_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + _thrust_int(0) = 0.0f; + _thrust_int(1) = 0.0f; + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int; + + if (!_control_mode->data().flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode->data().flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode->data().flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + float tilt_max = _params.tilt_max_air; + + /* adjust limits for landing mode */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && + _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.tilt_max_land; + + if (thr_min < 0.0f) { + thr_min = 0.0f; + } + } + + /* limit min lift */ + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } + } + + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att->data().R, 2, 2); + + } else if (PX4_R(_att->data().R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f; + saturation_z = true; + + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) { + _thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + _thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) { + _thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (_thrust_int(2) > 0.0f) { + _thrust_int(2) = 0.0f; + } + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode->data().flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp_msg.data().yaw_body), cosf(_att_sp_msg.data().yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + _R(i, 0) = body_x(i); + _R(i, 1) = body_y(i); + _R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = _R.to_euler(); + _att_sp_msg.data().roll_body = euler(0); + _att_sp_msg.data().pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else if (!_control_mode->data().flag_control_manual_enabled) { + /* autonomous altitude control without position control (failsafe landing), + * force level attitude, don't change yaw */ + _R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + } + + _att_sp_msg.data().thrust = thrust_abs; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } + + } else { + reset_int_z = true; + } + } + + } else { + /* position controller disabled, reset setpoints */ + _reset_alt_sp = true; + _reset_pos_sp = true; + _mode_auto = false; + reset_int_z = true; + reset_int_xy = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h new file mode 100644 index 000000000..05bd1387b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control.h + * Multicopter position controller. + * + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#pragma once + +#include <px4.h> +#include <cstdio> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +// #include <poll.h> +// #include <drivers/drv_hrt.h> +// #include <arch/board/board.h> +// #include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> +#include <lib/geo/geo.h> +// #include <mavlink/mavlink_log.h> + +using namespace px4; + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /* Callbacks for topics */ + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); + + void spin() { _n.spin(); } + +protected: + const float alt_ctl_dz = 0.2f; + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ + + Publisher<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */ + Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ + Publisher<px4_vehicle_global_velocity_setpoint> *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + + + Subscriber<px4_vehicle_attitude> *_att; /**< vehicle attitude */ + Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */ + Subscriber<px4_vehicle_control_mode> *_control_mode; /**< vehicle control mode */ + Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */ + Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */ + Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */ + Subscriber<px4_vehicle_local_position> *_local_pos; /**< local position */ + Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet; /**< local position */ + Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp; /**< local position */ + Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp; /**< local position */ + + px4_vehicle_attitude_setpoint _att_sp_msg; + px4_vehicle_local_position_setpoint _local_pos_sp_msg; + px4_vehicle_global_velocity_setpoint _global_vel_sp_msg; + + px4::NodeHandle _n; + + + struct { + px4_param_t thr_min; + px4_param_t thr_max; + px4_param_t z_p; + px4_param_t z_vel_p; + px4_param_t z_vel_i; + px4_param_t z_vel_d; + px4_param_t z_vel_max; + px4_param_t z_ff; + px4_param_t xy_p; + px4_param_t xy_vel_p; + px4_param_t xy_vel_i; + px4_param_t xy_vel_d; + px4_param_t xy_vel_max; + px4_param_t xy_ff; + px4_param_t tilt_max_air; + px4_param_t land_speed; + px4_param_t tilt_max_land; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max_air; + float land_speed; + float tilt_max_land; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + struct map_projection_reference_s _ref_pos; + float _ref_alt; + uint64_t _ref_timestamp; + + bool _reset_pos_sp; + bool _reset_alt_sp; + bool _mode_auto; + + math::Vector<3> _pos; + math::Vector<3> _pos_sp; + math::Vector<3> _vel; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; + + math::Vector<3> _thrust_int; + math::Matrix<3, 3> _R; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Update reference for local position projection + */ + void update_ref(); + /** + * Reset position setpoint to current position + */ + void reset_pos_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(float dt); + + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); +}; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp new file mode 100644 index 000000000..0b5775736 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_main.cpp + * Multicopter position controller. + * + * The controller has two loops: P loop for position error and PID loop for velocity error. + * Output of velocity controller is thrust vector that splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "mc_pos_control.h" + +bool thread_running = false; /**< Deamon status flag */ + +int main(int argc, char **argv) +{ + px4::init(argc, argv, "mc_pos_control_m"); + + PX4_INFO("starting"); + MulticopterPositionControl posctl; + thread_running = true; + posctl.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c new file mode 100644 index 000000000..c741a7f0a --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin <anton.babushkin@me.com> + * + * 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) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_params.c + * Multicopter position controller parameters. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <px4_defines.h> +#include "mc_pos_control_params.h" +#include <systemlib/param/param.h> + +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); + +/** + * Maximum tilt angle in air + * + * Limits maximum tilt in AUTO and POSCTRL modes during flight. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); + diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h index 6766bb58a..fec3bcb7c 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 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> + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,33 +33,29 @@ ****************************************************************************/ /** - * @file vehicle_local_position_setpoint.h - * Definition of the local NED position setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_local_position_setpoint_s { - uint64_t timestamp; /**< timestamp of the setpoint */ - float x; /**< in meters NED */ - float y; /**< in meters NED */ - float z; /**< in meters NED */ - float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame */ - -/** - * @} + * @file mc_pos_control_params.h + * Multicopter position controller parameters. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position_setpoint); +#pragma once + +#define PARAM_MPC_THR_MIN_DEFAULT 0.1f +#define PARAM_MPC_THR_MAX_DEFAULT 1.0f +#define PARAM_MPC_Z_P_DEFAULT 1.0f +#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f +#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_Z_FF_DEFAULT 0.5f +#define PARAM_MPC_XY_P_DEFAULT 1.0f +#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f +#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_XY_FF_DEFAULT 0.5f +#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f +#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f +#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f -#endif diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp new file mode 100644 index 000000000..87996d93b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_m_start_nuttx.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]); +int mc_pos_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_pos_control_m {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_pos_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk new file mode 100644 index 000000000..2852ebbec --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2013 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. +# +############################################################################ + +# +# Build multicopter position controller +# + +MODULE_COMMAND = mc_pos_control_m + +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_start_nuttx.cpp \ + mc_pos_control.cpp \ + mc_pos_control_params.c diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f827e70c9..a744d58cf 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -82,7 +82,7 @@ Loiter::on_activation() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b52b12ce7..a94082d6f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -397,7 +397,7 @@ Mission::set_mission_items() pos_sp_triplet->next.valid = false; /* reuse setpoint for LOITER only if it's not IDLE */ - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); set_mission_finished(); @@ -476,7 +476,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _need_takeoff = true; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index e39fb3216..52ccebac9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -194,25 +194,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite switch (item->nav_cmd) { case NAV_CMD_IDLE: - sp->type = SETPOINT_TYPE_IDLE; + sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; case NAV_CMD_TAKEOFF: - sp->type = SETPOINT_TYPE_TAKEOFF; + sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; break; case NAV_CMD_LAND: - sp->type = SETPOINT_TYPE_LAND; + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_TURN_COUNT: case NAV_CMD_LOITER_UNLIMITED: - sp->type = SETPOINT_TYPE_LOITER; + sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; break; default: - sp->type = SETPOINT_TYPE_POSITION; + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; break; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3f7670ec4..ad2349c94 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -217,7 +217,7 @@ Navigator::vehicle_status_update() { if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { /* in case the commander is not be running */ - _vstatus.arming_state = ARMING_STATE_STANDBY; + _vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } @@ -247,9 +247,6 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - /* inform about start */ - warnx("Initializing.."); - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); @@ -263,7 +260,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { - mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); + mavlink_log_info(_mavlink_fd, "No geofence set"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else @@ -419,25 +416,25 @@ Navigator::task_main() /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { - case NAVIGATION_STATE_MANUAL: - case NAVIGATION_STATE_ACRO: - case NAVIGATION_STATE_ALTCTL: - case NAVIGATION_STATE_POSCTL: - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; @@ -445,11 +442,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; @@ -459,11 +456,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; 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 86764d620..ec297e7f0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -53,6 +53,10 @@ #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> @@ -68,6 +72,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" @@ -458,7 +463,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]; } } @@ -491,7 +496,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; @@ -528,15 +533,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 */ @@ -559,7 +564,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]; } } @@ -568,7 +573,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 @@ -940,7 +945,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)) { @@ -965,7 +970,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/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 99632ef0b..8ac87b238 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -70,6 +70,10 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position.h> @@ -1117,7 +1121,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - + /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 67b7feef7..5e04241fe 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,20 +36,192 @@ * * Parameters defined by the sensors task. * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Lorenz Meier <lorenz@px4.io> + * @author Julian Oes <julian@px4.io> + * @author Thomas Gubler <thomas@px4.io> */ #include <nuttx/config.h> #include <systemlib/param/param.h> /** + * ID of the board this parameter set was calibrated on. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_BOARD_ID, 0); + +/** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0); + +/** + * Gyro X-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @min -5.0 + * @max 5.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f); + +/** + * Gyro X-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_XSCALE, 1.0f); + +/** + * Gyro Y-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_YSCALE, 1.0f); + +/** + * Gyro Z-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_ZSCALE, 1.0f); + +/** + * ID of Magnetometer the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); + +/** + * Magnetometer X-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YOFF, 0.0f); + +/** + * Magnetometer Z-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f); + +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC0_ID, 0); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0f); + +/** * ID of the Gyro that the calibration is for. * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); +PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0); /** * Gyro X-axis offset @@ -58,7 +230,7 @@ PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); * @max 10.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0f); /** * Gyro Y-axis offset @@ -67,7 +239,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); * @max 10.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0f); /** * Gyro Z-axis offset @@ -76,7 +248,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); * @max 5.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f); /** * Gyro X-axis scaling factor @@ -85,7 +257,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); * @max 1.5 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_XSCALE, 1.0f); /** * Gyro Y-axis scaling factor @@ -94,7 +266,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); * @max 1.5 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_YSCALE, 1.0f); /** * Gyro Z-axis scaling factor @@ -103,14 +275,14 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); * @max 1.5 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_GYRO1_ZSCALE, 1.0f); /** * ID of Magnetometer the calibration is for. * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_MAG_ID, 0); +PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); /** * Magnetometer X-axis offset @@ -119,7 +291,7 @@ PARAM_DEFINE_INT32(SENS_MAG_ID, 0); * @max 500.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0f); /** * Magnetometer Y-axis offset @@ -128,7 +300,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); * @max 500.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_YOFF, 0.0f); /** * Magnetometer Z-axis offset @@ -137,78 +309,242 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); * @max 500.0 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_ZOFF, 0.0f); /** * Magnetometer X-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_XSCALE, 1.0f); /** * Magnetometer Y-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f); /** * Magnetometer Z-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f); /** * ID of the Accelerometer that the calibration is for. * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_ACC_ID, 0); +PARAM_DEFINE_INT32(CAL_ACC1_ID, 0); /** * Accelerometer X-axis offset * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_XOFF, 0.0f); /** * Accelerometer Y-axis offset * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_YOFF, 0.0f); /** * Accelerometer Z-axis offset * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_ZOFF, 0.0f); /** * Accelerometer X-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_XSCALE, 1.0f); /** * Accelerometer Y-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0f); /** * Accelerometer Z-axis scaling factor * * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0f); + +/** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0); + +/** + * Gyro X-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @min -10.0 + * @max 10.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @min -5.0 + * @max 5.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f); + +/** + * Gyro X-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_XSCALE, 1.0f); + +/** + * Gyro Y-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_YSCALE, 1.0f); + +/** + * Gyro Z-axis scaling factor + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_ZSCALE, 1.0f); + +/** + * ID of Magnetometer the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); + +/** + * Magnetometer X-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YOFF, 0.0f); +/** + * Magnetometer Z-axis offset + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f); + +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC2_ID, 0); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f); /** * Differential pressure sensor offset diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 630c54335..87d7da537 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -184,13 +184,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); /** * Update paramters from RC channels if the functionality is activated and the @@ -512,7 +512,7 @@ Sensors::Sensors() : _hil_enabled(false), _publishing(true), -/* subscriptions */ + /* subscriptions */ _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), @@ -530,7 +530,7 @@ Sensors::Sensors() : _rc_parameter_map_sub(-1), _manual_control_sub(-1), -/* publications */ + /* publications */ _sensor_pub(-1), _manual_control_pub(-1), _actuator_group_3_pub(-1), @@ -539,7 +539,7 @@ Sensors::Sensors() : _airspeed_pub(-1), _diff_pres_pub(-1), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _mag_is_external(false), @@ -619,29 +619,29 @@ Sensors::Sensors() : _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); /* gyro offsets */ - _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); - _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); - _parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF"); - _parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE"); - _parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE"); - _parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE"); + _parameter_handles.gyro_offset[0] = param_find("CAL_GYRO0_XOFF"); + _parameter_handles.gyro_offset[1] = param_find("CAL_GYRO0_YOFF"); + _parameter_handles.gyro_offset[2] = param_find("CAL_GYRO0_ZOFF"); + _parameter_handles.gyro_scale[0] = param_find("CAL_GYRO0_XSCALE"); + _parameter_handles.gyro_scale[1] = param_find("CAL_GYRO0_YSCALE"); + _parameter_handles.gyro_scale[2] = param_find("CAL_GYRO0_ZSCALE"); /* accel offsets */ - _parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF"); - _parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF"); - _parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF"); - _parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE"); - _parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE"); - _parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE"); + _parameter_handles.accel_offset[0] = param_find("CAL_ACC0_XOFF"); + _parameter_handles.accel_offset[1] = param_find("CAL_ACC0_YOFF"); + _parameter_handles.accel_offset[2] = param_find("CAL_ACC0_ZOFF"); + _parameter_handles.accel_scale[0] = param_find("CAL_ACC0_XSCALE"); + _parameter_handles.accel_scale[1] = param_find("CAL_ACC0_YSCALE"); + _parameter_handles.accel_scale[2] = param_find("CAL_ACC0_ZSCALE"); /* mag offsets */ - _parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF"); - _parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF"); - _parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF"); + _parameter_handles.mag_offset[0] = param_find("CAL_MAG0_XOFF"); + _parameter_handles.mag_offset[1] = param_find("CAL_MAG0_YOFF"); + _parameter_handles.mag_offset[2] = param_find("CAL_MAG0_ZOFF"); - _parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE"); - _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); - _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + _parameter_handles.mag_scale[0] = param_find("CAL_MAG0_XSCALE"); + _parameter_handles.mag_scale[1] = param_find("CAL_MAG0_YSCALE"); + _parameter_handles.mag_scale[2] = param_find("CAL_MAG0_ZSCALE"); /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); @@ -786,9 +786,11 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); } + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); @@ -813,28 +815,28 @@ 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_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - _rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } /* gyro offsets */ @@ -883,16 +885,19 @@ Sensors::parameters_update() /* set px4flow rotation */ int flowfd; flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { - int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); - if (flowret) { - warnx("flow rotation could not be set"); + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + + if (flowret) { + warnx("flow rotation could not be set"); close(flowfd); return ERROR; - } + } + close(flowfd); - } - + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -902,9 +907,9 @@ Sensors::parameters_update() /** fine tune board offset on parameter update **/ math::Matrix<3, 3> board_rotation_offset; - board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], - M_DEG_TO_RAD_F * _parameters.board_offset[1], - M_DEG_TO_RAD_F * _parameters.board_offset[2]); + board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); _board_rotation = _board_rotation * board_rotation_offset; @@ -912,17 +917,20 @@ Sensors::parameters_update() param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); int barofd; barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH); return ERROR; } else { int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); close(barofd); return ERROR; } + close(barofd); } @@ -942,28 +950,11 @@ Sensors::accel_init() } else { - // XXX do the check more elegantly + /* set the accel internal sampling rate to default rate */ + ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - /* set the accel internal sampling rate up to at leat 1000Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, 1000); - - /* set the driver to poll at 1000Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 1000); - -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE - - /* set the accel internal sampling rate up to at leat 800Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, 800); - - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); - -#else -#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE - -#endif + /* set the driver to poll at default rate */ + ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); close(fd); } @@ -984,31 +975,12 @@ Sensors::gyro_init() } else { - // XXX do the check more elegantly - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - /* set the gyro internal sampling rate up to at least 1000Hz */ - if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) { - ioctl(fd, GYROIOCSSAMPLERATE, 800); - } - - /* set the driver to poll at 1000Hz */ - if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) { - ioctl(fd, SENSORIOCSPOLLRATE, 800); - } - -#else - - /* set the gyro internal sampling rate up to at least 760Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 760); - - /* set the driver to poll at 760Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 760); + /* set the gyro internal sampling rate to default rate */ + ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); -#endif + /* set the driver to poll at default rate */ + ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); } return OK; @@ -1348,14 +1320,17 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; /* don't risk to feed negative airspeed into the system */ - _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); - _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.indicated_airspeed_m_s = math::max(0.0f, + calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1479,8 +1454,10 @@ Sensors::parameter_update_poll(bool forced) } #if 0 - printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]); - printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]); + printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], + (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]); + printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], + (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]); printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100)); fflush(stdout); usleep(5000); @@ -1496,9 +1473,10 @@ Sensors::rc_parameter_map_poll(bool forced) if (map_updated) { orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + /* update paramter handles to which the RC channels are mapped */ for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ @@ -1508,21 +1486,24 @@ Sensors::rc_parameter_map_poll(bool forced) /* Set the handle by index if the index is set, otherwise use the id */ if (_rc_parameter_map.param_index[i] >= 0) { _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + } else { _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); } } + warnx("rc to parameter map updated"); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", - i, - _rc_parameter_map.param_id[i], - (double)_rc_parameter_map.scale[i], - (double)_rc_parameter_map.value0[i], - (double)_rc_parameter_map.value_min[i], - (double)_rc_parameter_map.value_max[i] - ); + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); } } } @@ -1616,7 +1597,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + + (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ @@ -1646,7 +1628,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]]; @@ -1667,41 +1649,41 @@ 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; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else if (mid_inv ? value < mid_th : value > mid_th) { - return SWITCH_POS_MIDDLE; + return manual_control_setpoint_s::SWITCH_POS_MIDDLE; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } 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; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } @@ -1709,22 +1691,23 @@ void Sensors::set_params_from_rc() { static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ continue; } - float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0); + float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { param_rc_values[i] = rc_val; float param_val = math::constrain( - _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, - _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); param_set(_parameter_handles.rc_param[i], ¶m_val); } } @@ -1808,10 +1791,12 @@ Sensors::rc_poll() * DO NOT REMOVE OR ALTER STEP 1! */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)( + _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)( + _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ @@ -1847,24 +1832,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_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value (rc_channels_s::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_s::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_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { @@ -1899,6 +1884,7 @@ Sensors::rc_poll() /* Update parameters from RC Channels (tuning with RC) if activated */ static hrt_abstime last_rc_to_param_map_time = 0; + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { set_params_from_rc(); last_rc_to_param_map_time = hrt_absolute_time(); @@ -1920,22 +1906,31 @@ Sensors::task_main() /* start individual sensors */ int ret; ret = accel_init(); + if (ret) { goto exit_immediate; } + ret = gyro_init(); + if (ret) { goto exit_immediate; } + ret = mag_init(); + if (ret) { goto exit_immediate; } + ret = baro_init(); + if (ret) { goto exit_immediate; } + ret = adc_init(); + if (ret) { goto exit_immediate; } @@ -2075,7 +2070,7 @@ Sensors::start() nullptr); /* wait until the task is up and running or has failed */ - while(_sensors_task > 0 && _task_should_exit) { + while (_sensors_task > 0 && _task_should_exit) { usleep(100); } diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp new file mode 100644 index 000000000..ea478a60f --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). + */ + +#include <px4.h> +#include <systemlib/circuit_breaker.h> + +bool circuit_breaker_enabled(const char* breaker, int32_t magic) +{ + int32_t val; + (void)PX4_PARAM_GET_BYNAME(breaker, &val); + + return (val == magic); +} + diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index b3431722f..c97dbc26f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,7 +61,7 @@ __BEGIN_DECLS -__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); __END_DECLS diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker_params.c index 12187d70e..e499ae27a 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -42,8 +42,8 @@ * parameter needs to set to the key (magic). */ -#include <systemlib/param/param.h> -#include <systemlib/circuit_breaker.h> +#include <px4.h> +#include <systemlib/circuit_breaker_params.h> /** * Circuit breaker for power supply check @@ -56,7 +56,7 @@ * @max 894281 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); /** * Circuit breaker for rate controller output @@ -69,7 +69,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); * @max 140253 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); +PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); /** * Circuit breaker for IO safety @@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); * @max 22027 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); /** * Circuit breaker for airspeed sensor @@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); * @max 162128 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); +PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); /** * Circuit breaker for flight termination @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); +PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); /** * Circuit breaker for engine failure detection @@ -120,27 +120,4 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); - -/** - * Circuit breaker for gps failure detection - * - * Setting this parameter to 240024 will disable the gps failure detection. - * If the aircraft is in gps failure mode the gps failure flag will be - * set to healthy - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 240024 - * @group Circuit Breaker - */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); - -bool circuit_breaker_enabled(const char* breaker, int32_t magic) -{ - int32_t val; - (void)param_get(param_find(breaker), &val); - - return (val == magic); -} - +PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h new file mode 100644 index 000000000..768bf7f53 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker_params.h @@ -0,0 +1,7 @@ +#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 +#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 +#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 +#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 +#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 +#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 +#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index ca13d6265..af90c2560 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -66,6 +66,7 @@ #define _SYSTEMLIB_ERR_H #include <stdarg.h> +#include "visibility.h" __BEGIN_DECLS diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index c8a0bf5cd..e951e1e39 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -35,6 +35,8 @@ #include <stdint.h> +__BEGIN_DECLS + /* magic numbers from reference manual */ enum MCU_REV { MCU_REV_STM32F4_REV_A = 0x1000, @@ -61,3 +63,5 @@ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit); * @return The silicon revision / version number as integer */ __EXPORT int mcu_version(char* rev, char** revstr); + +__END_DECLS diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f4dff2838..f2499bbb1 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,7 +53,8 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c \ + circuit_breaker.cpp \ + circuit_breaker_params.c \ mcu_version.c MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 0c1243de3..8543ba7bb 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -40,6 +40,7 @@ #define _SYSTEMLIB_PERF_COUNTER_H value #include <stdint.h> +#include <platforms/px4_defines.h> /** * Counter types. diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index d33f93060..eb2d84726 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 b64559734..8cbe51119 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 8884e5a3a..db47218d9 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,8 @@ 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>; +template class __EXPORT Subscription<vehicle_control_mode_s>; +template class __EXPORT Subscription<actuator_armed_s>; } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 75cf36254..12301ce96 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/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ba1ac0350..f60aa8d86 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -113,8 +113,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); +#include "topics/mc_virtual_rates_setpoint.h" +ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s); +#include "topics/fw_virtual_rates_setpoint.h" +ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); @@ -184,13 +186,19 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s); /* actuator controls, as requested by controller */ #include "topics/actuator_controls.h" -ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); +#include "topics/actuator_controls_0.h" +ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s); +#include "topics/actuator_controls_1.h" +ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s); +#include "topics/actuator_controls_2.h" +ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s); +#include "topics/actuator_controls_3.h" +ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s); //Virtual control groups, used for VTOL operation -ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); +#include "topics/actuator_controls_virtual_mc.h" +ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s); +#include "topics/actuator_controls_virtual_fw.h" +ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index d2ee754cd..676c37c77 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -40,7 +40,7 @@ #ifndef TOPIC_AIRSPEED_H_ #define TOPIC_AIRSPEED_H_ -#include "../uORB.h" +#include <platforms/px4_defines.h> #include <stdint.h> /** diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index 6f16c51cf..a61f078ba 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -42,7 +42,7 @@ #include <stdint.h> #include <stdbool.h> -#include "../uORB.h" +#include <platforms/px4_defines.h> /** * @addtogroup topics diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h deleted file mode 100644 index 50b7bd9e5..000000000 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @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 manual_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_ -#define TOPIC_MANUAL_CONTROL_SETPOINT_H_ - -#include <stdint.h> -#include "../uORB.h" - -/** - * Switch position - */ -typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ - SWITCH_POS_ON, /**< switch activated (value = 1) */ - SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ - SWITCH_POS_OFF /**< switch not activated (value = -1) */ -} switch_pos_t; - -/** - * @addtogroup topics - * @{ - */ - -struct manual_control_setpoint_s { - uint64_t timestamp; - - /** - * Any of the channels may not be available and be set to NaN - * to indicate that it does not contain valid data. - * The variable names follow the definition of the - * MANUAL_CONTROL mavlink message. - * The default range is from -1 to 1 (mavlink message -1000 to 1000) - * The range for the z variable is defined from 0 to 1. (The z field of - * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) - */ - float x; /**< stick position in x direction -1..1 - in general corresponds to forward/back motion or pitch of vehicle, - in general a positive value means forward or negative pitch and - a negative value means backward or positive pitch */ - float y; /**< stick position in y direction -1..1 - in general corresponds to right/left motion or roll of vehicle, - in general a positive value means right or positive roll and - a negative value means left or negative roll */ - float z; /**< throttle stick position 0..1 - in general corresponds to up/down motion or thrust of vehicle, - in general the value corresponds to the demanded throttle by the user, - if the input is used for setting the setpoint of a vertical position - controller any value > 0.5 means up and any value < 0.5 means down */ - float r; /**< yaw stick/twist positon, -1..1 - in general corresponds to the righthand rotation around the vertical - (downwards) axis of the vehicle */ - float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ - - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(manual_control_setpoint); - -#endif diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h deleted file mode 100644 index cb2262534..000000000 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 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 mission_item_triplet.h - * Definition of the global WGS84 position setpoint uORB topic. - * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ -#define TOPIC_MISSION_ITEM_TRIPLET_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -enum SETPOINT_TYPE -{ - SETPOINT_TYPE_POSITION = 0, /**< position setpoint */ - SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ - SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ - SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ -}; - -struct position_setpoint_s -{ - bool valid; /**< true if setpoint is valid */ - enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ - float x; /**< local position setpoint in m in NED */ - float y; /**< local position setpoint in m in NED */ - float z; /**< local position setpoint in m in NED */ - bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m/s in NED */ - float vy; /**< local velocity setpoint in m/s in NED */ - float vz; /**< local velocity setpoint in m/s in NED */ - bool velocity_valid; /**< true if local velocity setpoint valid */ - double lat; /**< latitude, in deg */ - double lon; /**< longitude, in deg */ - float alt; /**< altitude AMSL, in m */ - float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ - bool yaw_valid; /**< true if yaw setpoint valid */ - float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ - bool yawspeed_valid; /**< true if yawspeed setpoint valid */ - float loiter_radius; /**< loiter radius (only for fixed wing), in m */ - int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ - float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ - float a_x; //**< acceleration x setpoint */ - float a_y; //**< acceleration y setpoint */ - float a_z; //**< acceleration z setpoint */ - bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ - bool acceleration_is_force; //*< interprete acceleration as force */ -}; - -/** - * Global position setpoint triplet in WGS84 coordinates. - * - * This are the three next waypoints (or just the next two or one). - */ -struct position_setpoint_triplet_s -{ - struct position_setpoint_s previous; - struct position_setpoint_s current; - struct position_setpoint_s next; - - unsigned nav_state; /**< report the navigation state */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(position_setpoint_triplet); - -#endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h deleted file mode 100755 index 019944dc0..000000000 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ /dev/null @@ -1,91 +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 "../uORB.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/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h deleted file mode 100644 index 1cfc37cc6..000000000 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ /dev/null @@ -1,89 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @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_setpoint.h - * Definition of the vehicle attitude setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ -#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * vehicle attitude setpoint. - */ -struct vehicle_attitude_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - float roll_body; /**< body angle in NED frame */ - float pitch_body; /**< body angle in NED frame */ - float yaw_body; /**< body angle in NED frame */ - //float body_valid; /**< Set to true if body angles are valid */ - - float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - bool R_valid; /**< Set to true if rotation matrix is valid */ - - //! For quaternion-based attitude control - float q_d[4]; /** Desired quaternion for quaternion control */ - bool q_d_valid; /**< Set to true if quaternion vector is valid */ - float q_e[4]; /** Attitude error in quaternion */ - bool q_e_valid; /**< Set to true if quaternion error vector is valid */ - - float thrust; /**< Thrust in Newton the power system should generate */ - - bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ - bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ - bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_attitude_setpoint); -ORB_DECLARE(mc_virtual_attitude_setpoint); -ORB_DECLARE(fw_virtual_attitude_setpoint); - -#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h deleted file mode 100644 index ca7705456..000000000 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 vehicle_control_mode.h - * Definition of the vehicle_control_mode uORB topic. - * - * All control apps should depend their actions based on the flags set here. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - */ - -#ifndef VEHICLE_CONTROL_MODE -#define VEHICLE_CONTROL_MODE - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" -#include "vehicle_status.h" - -/** - * @addtogroup topics @{ - */ - - -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ - -struct vehicle_control_mode_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - bool flag_armed; - - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - // XXX needs yet to be set by state machine helper - bool flag_system_hil_enabled; - - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ - bool flag_control_offboard_enabled; /**< true if offboard control should be used */ - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_force_enabled; /**< true if force control is mixed in */ - bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_control_mode); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index bc7046690..137c86dd5 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -45,7 +45,7 @@ #include <stdint.h> #include <stdbool.h> -#include "../uORB.h" +#include <platforms/px4_defines.h> /** * @addtogroup topics diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h deleted file mode 100644 index 8b46c5a3f..000000000 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 vehicle_local_position.h - * Definition of the local fused NED position uORB topic. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> - */ - -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Fused local position in NED. - */ -struct vehicle_local_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool xy_valid; /**< true if x and y are valid */ - bool z_valid; /**< true if z is valid */ - bool v_xy_valid; /**< true if vy and vy are valid */ - bool v_z_valid; /**< true if vz is valid */ - /* Position in local NED frame */ - float x; /**< X position in meters in NED earth-fixed frame */ - float y; /**< X position in meters in NED earth-fixed frame */ - float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ - /* Velocity in NED frame */ - float vx; /**< Ground X Speed (Latitude), m/s in NED */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED */ - /* Heading */ - float yaw; - /* Reference position in GPS / WGS84 frame */ - bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ - bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ - uint64_t ref_timestamp; /**< Time when reference position was set */ - double ref_lat; /**< Reference point latitude in degrees */ - double ref_lon; /**< Reference point longitude in degrees */ - float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - /* Distance to surface */ - float dist_bottom; /**< Distance to bottom surface (ground) */ - float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ - uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ - bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ - float eph; - float epv; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position); - -#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h deleted file mode 100644 index b56e81e04..000000000 --- a/src/modules/uORB/topics/vehicle_status.h +++ /dev/null @@ -1,263 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 - 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 vehicle_status.h - * Definition of the vehicle_status uORB topic. - * - * Published the state machine and the system status bitfields - * (see SYS_STATUS mavlink message), published only by commander app. - * - * All apps should write to subsystem_info: - * - * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <julian@oes.ch> - */ - -#ifndef VEHICLE_STATUS_H_ -#define VEHICLE_STATUS_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics @{ - */ - -/** - * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. - */ -typedef enum { - MAIN_STATE_MANUAL = 0, - MAIN_STATE_ALTCTL, - MAIN_STATE_POSCTL, - MAIN_STATE_AUTO_MISSION, - MAIN_STATE_AUTO_LOITER, - MAIN_STATE_AUTO_RTL, - MAIN_STATE_ACRO, - MAIN_STATE_OFFBOARD, - MAIN_STATE_MAX -} main_state_t; - -// If you change the order, add or remove arming_state_t states make sure to update the arrays -// in state_machine_helper.cpp as well. -typedef enum { - ARMING_STATE_INIT = 0, - ARMING_STATE_STANDBY, - ARMING_STATE_ARMED, - ARMING_STATE_ARMED_ERROR, - ARMING_STATE_STANDBY_ERROR, - ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE, - ARMING_STATE_MAX, -} arming_state_t; - -typedef enum { - HIL_STATE_OFF = 0, - HIL_STATE_ON -} hil_state_t; - -/** - * Navigation state, i.e. "what should vehicle do". - */ -typedef enum { - NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ - NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ - NAVIGATION_STATE_POSCTL, /**< Position control mode */ - NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ - NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ - NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ - NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */ - NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ - NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ - NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ - NAVIGATION_STATE_ACRO, /**< Acro mode */ - NAVIGATION_STATE_LAND, /**< Land mode */ - NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ - NAVIGATION_STATE_TERMINATION, /**< Termination mode */ - NAVIGATION_STATE_OFFBOARD, - NAVIGATION_STATE_MAX, -} navigation_state_t; - -enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 -}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ - -/** - * Should match 1:1 MAVLink's MAV_TYPE ENUM - */ -enum VEHICLE_TYPE { - VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */ - VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */ - VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */ - VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */ - VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */ - VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */ - VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */ - VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */ - VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */ - VEHICLE_TYPE_ROCKET = 9, /* Rocket | */ - VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */ - VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */ - VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */ - VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */ - VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */ - VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ - VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ - VEHICLE_TYPE_KITE = 17, /* Kite | */ - VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ - VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */ - VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/ - VEHICLE_TYPE_ENUM_END = 21 /* | */ -}; - -enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ - VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ -}; - -/** - * @addtogroup topics - * @{ - */ - -/** - * state machine / state of vehicle. - * - * Encodes the complete system state and is set by the commander app. - */ -struct vehicle_status_s { - /* use of a counter and timestamp recommended (but not necessary) */ - - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - main_state_t main_state; /**< main state machine */ - navigation_state_t nav_state; /**< set navigation state machine to specified value */ - arming_state_t arming_state; /**< current arming state */ - hil_state_t hil_state; /**< current hil state */ - bool failsafe; /**< true if system is in failsafe state */ - - int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ - int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ - int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - - bool is_rotary_wing; /**< True if system is in rotary wing configuration, so for a VTOL - this is only true while flying as a multicopter */ - bool is_vtol; /**< True if the system is VTOL capable */ - - bool vtol_fw_permanent_stab; /**< True if vtol should stabilize attitude for fw in manual mode */ - - bool condition_battery_voltage_valid; - bool condition_system_in_air_restore; /**< true if we can restore in mid air */ - bool condition_system_sensors_initialized; - bool condition_system_returned_to_home; - bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ - bool condition_launch_position_valid; /**< indicates a valid launch position */ - bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ - bool condition_local_position_valid; - bool condition_local_altitude_valid; - bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ - bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ - bool condition_power_input_valid; /**< set if input power is valid */ - float avionics_power_rail_voltage; /**< voltage of the avionics power rail */ - - bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception lost */ - uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */ - bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ - bool rc_input_blocked; /**< set if RC input should be ignored */ - - bool data_link_lost; /**< datalink to GCS lost */ - bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */ - uint8_t data_link_lost_counter; /**< counts unique data link lost events */ - bool engine_failure; /** Set to true if an engine failure is detected */ - bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */ - bool gps_failure; /** Set to true if a gps failure is detected */ - bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ - - bool barometer_failure; /** Set to true if a barometer failure is detected */ - - bool offboard_control_signal_found_once; - bool offboard_control_signal_lost; - bool offboard_control_signal_weak; - uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command - and should not be overridden by RC */ - - /* see SYS_STATUS mavlink message for the following */ - uint32_t onboard_control_sensors_present; - uint32_t onboard_control_sensors_enabled; - uint32_t onboard_control_sensors_health; - - float load; /**< processor load from 0 to 1 */ - float battery_voltage; - float battery_current; - float battery_remaining; - - enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ - uint16_t drop_rate_comm; - uint16_t errors_comm; - uint16_t errors_count1; - uint16_t errors_count2; - uint16_t errors_count3; - uint16_t errors_count4; - - bool circuit_breaker_engaged_power_check; - bool circuit_breaker_engaged_airspd_check; - bool circuit_breaker_engaged_enginefailure_check; - bool circuit_breaker_engaged_gpsfailure_check; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_status); - -#endif diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 6f021459c..764e33179 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -56,6 +56,7 @@ #include <nuttx/arch.h> #include <nuttx/wqueue.h> #include <nuttx/clock.h> +#include <systemlib/systemlib.h> #include <drivers/drv_hrt.h> @@ -682,9 +683,11 @@ namespace { ORBDevMaster *g_dev; +bool pubsubtest_passed = false; struct orb_test { int val; + hrt_abstime time; }; ORB_DEFINE(orb_test, struct orb_test); @@ -718,6 +721,46 @@ test_note(const char *fmt, ...) return OK; } +int pubsublatency_main(void) +{ + /* poll on test topic and output latency */ + float latency_integral = 0.0f; + + /* wakeup source(s) */ + struct pollfd fds[1]; + + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + + fds[0].fd = test_multi_sub; + fds[0].events = POLLIN; + + struct orb_test t; + + const unsigned maxruns = 10; + + for (unsigned i = 0; i < maxruns; i++) { + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t); + + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + hrt_abstime elt = hrt_elapsed_time(&t.time); + latency_integral += elt; + } + + orb_unsubscribe(test_multi_sub); + + warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns)); + + pubsubtest_passed = true; + + return OK; +} + int test() { @@ -779,8 +822,7 @@ test() int instance0; int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - test_note("advertised"); - usleep(300000); + test_note("advertised"); int instance1; int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); @@ -795,8 +837,7 @@ test() if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) return test_fail("mult. pub0 fail"); - test_note("published"); - usleep(300000); + test_note("published"); t.val = 203; if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) @@ -805,19 +846,19 @@ test() /* subscribe to both topics and ensure valid data is received */ int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t)) + if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) return test_fail("sub #0 copy failed: %d", errno); - if (t.val != 103) - return test_fail("sub #0 val. mismatch: %d", t.val); + if (u.val != 103) + return test_fail("sub #0 val. mismatch: %d", u.val); int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); - if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t)) + if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) return test_fail("sub #1 copy failed: %d", errno); - if (t.val != 203) - return test_fail("sub #1 val. mismatch: %d", t.val); + if (u.val != 203) + return test_fail("sub #1 val. mismatch: %d", u.val); /* test priorities */ int prio; @@ -833,6 +874,30 @@ test() if (prio != ORB_PRIO_MIN) return test_fail("prio: %d", prio); + /* test pub / sub latency */ + + int pubsub_task = task_spawn_cmd("uorb_latency", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (main_t)&pubsublatency_main, + nullptr); + + /* give the test task some data */ + while (!pubsubtest_passed) { + t.val = 303; + t.time = hrt_absolute_time(); + if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 timing fail"); + + /* simulate >800 Hz system operation */ + usleep(1000); + } + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + return test_note("PASS"); } diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 9c33c8a3e..b54f0322b 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -144,7 +144,7 @@ typedef intptr_t orb_advert_t; * node in /obj if required and publishes the initial data. * * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. + * but co-ordination between publishers is not provided by the ORB. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -351,4 +351,13 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; __END_DECLS +/* Diverse uORB header defines */ //XXX: move to better location +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; +typedef uint8_t arming_state_t; +typedef uint8_t main_state_t; +typedef uint8_t hil_state_t; +typedef uint8_t navigation_state_t; +typedef uint8_t switch_pos_t; + #endif /* _UORB_UORB_H */ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4dc03b61b..b93a95f96 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -381,7 +381,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; @@ -393,14 +393,14 @@ int UavcanNode::run() /* see if we have any direct actuator updates */ - if (_actuator_direct_sub != -1 && + if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && !_test_in_progress) { if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; } - memcpy(&_outputs.output[0], &_actuator_direct.values[0], + memcpy(&_outputs.output[0], &_actuator_direct.values[0], _actuator_direct.nvalues*sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; @@ -476,7 +476,7 @@ int UavcanNode::run() if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down and motor + // Update the armed status and check that we're not locked down and motor // test is not running bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; @@ -502,7 +502,7 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; @@ -526,7 +526,7 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 8e68730b8..0a333eade 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -58,7 +58,11 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_virtual_mc.h> +#include <uORB/topics/actuator_controls_virtual_fw.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/mc_virtual_rates_setpoint.h> +#include <uORB/topics/fw_virtual_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vtol_vehicle_status.h> 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/modules/uORB/topics/parameter_update.h b/src/platforms/nuttx/px4_nuttx_impl.cpp index 68964deb0..70e292320 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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,30 +32,26 @@ ****************************************************************************/ /** - * @file parameter_update.h - * Notification about a parameter update. + * @file px4_nuttx_impl.cpp + * + * PX4 Middleware Wrapper NuttX Implementation */ -#ifndef TOPIC_PARAMETER_UPDATE_H -#define TOPIC_PARAMETER_UPDATE_H - -#include <stdint.h> -#include "../uORB.h" +#include <px4.h> +#include <drivers/drv_hrt.h> -/** - * @addtogroup topics - * @{ - */ -struct parameter_update_s { - /** time at which the latest parameter was updated */ - uint64_t timestamp; -}; +namespace px4 +{ -/** - * @} - */ +void init(int argc, char *argv[], const char *process_name) +{ + PX4_WARN("process: %s", process_name); +} -ORB_DECLARE(parameter_update); +uint64_t get_time_micros() +{ + return hrt_absolute_time(); +} -#endif
\ No newline at end of file +} diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h new file mode 100644 index 000000000..fa4e1398e --- /dev/null +++ b/src/platforms/px4_defines.h @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * 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_defines.h + * + * 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(__PX4_ROS) +/* + * Building for running within the ROS environment + */ +#define noreturn_function +#ifdef __cplusplus +#include "ros/ros.h" +#endif +/* 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) px4::_name + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr); +/* 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); +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name)); + +/* Parameter handle datatype */ +typedef const char *px4_param_t; + +/* Helper functions to set ROS params, only int and float supported */ +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) +{ + if (!ros::param::has(name)) { + ros::param::set(name, value); + } + + return (px4_param_t)name; +}; +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) +{ + if (!ros::param::has(name)) { + 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 by handle */ +#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) + +/* Get value of parameter by name, which is equal to the handle for ros */ +#define PX4_PARAM_GET_BYNAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt) + +#define OK 0 +#define ERROR -1 + +//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work +#define isfinite(_value) std::isfinite(_value) + +/* Useful constants. */ +#define M_E_F 2.7182818284590452354f +#define M_LOG2E_F 1.4426950408889634074f +#define M_LOG10E_F 0.43429448190325182765f +#define M_LN2_F _M_LN2_F +#define M_LN10_F 2.30258509299404568402f +#define M_PI_F 3.14159265358979323846f +#define M_TWOPI_F (M_PI_F * 2.0f) +#define M_PI_2_F 1.57079632679489661923f +#define M_PI_4_F 0.78539816339744830962f +#define M_3PI_4_F 2.3561944901923448370E0f +#define M_SQRTPI_F 1.77245385090551602792981f +#define M_1_PI_F 0.31830988618379067154f +#define M_2_PI_F 0.63661977236758134308f +#define M_2_SQRTPI_F 1.12837916709551257390f +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f +#define M_SQRT2_F 1.41421356237309504880f +#define M_SQRT1_2_F 0.70710678118654752440f +#define M_LN2LO_F 1.9082149292705877000E-10f +#define M_LN2HI_F 6.9314718036912381649E-1f +#define M_SQRT3_F 1.73205080756887719000f +#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ +#define M_LOG2_E_F _M_LN2_F +#define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 + +#else +/* + * Building for NuttX + */ +#include <platforms/px4_includes.h> +/* Main entry point */ +#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_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, _objptr, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, 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) +/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), _interval) + +/* Parameter handle datatype */ +#include <systemlib/param/param.h> +typedef param_t px4_param_t; + +/* Initialize a param, get param handle */ +#define PX4_PARAM_INIT(_name) param_find(#_name) + +/* Get value of parameter by handle */ +#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) + +/* Get value of parameter by name */ +#define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt) + +/* XXX this is a hack to resolve conflicts with NuttX headers */ +#if !defined(__PX4_TESTS) +#define isspace(c) \ + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') +#endif + +#endif + +/* Defines for all platforms */ + +/* Shortcut for subscribing to topics + * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all + */ +#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, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__) + +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER(_name) Subscriber<PX4_TOPIC_T(_name)> + +/* 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..f8561fa3b --- /dev/null +++ b/src/platforms/px4_includes.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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(__PX4_ROS) +/* + * Building for running within the ROS environment + */ + +#ifdef __cplusplus +#include "ros/ros.h" +#include <px4_rc_channels.h> +#include <px4_vehicle_attitude.h> +#include <px4_vehicle_attitude_setpoint.h> +#include <px4_manual_control_setpoint.h> +#include <px4_actuator_controls.h> +#include <px4_actuator_controls_0.h> +#include <px4_actuator_controls_virtual_mc.h> +#include <px4_vehicle_rates_setpoint.h> +#include <px4_mc_virtual_rates_setpoint.h> +#include <px4_vehicle_attitude.h> +#include <px4_vehicle_control_mode.h> +#include <px4_actuator_armed.h> +#include <px4_parameter_update.h> +#include <px4_vehicle_status.h> +#include <px4_vehicle_local_position_setpoint.h> +#include <px4_vehicle_global_velocity_setpoint.h> +#include <px4_vehicle_local_position.h> +#include <px4_position_setpoint_triplet.h> +#endif + +#else +/* + * Building for NuttX + */ +#include <nuttx/config.h> +#include <uORB/uORB.h> +#ifdef __cplusplus +#include <platforms/nuttx/px4_messages/px4_rc_channels.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_manual_control_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_0.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_1.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_2.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h> +#include <platforms/nuttx/px4_messages/px4_actuator_armed.h> +#include <platforms/nuttx/px4_messages/px4_parameter_update.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_status.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_local_position_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h> +#include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h> +#endif +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> + +#endif diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/platforms/px4_message.h index 0f6c9aca1..bff7aa313 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/platforms/px4_message.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 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,38 +32,46 @@ ****************************************************************************/ /** - * @file actuator_armed.h - * - * Actuator armed topic + * @file px4_message.h * + * Defines the message base types */ +#pragma once -#ifndef TOPIC_ACTUATOR_ARMED_H -#define TOPIC_ACTUATOR_ARMED_H +#if defined(__PX4_ROS) +typedef const char* PX4TopicHandle; +#else +#include <uORB/uORB.h> +typedef orb_id_t PX4TopicHandle; +#endif -#include <stdint.h> -#include "../uORB.h" +namespace px4 +{ -/** - * @addtogroup topics - * @{ - */ +template <typename M> +class __EXPORT PX4Message +{ + // friend class NodeHandle; +// #if defined(__PX4_ROS) + // template<typename T> + // friend class SubscriberROS; +// #endif -/** global 'actuator output is live' control. */ -struct actuator_armed_s { +public: + PX4Message() : + _data() + {} - uint64_t 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 */ -}; + PX4Message(M msg) : + _data(msg) + {} -/** - * @} - */ + virtual ~PX4Message() {}; -/* register this as object request broker structure */ -ORB_DECLARE(actuator_armed); + virtual M& data() {return _data;} + virtual const M& data() const {return _data;} +private: + M _data; +}; -#endif +} diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/platforms/px4_middleware.h index 47d51f199..735d34234 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/platforms/px4_middleware.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * 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 @@ -33,36 +32,53 @@ ****************************************************************************/ /** - * @file vehicle_rates_setpoint.h - * Definition of the vehicle rates setpoint topic + * @file px4_middleware.h + * + * PX4 generic middleware wrapper */ -#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ -#define TOPIC_VEHICLE_RATES_SETPOINT_H_ +#pragma once #include <stdint.h> -#include "../uORB.h" +#include <unistd.h> + +namespace px4 +{ + +__EXPORT void init(int argc, char *argv[], const char *process_name); +__EXPORT uint64_t get_time_micros(); + +#if defined(__PX4_ROS) +/** + * Returns true if the app/task should continue to run + */ +inline bool ok() { return ros::ok(); } +#else +extern bool task_should_exit; /** - * @addtogroup topics - * @{ + * Returns true if the app/task should continue to run */ -struct vehicle_rates_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start */ +__EXPORT inline bool ok() { return !task_should_exit; } +#endif - float roll; /**< body angular rates in NED frame */ - float pitch; /**< body angular rates in NED frame */ - float yaw; /**< body angular rates in NED frame */ - float thrust; /**< thrust normalized to 0..1 */ +class Rate +{ +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; } -}; /**< vehicle_rates_setpoint */ + /** + * Sleep for 1/rate_hz s + */ + void sleep() { usleep(sleep_interval); } -/** -* @} -*/ +private: + uint64_t sleep_interval; -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_rates_setpoint); -ORB_DECLARE(mc_virtual_rates_setpoint); -ORB_DECLARE(fw_virtual_rates_setpoint); -#endif +}; + +} // namespace px4 diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h new file mode 100644 index 000000000..83a3e422d --- /dev/null +++ b/src/platforms/px4_nodehandle.h @@ -0,0 +1,302 @@ +/**************************************************************************** + * + * 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(__PX4_ROS) +/* includes when building for ros */ +#include "ros/ros.h" +#include <list> +#include <inttypes.h> +#include <type_traits> +#else +/* includes when building for NuttX */ +#include <poll.h> +#endif +#include <functional> + +namespace px4 +{ +#if defined(__PX4_ROS) +class NodeHandle : + private ros::NodeHandle +{ +public: + NodeHandle() : + ros::NodeHandle(), + _subs(), + _pubs() + {} + + ~NodeHandle() + { + _subs.clear(); + _pubs.clear(); + }; + + /** + * Subscribe with callback to function + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ + template<typename T> + Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval) + { + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); + _subs.push_back(sub); + return (Subscriber<T> *)sub; + } + + /** + * Subscribe with callback to class method + * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance + */ + template<typename T, typename C> + Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) + { + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); + _subs.push_back(sub); + return (Subscriber<T> *)sub; + } + + /** + * Subscribe with no callback, just the latest value is stored on updates + */ + template<typename T> + Subscriber<T> *subscribe(unsigned interval) + { + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this); + _subs.push_back(sub); + return (Subscriber<T> *)sub; + } + + /** + * Advertise topic + */ + template<typename T> + Publisher<T>* advertise() + { + PublisherROS<T> *pub = new PublisherROS<T>((ros::NodeHandle*)this); + _pubs.push_back((PublisherBase*)pub); + return (Publisher<T>*)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(); } + + +protected: + std::list<SubscriberBase *> _subs; /**< Subcriptions of node */ + std::list<PublisherBase *> _pubs; /**< Publications of node */ +}; +#else //Building for NuttX +class __EXPORT NodeHandle +{ +public: + NodeHandle() : + _subs(), + _pubs(), + _sub_min_interval(nullptr) + {} + + ~NodeHandle() + { + /* Empty subscriptions list */ + SubscriberNode *sub = _subs.getHead(); + int count = 0; + + while (sub != nullptr) { + if (count++ > kMaxSubscriptions) { + PX4_WARN("exceeded max subscriptions"); + break; + } + + SubscriberNode *sib = sub->getSibling(); + delete sub; + sub = sib; + } + + /* Empty publications list */ + PublisherNode *pub = _pubs.getHead(); + count = 0; + + while (pub != nullptr) { + if (count++ > kMaxPublications) { + PX4_WARN("exceeded max publications"); + break; + } + + PublisherNode *sib = pub->getSibling(); + delete pub; + pub = sib; + } + }; + + /** + * Subscribe with callback to function + * @param fp Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + + template<typename T> + Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval) + { + (void)interval; + SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1)); + update_sub_min_interval(interval, sub_px4); + _subs.add((SubscriberNode *)sub_px4); + return (Subscriber<T> *)sub_px4; + } + + /** + * Subscribe with callback to class method + * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance + */ + template<typename T, typename C> + Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) + { + (void)interval; + SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1)); + update_sub_min_interval(interval, sub_px4); + _subs.add((SubscriberNode *)sub_px4); + return (Subscriber<T> *)sub_px4; + } + + /** + * Subscribe without callback to function + * @param interval Minimal interval between data fetches from orb + */ + + template<typename T> + Subscriber<T> *subscribe(unsigned interval) + { + (void)interval; + SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval); + update_sub_min_interval(interval, sub_px4); + _subs.add((SubscriberNode *)sub_px4); + return (Subscriber<T> *)sub_px4; + } + + /** + * Advertise topic + */ + template<typename T> + Publisher<T> *advertise() + { + PublisherUORB<T> *pub = new PublisherUORB<T>(); + _pubs.add(pub); + return (Publisher<T>*)pub; + } + + /** + * Calls all callback waiting to be called + */ + void spinOnce() + { + /* Loop through subscriptions, call callback for updated subscriptions */ + SubscriberNode *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->getUORBHandle(); + pfd.events = POLLIN; + poll(&pfd, 1, timeout_ms); + spinOnce(); + } + } +protected: + static const uint16_t kMaxSubscriptions = 100; + static const uint16_t kMaxPublications = 100; + List<SubscriberNode *> _subs; /**< Subcriptions of node */ + List<PublisherNode *> _pubs; /**< Publications of node */ + SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + of all Subscriptions in _subs*/ + + /** + * Check if this is the smallest interval so far and update _sub_min_interval + */ + template<typename T> + void update_sub_min_interval(unsigned interval, SubscriberUORB<T> *sub) + { + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub; + } + } +}; +#endif +} diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h new file mode 100644 index 000000000..d9cd7a3c1 --- /dev/null +++ b/src/platforms/px4_publisher.h @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * 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(__PX4_ROS) +/* includes when building for ros */ +#include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Publication.hpp> +#include <containers/List.hpp> +#endif + +#include <platforms/px4_message.h> + +namespace px4 +{ + +/** + * Untemplated publisher base class + * */ +class __EXPORT PublisherBase +{ +public: + PublisherBase() {}; + ~PublisherBase() {}; +}; + +/** + * Publisher base class, templated with the message type + * */ +template <typename T> +class __EXPORT Publisher +{ +public: + Publisher() {}; + ~Publisher() {}; + + virtual int publish(const T &msg) = 0; +}; + +#if defined(__PX4_ROS) +template <typename T> +class PublisherROS : + public Publisher<T> +{ +public: + /** + * Construct Publisher by providing a ros::Publisher + * @param ros_pub the ros publisher which will be used to perform the publications + */ + PublisherROS(ros::NodeHandle *rnh) : + Publisher<T>(), + _ros_pub(rnh->advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault)) + {} + + ~PublisherROS() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ + int publish(const T &msg) + { + _ros_pub.publish(msg.data()); + return 0; + } +protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ + ros::Publisher _ros_pub; /**< Handle to the ros publisher */ +}; +#else +/** + * Because we maintain a list of publishers we need a node class + */ +class __EXPORT PublisherNode : + public ListNode<PublisherNode *> +{ +public: + PublisherNode() : + ListNode() + {} + + virtual ~PublisherNode() {} + + virtual void update() = 0; +}; + +template <typename T> +class __EXPORT PublisherUORB : + public Publisher<T>, + public PublisherNode + +{ +public: + /** + * Construct Publisher by providing orb meta data + */ + PublisherUORB() : + Publisher<T>(), + PublisherNode(), + _uorb_pub(new uORB::PublicationBase(T::handle())) + {} + + ~PublisherUORB() { + delete _uorb_pub; + }; + + /** Publishes msg + * @param msg the message which is published to the topic + */ + int publish(const T &msg) + { + _uorb_pub->update((void *)&(msg.data())); + return 0; + } + + /** + * Empty callback for list traversal + */ + void update() {} ; +private: + uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + +}; +#endif +} diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h new file mode 100644 index 000000000..6ca35b173 --- /dev/null +++ b/src/platforms/px4_subscriber.h @@ -0,0 +1,284 @@ +/**************************************************************************** + * + * 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 + +#include <functional> +#include <type_traits> + +#if defined(__PX4_ROS) +/* includes when building for ros */ +#include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Subscription.hpp> +#include <containers/List.hpp> +#include "px4_nodehandle.h" +#endif + +namespace px4 +{ + +/** + * Untemplated subscriber base class + * */ +class __EXPORT SubscriberBase +{ +public: + SubscriberBase() {}; + virtual ~SubscriberBase() {}; + +}; + +/** + * Subscriber class which is used by nodehandle + */ +template<typename T> +class __EXPORT Subscriber : + public SubscriberBase +{ +public: + Subscriber() : + SubscriberBase(), + _msg_current() + {}; + + virtual ~Subscriber() {} + + /* Accessors*/ + /** + * Get the last message value + */ + virtual T& get() {return _msg_current;} + + /** + * Get the last native message value + */ + virtual decltype(((T*)nullptr)->data()) data() + { + return _msg_current.data(); + } + +protected: + T _msg_current; /**< Current Message value */ +}; + +#if defined(__PX4_ROS) +/** + * Subscriber class that is templated with the ros n message type + */ +template<typename T> +class SubscriberROS : + public Subscriber<T> +{ +public: + /** + * Construct Subscriber without a callback function + */ + SubscriberROS(ros::NodeHandle *rnh) : + px4::Subscriber<T>(), + _cbf(NULL), + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this)) + {} + + /** + * Construct Subscriber by providing a callback function + */ + SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) : + _cbf(cbf), + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this)) + {} + + virtual ~SubscriberROS() {}; + +protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ + + /** + * Called on topic update, saves the current message and then calls the provided callback function + * needs to use the native type as it is called by ROS + */ + void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg) + { + /* Store data */ + this->_msg_current.data() = msg; + + /* Call callback */ + if (_cbf != NULL) { + _cbf(this->get()); + } + + } + + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ + +}; + +#else // Building for NuttX +/** + * Because we maintain a list of subscribers we need a node class + */ +class __EXPORT SubscriberNode : + public ListNode<SubscriberNode *> +{ +public: + SubscriberNode(unsigned interval) : + ListNode(), + _interval(interval) + {} + + virtual ~SubscriberNode() {} + + virtual void update() = 0; + + virtual int getUORBHandle() = 0; + + unsigned get_interval() { return _interval; } + +protected: + unsigned _interval; + +}; + +/** + * Subscriber class that is templated with the uorb subscription message type + */ +template<typename T> +class __EXPORT SubscriberUORB : + public Subscriber<T>, + public SubscriberNode +{ +public: + + /** + * Construct SubscriberUORB by providing orb meta data without callback + * @param interval Minimal interval between calls to callback + */ + SubscriberUORB(unsigned interval) : + SubscriberNode(interval), + _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) + {} + + virtual ~SubscriberUORB() { + delete _uorb_sub; + }; + + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + */ + virtual void update() + { + if (!_uorb_sub->updated()) { + /* Topic not updated, do not call callback */ + return; + } + + _uorb_sub->update(get_void_ptr()); + }; + + /* Accessors*/ + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + + typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() + { + return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; + } + + /** + * Get void pointer to last message value + */ + void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } + +}; + +//XXX reduce to one class with overloaded constructor? +template<typename T> +class __EXPORT SubscriberUORBCallback : + public SubscriberUORB<T> +{ +public: + /** + * Construct SubscriberUORBCallback by providing orb meta data + * @param cbf Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + SubscriberUORBCallback(unsigned interval, + std::function<void(const T &)> cbf) : + SubscriberUORB<T>(interval), + _cbf(cbf) + {} + + virtual ~SubscriberUORBCallback() {}; + + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + * If new data is available the callback is called + */ + virtual void update() + { + if (!this->_uorb_sub->updated()) { + /* Topic not updated, do not call callback */ + return; + } + + /* get latest data */ + this->_uorb_sub->update(this->get_void_ptr()); + + + /* Check if there is a callback */ + if (_cbf == nullptr) { + return; + } + + /* Call callback which performs actions based on this data */ + _cbf(Subscriber<T>::get()); + + }; + +protected: + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ +}; +#endif + +} diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h new file mode 100755 index 000000000..c7271c157 --- /dev/null +++ b/src/platforms/ros/eigen_math.h @@ -0,0 +1,19 @@ +/* + * eigen_math.h + * + * Created on: Aug 25, 2014 + * Author: roman + */ + +#ifndef EIGEN_MATH_H_ +#define EIGEN_MATH_H_ + + +struct eigen_matrix_instance { + int numRows; + int numCols; + float *pData; +}; + + +#endif /* EIGEN_MATH_H_ */ diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp new file mode 100644 index 000000000..04094de8b --- /dev/null +++ b/src/platforms/ros/geo.cpp @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <geo/geo.h> +#include <px4.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <string.h> +#include <float.h> + +__EXPORT float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= M_PI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < -M_PI_F) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_2pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= M_TWOPI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < 0.0f) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_180(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= 180.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < -180.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_360(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= 360.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < 0.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) +{ + return ref->init_done; +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); + + ref->timestamp = timestamp; + ref->init_done = true; + + return 0; +} + +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; + + return 0; +} + +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; + + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon_rad); + + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); + + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; + + return 0; +} + +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_rad; + double lon_rad; + + if (fabs(c) > DBL_EPSILON) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + + } else { + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; + } + + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; + + return 0; +} diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md new file mode 100644 index 000000000..aafc647ff --- /dev/null +++ b/src/platforms/ros/nodes/README.md @@ -0,0 +1,22 @@ +# PX4 Nodes + +This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in +ROS. They act as a bridge between the PX4 specific topics and the ROS topics. + +## Joystick Input + +You will need to install the ros joystick packages +See http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick + +### Arch Linux +```sh +yaourt -Sy ros-indigo-joystick-drivers --noconfirm +``` +check joystick +```sh +ls /dev/input/ +ls -l /dev/input/js0 +``` +(replace 0 by the number you find with the first command) + +make sure the joystick is accessible by the `input` group and that your user is in the `input` group diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp new file mode 100644 index 000000000..a3e4bca30 --- /dev/null +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * 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 att_estimator.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Roman Bapst <romanbapst@yahoo.de> +*/ + +#include "attitude_estimator.h" + +#include <px4/vehicle_attitude.h> +#include <mathlib/mathlib.h> +#include <platforms/px4_defines.h> +#include <platforms/px4_middleware.h> + +AttitudeEstimator::AttitudeEstimator() : + _n(), + // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), + _vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1)) +{ + std::string vehicle_model; + _n.param("vehicle_model", vehicle_model, std::string("iris")); + _sub_imu = _n.subscribe("/" + vehicle_model + "/imu", 1, &AttitudeEstimator::ImuCallback, this); +} + +void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) +{ + px4::vehicle_attitude msg_v_att; + + /* Fill px4 attitude topic with contents from modelstates topic */ + + /* Convert quaternion to rotation matrix */ + math::Quaternion quat; + //XXX: search for ardrone or other (other than 'plane') vehicle here + int index = 1; + quat(0) = (float)msg->pose[index].orientation.w; + quat(1) = (float)msg->pose[index].orientation.x; + quat(2) = (float) - msg->pose[index].orientation.y; + quat(3) = (float) - msg->pose[index].orientation.z; + + msg_v_att.q[0] = quat(0); + msg_v_att.q[1] = quat(1); + msg_v_att.q[2] = quat(2); + msg_v_att.q[3] = quat(3); + + math::Matrix<3, 3> rot = quat.to_dcm(); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(msg_v_att.R, i, j) = rot(i, j); + } + } + + msg_v_att.R_valid = true; + + math::Vector<3> euler = rot.to_euler(); + msg_v_att.roll = euler(0); + msg_v_att.pitch = euler(1); + msg_v_att.yaw = euler(2); + + //XXX this is in inertial frame + // msg_v_att.rollspeed = (float)msg->twist[index].angular.x; + // msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y; + // msg_v_att.yawspeed = -(float)msg->twist[index].angular.z; + + msg_v_att.timestamp = px4::get_time_micros(); + _vehicle_attitude_pub.publish(msg_v_att); +} + +void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) +{ + px4::vehicle_attitude msg_v_att; + + /* Fill px4 attitude topic with contents from modelstates topic */ + + /* Convert quaternion to rotation matrix */ + math::Quaternion quat; + //XXX: search for ardrone or other (other than 'plane') vehicle here + int index = 1; + quat(0) = (float)msg->orientation.w; + quat(1) = (float)msg->orientation.x; + quat(2) = (float) - msg->orientation.y; + quat(3) = (float) - msg->orientation.z; + + msg_v_att.q[0] = quat(0); + msg_v_att.q[1] = quat(1); + msg_v_att.q[2] = quat(2); + msg_v_att.q[3] = quat(3); + + math::Matrix<3, 3> rot = quat.to_dcm(); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(msg_v_att.R, i, j) = rot(i, j); + } + } + + msg_v_att.R_valid = true; + + math::Vector<3> euler = rot.to_euler(); + msg_v_att.roll = euler(0); + msg_v_att.pitch = euler(1); + msg_v_att.yaw = euler(2); + + msg_v_att.rollspeed = (float)msg->angular_velocity.x; + msg_v_att.pitchspeed = -(float)msg->angular_velocity.y; + msg_v_att.yawspeed = -(float)msg->angular_velocity.z; + + msg_v_att.timestamp = px4::get_time_micros(); + _vehicle_attitude_pub.publish(msg_v_att); +} + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "attitude_estimator"); + AttitudeEstimator m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h new file mode 100644 index 000000000..f760a39d8 --- /dev/null +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 att_estimator.h + * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <gazebo_msgs/ModelStates.h> +#include <sensor_msgs/Imu.h> + +class AttitudeEstimator +{ +public: + AttitudeEstimator(); + + ~AttitudeEstimator() {} + +protected: + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); + void ImuCallback(const sensor_msgs::ImuConstPtr &msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_modelstates; + ros::Subscriber _sub_imu; + ros::Publisher _vehicle_attitude_pub; + + +}; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp new file mode 100644 index 000000000..2673122c7 --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * 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 commander.cpp + * Dummy commander node that publishes the various status topics + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "commander.h" + +#include <platforms/px4_middleware.h> + +Commander::Commander() : + _n(), + _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)), + _vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)), + _actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)), + _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)), + _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)), + _msg_parameter_update(), + _msg_actuator_armed(), + _msg_vehicle_control_mode() +{ +} + +void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) +{ + px4::vehicle_status msg_vehicle_status; + + /* fill vehicle control mode based on (faked) stick positions*/ + EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode); + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + + /* fill actuator armed */ + float arm_th = 0.95; + _msg_actuator_armed.timestamp = px4::get_time_micros(); + + if (_msg_actuator_armed.armed) { + /* Check for disarm */ + if (msg->r < -arm_th && msg->z < (1 - arm_th)) { + _msg_actuator_armed.armed = false; + _msg_vehicle_control_mode.flag_armed = false; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; + } + + } else { + /* Check for arm */ + if (msg->r > arm_th && msg->z < (1 - arm_th)) { + _msg_actuator_armed.armed = true; + _msg_vehicle_control_mode.flag_armed = true; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + } + } + + /* fill vehicle status */ + msg_vehicle_status.timestamp = px4::get_time_micros(); + msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; + msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + msg_vehicle_status.is_rotary_wing = true; + + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); + _actuator_armed_pub.publish(_msg_actuator_armed); + _vehicle_status_pub.publish(msg_vehicle_status); + + /* Fill parameter update */ + if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) { + _msg_parameter_update.timestamp = px4::get_time_micros(); + _parameter_update_pub.publish(_msg_parameter_update); + } +} + +void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) { + // XXX this is a minimal implementation. If more advanced functionalities are + // needed consider a full port of the commander + + switch (msg->mode_switch) { + case px4::manual_control_setpoint::SWITCH_POS_NONE: + ROS_WARN("Joystick button mapping error, main mode not set"); + break; + + case px4::manual_control_setpoint::SWITCH_POS_OFF: // MANUAL + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = false; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = false; + msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; + + break; + + case px4::manual_control_setpoint::SWITCH_POS_MIDDLE: // ASSIST + if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_POSCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = true; + msg_vehicle_control_mode.flag_control_velocity_enabled = true; + } else { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; + } + break; + } + +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "commander"); + Commander m; + ros::spin(); + return 0; +} diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h new file mode 100644 index 000000000..58b7257b7 --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 commander.h + * Dummy commander node that publishes the various status topics + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <px4/manual_control_setpoint.h> +#include <px4/vehicle_control_mode.h> +#include <px4/vehicle_status.h> +#include <px4/parameter_update.h> +#include <px4/actuator_armed.h> + +class Commander +{ +public: + Commander(); + + ~Commander() {} + +protected: + /** + * Based on manual control input the status will be set + */ + void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); + + /** + * Set control mode flags based on stick positions (equiv to code in px4 commander) + */ + void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); + + ros::NodeHandle _n; + ros::Subscriber _man_ctrl_sp_sub; + ros::Publisher _vehicle_control_mode_pub; + ros::Publisher _actuator_armed_pub; + ros::Publisher _vehicle_status_pub; + ros::Publisher _parameter_update_pub; + + px4::parameter_update _msg_parameter_update; + px4::actuator_armed _msg_actuator_armed; + px4::vehicle_control_mode _msg_vehicle_control_mode; + +}; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp new file mode 100644 index 000000000..72f6e252f --- /dev/null +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * 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 manual_input.cpp + * Reads from the ros joystick topic and publishes to the px4 manual control input topic. + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "manual_input.h" + +#include <platforms/px4_middleware.h> + +ManualInput::ManualInput() : + _n(), + _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)), + _man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1)), + _buttons_state(), + _msg_mc_sp() +{ + double dz_default = 0.2; + /* Load parameters, default values work for Microsoft XBox Controller */ + _n.param("map_x", _param_axes_map[0], 4); + _n.param("scale_x", _param_axes_scale[0], 1.0); + _n.param("off_x", _param_axes_offset[0], 0.0); + _n.param("dz_x", _param_axes_dz[0], dz_default); + + _n.param("map_y", _param_axes_map[1], 3); + _n.param("scale_y", _param_axes_scale[1], -1.0); + _n.param("off_y", _param_axes_offset[1], 0.0); + _n.param("dz_y", _param_axes_dz[1], dz_default); + + _n.param("map_z", _param_axes_map[2], 2); + _n.param("scale_z", _param_axes_scale[2], -0.5); + _n.param("off_z", _param_axes_offset[2], -1.0); + _n.param("dz_z", _param_axes_dz[2], 0.0); + + _n.param("map_r", _param_axes_map[3], 0); + _n.param("scale_r", _param_axes_scale[3], -1.0); + _n.param("off_r", _param_axes_offset[3], 0.0); + _n.param("dz_r", _param_axes_dz[3], dz_default); + + _n.param("map_manual", _param_buttons_map[0], 0); + _n.param("map_altctl", _param_buttons_map[1], 1); + _n.param("map_posctl", _param_buttons_map[2], 2); + _n.param("map_auto_mission", _param_buttons_map[3], 3); + _n.param("map_auto_loiter", _param_buttons_map[4], 4); + _n.param("map_auto_rtl", _param_buttons_map[5], 4); + + /* Default to manual */ + _msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + +} + +void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) +{ + + /* Fill px4 manual control setpoint topic with contents from ros joystick */ + /* Map sticks to x, y, z, r */ + MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], _msg_mc_sp.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r); + //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r); + + /* Map buttons to switches */ + MapButtons(msg, _msg_mc_sp); + + _msg_mc_sp.timestamp = px4::get_time_micros(); + + _man_ctrl_sp_pub.publish(_msg_mc_sp); +} + +void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, + double deadzone, float &out) +{ + double val = msg->axes[map_index]; + + if (val + offset > deadzone || val + offset < -deadzone) { + out = (float)((val + offset)) * scale; + } else { + out = 0.0f; + } +} + +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { + msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + + if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + } + +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "manual_input"); + ManualInput m; + ros::spin(); + return 0; +} diff --git a/src/modules/uORB/topics/rc_channels.h b/src/platforms/ros/nodes/manual_input/manual_input.h index 2fde5d424..bf704f675 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.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,68 +32,62 @@ ****************************************************************************/ /** - * @file rc_channels.h - * Definition of the rc_channels uORB topic. + * @file manual_input.h + * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic. * - * @deprecated DO NOT USE FOR NEW CODE - */ + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ -#ifndef RC_CHANNELS_H_ -#define RC_CHANNELS_H_ +#include "ros/ros.h" +#include <px4/manual_control_setpoint.h> +#include <sensor_msgs/Joy.h> -#include <stdint.h> -#include "../uORB.h" +class ManualInput +{ +public: + ManualInput(); -/** - * 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, - PARAM_1, - PARAM_2, - PARAM_3 -}; + ~ManualInput() {} -// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS +protected: + /** + * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic + */ + void JoyCallback(const sensor_msgs::JoyConstPtr &msg); -#define RC_CHANNELS_FUNCTION_MAX 19 + /** + * Helper function to map and scale joystick axis + */ + void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, + float &out); + /** + * Helper function to map joystick buttons + */ + void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp); -/** - * @addtogroup topics - * @{ - */ -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. */ + ros::NodeHandle _n; + ros::Subscriber _joy_sub; + ros::Publisher _man_ctrl_sp_pub; -/** - * @} - */ + /* Parameters */ + enum MAIN_STATE { + MAIN_STATE_MANUAL = 0, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, + MAIN_STATE_MAX + }; + + int _param_buttons_map[MAIN_STATE_MAX]; /**< joystick button mapping, order according to MAIN_STATE */ + bool _buttons_state[MAIN_STATE_MAX]; /**< joystick button state of last iteration, + order according to MAIN_STATE */ -/* register this as object request broker structure */ -ORB_DECLARE(rc_channels); + int _param_axes_map[4]; + double _param_axes_scale[4]; + double _param_axes_offset[4]; + double _param_axes_dz[4]; -#endif + px4::manual_control_setpoint _msg_mc_sp; +}; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp new file mode 100644 index 000000000..0749c8e92 --- /dev/null +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -0,0 +1,272 @@ +/**************************************************************************** + * + * 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 mc_mixer.cpp + * Dummy multicopter mixer for euroc simulator (gazebo) + * + * @author Roman Bapst <romanbapst@yahoo.de> +*/ +#include <ros/ros.h> +#include <px4.h> +#include <lib/mathlib/mathlib.h> +#include <mav_msgs/MotorSpeed.h> +#include <string> + +class MultirotorMixer +{ +public: + + MultirotorMixer(); + + struct Rotor { + float roll_scale; + float pitch_scale; + float yaw_scale; + }; + + void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); + void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg); + +private: + + ros::NodeHandle _n; + ros::Subscriber _sub; + ros::Publisher _pub; + + const Rotor *_rotors; + + unsigned _rotor_count; + + struct { + float control[8]; + } inputs; + + struct { + float control[8]; + } outputs; + + bool _armed; + ros::Subscriber _sub_actuator_armed; + + void mix(); +}; + +const MultirotorMixer::Rotor _config_x[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, -1.00 }, +}; + +const MultirotorMixer::Rotor _config_quad_plus[] = { + { -1.000000, 0.000000, 1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, +}; + +const MultirotorMixer::Rotor _config_quad_plus_euroc[] = { + { 0.000000, 1.000000, 1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 1.000000, 0.000000, -1.00 }, + { -1.000000, 0.000000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_wide[] = { + { -0.927184, 0.374607, 1.000000 }, + { 0.777146, -0.629320, 1.000000 }, + { 0.927184, 0.374607, -1.000000 }, + { -0.777146, -0.629320, -1.000000 }, +}; +const MultirotorMixer::Rotor _config_quad_iris[] = { + { -0.876559, 0.481295, 1.000000 }, + { 0.826590, -0.562805, 1.000000 }, + { 0.876559, 0.481295, -1.000000 }, + { -0.826590, -0.562805, -1.000000 }, +}; + +const MultirotorMixer::Rotor *_config_index[5] = { + &_config_x[0], + &_config_quad_plus[0], + &_config_quad_plus_euroc[0], + &_config_quad_wide[0], + &_config_quad_iris[0] +}; + +MultirotorMixer::MultirotorMixer(): + _n(), + _rotor_count(4), + _rotors(_config_index[0]) +{ + _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); + _pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10); + + if (!_n.hasParam("motor_scaling_radps")) { + _n.setParam("motor_scaling_radps", 150.0); + } + + if (!_n.hasParam("motor_offset_radps")) { + _n.setParam("motor_offset_radps", 600.0); + } + + if (!_n.hasParam("mixer")) { + _n.setParam("mixer", "x"); + } + + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this); +} + +void MultirotorMixer::mix() +{ + float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); + float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); + float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); + float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f); + float min_out = 0.0f; + float max_out = 0.0f; + + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ + for (unsigned i = 0; i < _rotor_count; i++) { + float out = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + thrust; + + /* limit yaw if it causes outputs clipping */ + if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { + yaw = -out / _rotors[i].yaw_scale; + } + + /* calculate min and max output values */ + if (out < min_out) { + min_out = out; + } + + if (out > max_out) { + max_out = out; + } + + outputs.control[i] = out; + } + + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ + if (min_out < 0.0f) { + float scale_in = thrust / (thrust - min_out); + + /* mix again with adjusted controls */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = scale_in + * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) + thrust; + } + + } else { + /* roll/pitch mixed without limiting, add yaw control */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] += yaw * _rotors[i].yaw_scale; + } + } + + /* scale down all outputs if some outputs are too large, reduce total thrust */ + float scale_out; + + if (max_out > 1.0f) { + scale_out = 1.0f / max_out; + + } else { + scale_out = 1.0f; + } + + /* scale outputs to range _idle_speed..1, and do final limiting */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f); + } +} + +void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) +{ + // read message + for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) { + inputs.control[i] = msg.control[i]; + } + + // switch mixer if necessary + std::string mixer_name; + _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { + _rotors = _config_index[0]; + } else if (mixer_name == "+") { + _rotors = _config_index[1]; + } else if (mixer_name == "e") { + _rotors = _config_index[2]; + } else if (mixer_name == "w") { + _rotors = _config_index[3]; + } else if (mixer_name == "i") { + _rotors = _config_index[4]; + } + + // mix + mix(); + + // publish message + mav_msgs::MotorSpeed rotor_vel_msg; + double scaling; + double offset; + _n.getParamCached("motor_scaling_radps", scaling); + _n.getParamCached("motor_offset_radps", offset); + + if (_armed) { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset); + } + + } else { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(0.0); + } + } + + _pub.publish(rotor_vel_msg); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "mc_mixer"); + MultirotorMixer mixer; + ros::spin(); + + return 0; +} + +void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg) +{ + _armed = msg.armed; +} diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp new file mode 100644 index 000000000..ed3a4efa5 --- /dev/null +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 position_estimator.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Roman Bapst <romanbapst@yahoo.de> +*/ + +#include "position_estimator.h" + +#include <px4/vehicle_local_position.h> +#include <mathlib/mathlib.h> +#include <platforms/px4_defines.h> +#include <platforms/px4_middleware.h> +#include <vector> +#include <string> +#include <gazebo_msgs/ModelStates.h> + +PositionEstimator::PositionEstimator() : + _n(), + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)), + _vehicle_position_pub(_n.advertise<px4::vehicle_local_position>("vehicle_local_position", 1)), + _startup_time(1) +{ +} + +void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) +{ + //XXX: use a proper sensor topic + + px4::vehicle_local_position msg_v_l_pos; + + /* Fill px4 position topic with contents from modelstates topic */ + int index = 0; + //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when + //gazebo rearranges indexes. + for(std::vector<std::string>::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { + if (*it == "iris" || *it == "ardrone") { + index = it - msg->name.begin(); + break; + } + } + msg_v_l_pos.xy_valid = true; + msg_v_l_pos.z_valid = true; + msg_v_l_pos.v_xy_valid = true; + msg_v_l_pos.v_z_valid = true; + + msg_v_l_pos.x = msg->pose[index].position.x; + msg_v_l_pos.y = -msg->pose[index].position.y; + msg_v_l_pos.z = -msg->pose[index].position.z; + msg_v_l_pos.vx = msg->twist[index].linear.x; + msg_v_l_pos.vy = -msg->twist[index].linear.y; + msg_v_l_pos.vz = -msg->twist[index].linear.z; + + msg_v_l_pos.xy_global = true; + msg_v_l_pos.z_global = true; + msg_v_l_pos.ref_timestamp = _startup_time; + msg_v_l_pos.ref_lat = 47.378301; + msg_v_l_pos.ref_lon = 8.538777; + msg_v_l_pos.ref_alt = 1200.0f; + + + msg_v_l_pos.timestamp = px4::get_time_micros(); + _vehicle_position_pub.publish(msg_v_l_pos); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "position_estimator"); + PositionEstimator m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h new file mode 100644 index 000000000..da1fc2c5a --- /dev/null +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 position_estimator.h + * Dummy position estimator that forwards position from gazebo to px4 topic + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <gazebo_msgs/ModelStates.h> +#include <sensor_msgs/Imu.h> + +class PositionEstimator +{ +public: + PositionEstimator(); + + ~PositionEstimator() {} + +protected: + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_modelstates; + ros::Publisher _vehicle_position_pub; + + uint64_t _startup_time; + + +}; diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp new file mode 100755 index 000000000..a71801397 --- /dev/null +++ b/src/platforms/ros/perf_counter.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (C) 2012 - 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 perf_counter.cpp + * + * Empty function calls for ros compatibility + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ +#include <stdlib.h> +#include <stdio.h> +#include <systemlib/perf_counter.h> + + + +perf_counter_t perf_alloc(enum perf_counter_type type, const char *name) +{ + return NULL; +} + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +void perf_free(perf_counter_t handle) +{ + +} + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_count(perf_counter_t handle) +{ + +} + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_begin(perf_counter_t handle) +{ + +} + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_end(perf_counter_t handle) +{ + +} + +/** + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_cancel(perf_counter_t handle) +{ + +} + +/** + * Reset a performance counter. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +void perf_reset(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to stdout + * + * @param handle The counter to print. + */ +void perf_print_counter(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to a fd. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + * @param handle The counter to print. + */ +void perf_print_counter_fd(int fd, perf_counter_t handle) +{ + +} + +/** + * Print all of the performance counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +void perf_print_all(int fd) +{ + +} + +/** + * Reset all of the performance counters. + */ +void perf_reset_all(void) +{ + +} + +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +uint64_t perf_event_count(perf_counter_t handle) +{ + +} + + 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/systemcmds/config/config.c b/src/systemcmds/config/config.c index f54342f06..78db19801 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -195,7 +195,7 @@ do_gyro(int argc, char *argv[]) int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; - param_get(param_find("SENS_GYRO_ID"), &(calibration_id)); + param_get(param_find("CAL_GYRO0_ID"), &(calibration_id)); warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); @@ -272,7 +272,7 @@ do_mag(int argc, char *argv[]) int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; - param_get(param_find("SENS_MAG_ID"), &(calibration_id)); + param_get(param_find("CAL_MAG0_ID"), &(calibration_id)); warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); @@ -349,7 +349,7 @@ do_accel(int argc, char *argv[]) int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; - param_get(param_find("SENS_ACC_ID"), &(calibration_id)); + param_get(param_find("CAL_ACC0_ID"), &(calibration_id)); warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 7d80af307..32682f890 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -39,6 +39,7 @@ */ #include <nuttx/config.h> +#include <platforms/px4_defines.h> #include <stdio.h> #include <stdlib.h> @@ -63,6 +64,10 @@ #include "drivers/drv_pwm_output.h" #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 3e1f76714..c5c959cf3 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -99,7 +99,7 @@ int preflight_check_main(int argc, char *argv[]) } devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_MAG_ID"), &(calibration_devid)); + param_get(param_find("CAL_MAG0_ID"), &(calibration_devid)); if (devid != calibration_devid){ warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); @@ -122,7 +122,7 @@ int preflight_check_main(int argc, char *argv[]) fd = open(ACCEL_DEVICE_PATH, O_RDONLY); devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_ACC_ID"), &(calibration_devid)); + param_get(param_find("CAL_ACC0_ID"), &(calibration_devid)); if (devid != calibration_devid){ warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); @@ -168,7 +168,7 @@ int preflight_check_main(int argc, char *argv[]) fd = open(GYRO_DEVICE_PATH, 0); devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_GYRO_ID"), &(calibration_devid)); + param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid)); if (devid != calibration_devid){ warnx("gyro calibration is for a different device - calibrate gyro first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 0dc333f0a..1d728e857 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -32,8 +32,7 @@ SRCS = test_adc.c \ test_ppm_loopback.c \ test_rc.c \ test_conv.cpp \ - test_mount.c \ - test_mtd.c + test_mount.c EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c deleted file mode 100644 index 43231ccad..000000000 --- a/src/systemcmds/tests/test_mtd.c +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 test_mtd.c - * - * Param storage / file test. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <sys/stat.h> -#include <poll.h> -#include <dirent.h> -#include <stdio.h> -#include <stddef.h> -#include <unistd.h> -#include <fcntl.h> -#include <systemlib/err.h> -#include <systemlib/perf_counter.h> -#include <string.h> - -#include <drivers/drv_hrt.h> - -#include "tests.h" - -#define PARAM_FILE_NAME "/fs/mtd_params" - -static int check_user_abort(int fd); -static void print_fail(void); -static void print_success(void); - -int check_user_abort(int fd) { - /* check if user wants to abort */ - char c; - - struct pollfd fds; - int ret; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 0); - - if (ret > 0) { - - read(0, &c, 1); - - switch (c) { - case 0x03: // ctrl-c - case 0x1b: // esc - case 'c': - case 'q': - { - warnx("Test aborted."); - fsync(fd); - close(fd); - return OK; - /* not reached */ - } - } - } - - return 1; -} - -void print_fail() -{ - printf("<[T]: MTD: FAIL>\n"); -} - -void print_success() -{ - printf("<[T]: MTD: OK>\n"); -} - -int -test_mtd(int argc, char *argv[]) -{ - unsigned iterations= 0; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat(PARAM_FILE_NAME, &buffer)) { - warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); - print_fail(); - return 1; - } - - // XXX get real storage space here - unsigned file_size = 4096; - - /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {256, 512, 4096}; - - for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - - printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); - - uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); - - /* fill write buffer with known values */ - for (unsigned i = 0; i < sizeof(write_buf); i++) { - /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; - } - - uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); - hrt_abstime start, end; - - int fd = open(PARAM_FILE_NAME, O_RDONLY); - int rret = read(fd, read_buf, chunk_sizes[c]); - close(fd); - if (rret <= 0) { - err(1, "read error"); - } - - fd = open(PARAM_FILE_NAME, O_WRONLY); - - printf("printing 2 percent of the first chunk:\n"); - for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) { - printf("%02X", read_buf[i]); - } - printf("\n"); - - iterations = file_size / chunk_sizes[c]; - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - int wret = write(fd, write_buf, chunk_sizes[c]); - - if (wret != (int)chunk_sizes[c]) { - warn("WRITE ERROR!"); - print_fail(); - return 1; - } - - fsync(fd); - - if (!check_user_abort(fd)) - return OK; - - } - end = hrt_absolute_time(); - - warnx("write took %llu us", (end - start)); - - close(fd); - fd = open(PARAM_FILE_NAME, O_RDONLY); - - /* read back data for validation */ - for (unsigned i = 0; i < iterations; i++) { - int rret2 = read(fd, read_buf, chunk_sizes[c]); - - if (rret2 != (int)chunk_sizes[c]) { - warnx("READ ERROR!"); - print_fail(); - return 1; - } - - /* compare value */ - bool compare_ok = true; - - for (unsigned j = 0; j < chunk_sizes[c]; j++) { - if (read_buf[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d", j); - print_fail(); - compare_ok = false; - break; - } - } - - if (!compare_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - print_fail(); - return 1; - } - - if (!check_user_abort(fd)) - return OK; - - } - - - close(fd); - - } - - /* fill the file with 0xFF to make it look new again */ - char ffbuf[64]; - memset(ffbuf, 0xFF, sizeof(ffbuf)); - int fd = open(PARAM_FILE_NAME, O_WRONLY); - for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) { - int ret = write(fd, ffbuf, sizeof(ffbuf)); - - if (ret != sizeof(ffbuf)) { - warnx("ERROR! Could not fill file with 0xFF"); - close(fd); - print_fail(); - return 1; - } - } - - (void)close(fd); - print_success(); - - return 0; -} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index ad55e1410..69fb0e57b 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -112,7 +112,6 @@ extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); -extern int test_mtd(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0f56704e6..2f586fa85 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -109,7 +109,6 @@ const struct { {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, - {"mtd", test_mtd, 0}, #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 666570a71..a87c23be2 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -23,8 +23,14 @@ include_directories(${CMAKE_SOURCE_DIR}) include_directories(${PX_SRC}) include_directories(${PX_SRC}/modules) include_directories(${PX_SRC}/lib) +include_directories(${PX_SRC}/drivers) add_definitions(-D__EXPORT=) +add_definitions(-D__PX4_TESTS) +add_definitions(-Dnoreturn_function=) +add_definitions(-Dmain_t=int) +add_definitions(-DERROR=-1) +add_definitions(-DOK=0) # check add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index ee4f3d1d6..15a21c86b 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -11,7 +11,7 @@ #include "gtest/gtest.h" TEST(SBUS2Test, SBUS2) { - char *filepath = "testdata/sbus2_r7008SB.txt"; + const char *filepath = "testdata/sbus2_r7008SB.txt"; FILE *fp; fp = fopen(filepath,"rt"); |